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()