卡尔曼滤波python实现，简单情况以及多传感器融合情况

import numpy as np
import matplotlib.pyplot as plt
# 生成数据
t = np.linspace(0, 1, 100, endpoint=False)
length = len(t)
X = t**2
# 两组数据的噪声
noise = np.random.normal(0, 0.1,length)
noise2 = np.random.normal(0, 0.1,length)
# 两组数据
Y = X + noise
Y2 = X + noise2

# 查看数据
plt.plot(t,Y,t,X,t,Y2)

# 问题一
# 观测方程为Y(K)=X(K)+R R~N(0,0.1)
# 预测方程需要对数据建模
# 模型一
# X(K)=X(K-1)+Q
# Y(K)=X(K)+R
# Q~N(0,1)
F1=np.array(1)
H1=np.array(1)
Q1=np.array(0.1) # 改小该值可以使其更相信预测值的走势
R1=np.array(0.01) # 改大该值可以使其更相信观测值
# 设置初值
Xplus1=np.zeros(100)
Xplus1[0]=0.1
Pplus1=0.01**2
# 卡尔曼滤波算法
# X(K)minus=F*X(K-1)plus
# P(K)minux=F*P(K-1)plus*F'+Q
# K=P(K)minus*H'*inv(H*P(K)minus*H'+R)
# X(K)plus=X(K)minus+K*(y(k)-H*X(K)minus)
# P(K)plus=(I-K*H)*P(K)minus
for i in range(1,length):
    Xminus1=F1*Xplus1[i-1]
    Pminus1=F1*Pplus1*F1.T+Q1
    K1=(Pminus1*H1)*(H1*Pminus1*H1.T+R1)
    Xplus1[i]=Xminus1+K1*(Y[i]-H1*Xminus1)
    Pplus1=(1-K1*H1)*Pminus1

# 查看数据
plt.plot(t,X,t,Y,t,Xplus1,'r')

# 模型2
# X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2
# Y(K)=X(K)+R R~N(0,1)
# 此时的状态变量X=[X(K) X'(K) X''(K)]T
# Y(K)=H*X+R H=[1 0 0]（行向量）
# 预测方程
# X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2
# X'(K)=0*X(K-1)+X'(K-1)+X''(K-1)*dt+Q3
# X''(K)=0*X(K-1)+0*X'(K-1)+X''(K-1)+Q4 #多项式信号多求几阶倒数，会比较平滑
# F = 1 dt 0.5*dt/2
#     0  1  dt
#     0  0  1
# H = [1 0 0]
dt = 0.01
F2 = np.array([1.,dt, 0.5*(dt**2), 0.,1.,dt,0.,0.,1.]).reshape(3,3)
H2 = np.array([1,0,0])
base = 2
Q2=np.eye(3) # 改小该值可以使其更相信预测值的走势

Q2[1][1]=base*0.01
Q2[2][2]=base*0.01*0.01
R2=10 # 改大该值可以使其更相信观测值
Xplus2=np.zeros((3,length))
Xplus2[0][0]=0.1**2
Xplus2[1][0]=0
Xplus2[2][0]=0           
Pplus2=np.eye(3)
Pplus2[1][1]=base*0.01
Pplus2[2][2]=base*0.01*0.01

for i in range(1,length):
    Xminus2=F2.dot(Xplus2[:,i-1].T)
    Pminus2=F2*Pplus2*F2.T+Q2
    K2=(Pminus2.dot(H2.T))*(1/(H2.dot(Pminus2).dot(H2.T)+R2))
    Xplus2[:,i]=Xminus2+K2*(Y[i]-H2.dot(Xminus2))
    Pplus2=(np.eye(3)-K2*H2)*Pminus2

# 查看数据
plt.plot(t,X,t,Y,t,Xplus2[0,:])

# 问题2 两个传感器进行滤波
# Y1(K)=X(K)+R
# Y2(K)=X(K)+R
# H=[1 1]T
# H=1 0 0 X=X(K) X'(K) X''(K)
#   1 0 0 

dt=0.01
F3 = np.array([1.,dt, 0.5*(dt**2), 0.,1.,dt,0.,0.,1.]).reshape(3,3)
# 同时考虑传感器1和传感器2
H3 = np.array([1,0,0,1,0,0]).reshape(2,3) # [2 3]
base = 2
Q3=np.eye(3) # 改小该值可以使其更相信预测值的走势
Q3[1][1]=base*0.01
Q3[2][2]=base*0.01*0.01
R3=np.eye(2)*10 # [2 2]
Xplus3=np.zeros((3,length))
Xplus3[0][0]=0.1**2
Xplus3[1][0]=0
Xplus3[2][0]=0     
Pplus3=np.eye(3)
Pplus3[1][1]=base*0.01
Pplus3[2][2]=base*0.01*0.01
Y_all = np.vstack((Y,Y2))

for i in range(1,length):
    Xminus3=F3.dot(Xplus3[:,i-1].T) # 3x1
    Pminus3=F3*Pplus3*F3.T+Q3 # 3x3
    K3=(Pminus3.dot(H3.T)).dot(np.linalg.inv(H3.dot(Pminus3).dot(H3.Ｔ)+R2+np.eye(2)*0.00001)) # [3 2] [2 2] = [3 2]
    Xplus3[:,i]=Xminus3+K3.dot(Y[i]-H3.dot(Xminus3)) # [2 1]
    Pplus3=(np.eye(3)-K3.dot(H3))*Pminus3

# 查看数据
plt.plot(t,X,t,Y,'r',t,Y2,'g',t,Xplus3[0,:],'b')