Python ⼗⾏代码实现简单卡尔曼滤波（KalmanFilter ）
⽂章⽬录
关键代码
import  numpy as  np
#⼀步预测
def  kf_predict (X0, P0, A , Q , B , U1):
X10 = np .dot (A ,X0) + np .dot (B ,U1)
P10 = np .dot (np .dot (A ,P0),A .T )+ Q
return  (X10, P10)
#测量更新
def  kf_update (X10, P10, Z , H , R ):
K = np .dot (np .dot (P10,H .T ),np .linalg .pinv (np .dot (np .dot (H ,P10),H .T ) + R ))
X1 = X10 + np .dot (K ,Z - np .dot (H ,X10))
P1 = np .dot (1 - np .dot (K ,H ),P10)
return  (X1, P1, K )
解释
离散的状态⽅程、观测⽅程及它们的随机过程如下：
如果是连续的状态⽅程则需要离散化，例如上式中的A等于：
其中expm指矩阵指数，F为微分运动⽅程线性化后的系数矩阵，可以使⽤p协助推导。（线性化及离散化不属于本⽂范围）
Kalman Filter主要步骤为⼀步预测和测量更新两个部分，以下列出Kalman黄⾦5公式⼀步预测
测量更新：
X (k )=AX (k −1)+BU (k )+w (k −1)
Z (k )=HX (k )+e (k )
p (w )=N (0,Q )
p (e )=N (0,R )
A =expm (F Δt )
=X
˙FX X (k ,k −1)=AX (k −1)+BU (k )
P (k ,k −1)=AP (k −1)A +T Q
K (k )=P (k ,k −1)H [HP (k ,k −T 1)H +T R ]−1
X (k )=X (k ,k −1)+K (k )[Z (k )−HX (k ,k −1)]
P (k )=[I −K (k )H ]P (k ,k −1)
import numpy as np
#⼀步预测
'''
设状态量有xn个
- X0为前⼀时刻状态量，shape=(xn,1)
- P0为初始状态不确定度， shape=(xn,xn)
- A为状态转移矩阵，shape=(xn,xn)
- Q为递推噪声协⽅差矩阵，shape=(xn,xn)
- B、U1是外部输⼊部分
返回的结果为
- X10为⼀步预测的状态量结果，shape=(xn,1)
- P10为⼀步预测的协⽅差，shape=(xn,xn)
'''
def kf_predict(X0, P0, A, Q, B, U1):
X10 = np.dot(A,X0)+ np.dot(B,U1)
P10 = np.dot(np.dot(A,P0),A.T)+ Q
return(X10, P10)
'''
设状态量有xn个
- X10为⼀步预测的状态量结果，shape=(xn,1)
- P10为⼀步预测的协⽅差，shape=(xn,xn)
- Z为观测值，shape=(xn,1)
- H为观测系数矩阵，shape=(xn,xn)
-
R为观测噪声协⽅差，shape=(xn,xn)
返回的结果为
- X1为⼀步预测的状态量结果，shape=(xn,1)
- P1为⼀步预测的协⽅差，shape=(xn,xn)
- K为卡尔曼增益，不需要返回，但是可以看⼀下它的值来判断是否正常运⾏'''
#测量更新
def kf_update(X10, P10, Z, H, R):
K = np.dot(np.dot(P10,H.T),np.linalg.pinv(np.dot(np.dot(H,P10),H.T)+ R))    X1 = X10 + np.dot(K,Z - np.dot(H,X10))
P1 = np.dot(1- np.dot(K,H),P10)
return(X1, P1, K)
注意在Numpy shape(n,) 不等于shape(n,1)
例⼦
以匀加速度运动为例，结果如下，代码在最后
可见前期偏预测、后期偏观测
# -*- coding: utf-8 -*-
"""
Created on Wed Mar 31 16:02:39 2021
@author: Canvas
@function: Kalman Filter Demo
"""
import numpy as np
import matplotlib.pyplot as plt
"""
X(k) = AX(k-1) + BU(k) + w(k-1)
Z(k) = HX(k) + e(k)
p(w) = N(0, Q)
p(e) = N(0, R)
"""
def kf_predict(X0, P0, A, Q, B, U1):
X10 = np.dot(A,X0)+ np.dot(B,U1)
P10 = np.dot(np.dot(A,P0),A.T)+ Q
return(X10, P10)
linspace函数python
def kf_update(X10, P10, Z, H, R):
V = Z - np.dot(H,X10)
K = np.dot(np.dot(P10,H.T),np.linalg.pinv(np.dot(np.dot(H,P10),H.T)+ R))    X1 = X10 + np.dot(K,V)
P1 = np.dot(1- np.dot(K,H),P10)
return(X1, P1, K)
"""
加速度⽩噪声建模
状态⽅程：
x' = v'
v' = a'
a' = 0
离散化得到；
x(k) = x(k-1)+t*v(k)+0.5*t^2*a(k)
v(k) = v(k-1)+t*a(k)
a(k) = a(k-1)
观测⽅程：
z(k) = x(k) + e
"""
n =20#数据量
nx =3#变量数量
t = np.linspace(0,3,n)#时间序列
dt = t[1]- t[0]
#真实函数关系
a_true = np.ones(n)*9.8+ al(0,1,size=n)
v_true = np.cumsum(a_true*dt)
x_true = np.cumsum(v_true*dt)
X_true = np.concatenate([x_true, v_true, a_true]).reshape([nx,-1])
# 观测噪声协⽅差（可调整）
R = np.diag([10**2])
#仿真观测值
e = al(0,2,n)
x_obs = x_true + e
# 计算系数
A = np.array([1,dt,0.5*dt**2,
0,1,dt,
0,0,1]).reshape([nx,nx])
B =0
U1 =0
#状态假设（观测）初始值
x0 =0
v0 =0
a0 =10.0
X0 = np.array([x0,v0,a0]).reshape(nx,1)
#初始状态不确定度（可调整）
P0 = np.diag([0**2,0**2,0.06**2])
#状态递推噪声协⽅差（可调整）
Q = np.diag([0.0**2,0**2,0.0**2])
###开始处理
X1_np = np.copy(X0)
P1_list =[P0]
X10_np = np.copy(X0)
P10_list =[P0]
P10_list =[P0]
for i in range(n):
Zi = np.array(x_obs[i]).reshape([1,1])
Hi = np.array([1,0,0]).reshape([1,nx])
if(i ==0):
continue
else:
Xi = X1_np[:,i-1].reshape([nx,1])
Pi = P1_list[i-1]
X10, P10 = kf_predict(Xi, Pi, A, Q, B, U1)
X10_np = np.concatenate([X10_np, X10], axis=1)
P10_list.append(P10)
X1, P1, K = kf_update(X10, P10, Zi, Hi, R)
X1_np = np.concatenate([X1_np, X1], axis=1)
P1_list.append(P1)
#结束，绘图
fig = plt.figure()
ax1 = fig.add_subplot(1,1,1)
ax1.plot(x_true,'k-', label="Truth")
ax1.plot(X1_np[0,:],'go--', label="Kalman Filter")
ax1.scatter(np.arange(n), x_obs, label="Observation", marker='*')
plt.legend()
plt.show()

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。