Python十行代码实现简单卡尔曼滤波（Kalman Filter）

关键代码

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)

解释

离散的状态方程、观测方程及它们的随机过程如下：
$$\begin{gathered} 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) \end{gathered}$$

如果是连续的状态方程则需要离散化，例如上式中的A等于： $$A=expm(F\Delta t)$$
其中expm指矩阵指数，F为微分运动方程$\dot{X}=FX$线性化后的系数矩阵，可以使用sympy.exp协助推导。（线性化及离散化不属于本文范围）

Kalman Filter主要步骤为一步预测和测量更新两个部分，以下列出Kalman黄金5公式
一步预测
$$\begin{gathered} X(k,k-1)=AX(k-1)+BU(k) \\ P(k,k-1)=AP(k-1)A^T+Q \end{gathered}$$
测量更新：
$$\begin{gathered} K(k)=P(k,k-1)H^T[HP(k,k-1{)}^{H}T+R{]}^{-1} \\ X(k)=X(k,k-1)+K(k)[Z(k)-HX(k,k-1)]P(k)=[I-K(k)]P(k,k-1) \end{gathered}$$

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)


        
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 = 101 #数据量
nx = 3 #变量数量
t = np.linspace(0,5,n) #时间序列
dt = t[1] - t[0]

#真实函数关系
a_true = np.ones(n)*9.8
v_true = a_true*t
x_true = 0.5*a_true*(t**2)
X_true = np.concatenate([x_true, v_true, a_true]).reshape([nx,-1])


# 观测噪声协方差！！！！！！！！！！！！！！！！！！！！（可调整）
R = np.diag([5**2,0,0])

#仿真观测值
e = np.random.normal(0,np.sqrt(R[0][0]),n)
x_obs = X_true[0,:]
x_obs += e
Z = np.zeros([nx,n])
Z[0,:] = x_obs

# 计算系数
A = np.array([1,dt,0.5*dt**2,
              0,1,dt,
              0,0,1]).reshape([nx,nx])
B = 0
U1 = 0

#状态假设（观测）初始值
x0 = -1.0
v0 = 1.0
a0 = 9.0
X0 = np.array([x0,v0,a0]).reshape(-1,1)

#初始状态不确定度！！！！！！！！！！！！！！！！（可调整）
P0 = np.diag([5**2,2**2,1**2])

#状态递推噪声协方差！！！！！！！！！！！！！！！！！！（可调整）
Q = np.diag([0,0,1.0**2])

###开始处理
X1_np = np.copy(X0)
P1_list = [P0]
X10_np = np.copy(X0)
P10_list = [P0]

for i in range(n):
    Zi = np.array(Z[:,i]).reshape([-1,1])
    Hi = np.array([1,0,0,
                   0,0,0,
                   0,0,0]).reshape([nx,nx])
    
    if (i == 0):
        continue
    else:
        Xi = X10_np[:,i-1].reshape([-1,1])
        Pi = P10_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.plot(X10_np[0,:], 'ro--', label="Prediction")
ax1.scatter(np.arange(n), Z[0,:], label="Observation", marker='*')

plt.legend()
plt.show()