kalman滤波 python实现

import pandas as pd import matplotlib.pyplot as plt import math from scipy import linalg as lg import numpy as np F = np.array([[1, 1], [0, 1]]) H = np.array([[0.9, 0], [0, 1.1]]) Q = np.array([[0.4, 0], [0, 0.4]]) R = np.array([[1.2, 0], [0, 1.2]]) X0 = np.array([1,3]) P0 = np.array([[1.5, 0], [0, 1.6]]) N = 200 W = np.sqrt(Q)@np.random.normal(loc=0.0, scale=1.0, size=(2,N)) V = np.sqrt(R)@np.random.normal(loc=0.0, scale=1.0, size=(2,N)) X = np.zeros((2, N)) Y = np.zeros((2, N)) X_pre = np.zeros((2, N)) X_est = np.zeros((2, N)) X_est1 = np.zeros((2, N)) Y_pre = np.zeros((2, N)) P_est = np.zeros((2, 2)) P_est1 = np.zeros((N,2, 2)) P_pre = np.zeros((2, 2)) K = np.zeros((2, 2)) y = [] z = []

状态方程

X[:, 0] = X0 + W[:, 0] for i in range(1, N): X[:, i] = F@X[:, i - 1] + W[:, i]

观测方程

for i in range(0, N): Y[:, i] = H@X[:, i] + V[:, i] # 观测方程 X_est[:, 0] = X[:, 0] PP = P0 XX = X[:, 1]

滤波过程

for i in range(1, N): X_pre[:, i] = F@XX Y_pre[:, i] = F@X_pre[:, i] P_pre = F@PP@F.T + Q K = P_pre@H.T@(np.linalg.inv(H@P_pre@H.T + R)) X_est[:, i] = X_pre[:, i] + K@(Y[:, i] - Y_pre[:, i]) P_est = P_pre - K@H@P_pre PP = P_est XX = X_est[:, i] P_est1[i,:,:] = P_est plt.figure(1) plt.plot([i for i in range(0,N)],[X[1,i] for i in range(0,N)],color = 'red',label = 'True') plt.plot([i for i in range(0,N)],[X_est[1,i] for i in range(0,N)],color = 'blue',label = 'Filter') plt.xlabel = 'Time' plt.title('X1 performance') plt.legend(loc = 'best') plt.show()

plt.figure(2) plt.plot([j for j in range(0,N)],[X[0,j] for j in range(0,N)],color = 'red',label = 'True') plt.plot([j for j in range(0,N)],[X_est[0,j] for j in range(0,N)],color = 'green',label = 'Filter') plt.xlabel = 'Time' plt.title('X2 performance') plt.legend(loc = 'best') plt.show()

P_est1[2,:,:] array([[0.85373526, 0.16610992], [0.16610992, 0.45267786]]) Perro = np.zeros([2,N]) for k in range(0,N): Perro[0,k] = P_est1[k,0,0]#估计误差协方差的误差,准确值 Perro[1,k] = P_est1[k,1,1] Perro[0,0] = P0[0,0] Perro[1,0] = P0[1,1] plt.figure(3) plt.plot([j for j in range(0,N)],[Perro[0,j] for j in range(0,N)],color = 'red',label = 'X1_Erro') plt.plot([j for j in range(0,N)],[Perro[1,j] for j in range(0,N)],color = 'green',label = 'X2_Erro') plt.xlabel = 'Time' plt.title('MSE') plt.legend(loc = 'best') plt.show()