卡尔曼滤波之二：Python实现

了解了卡尔曼滤波之一：基本概念，可以结合代码来理解下卡尔曼滤波的2个预测+3个更新环节。

1.背景描述

设有一个球在30m的起始高度，以10m/s的速度竖直上抛，用传感器来跟踪球的高度。

对应于卡尔曼滤波，此系统的状态包括位置$h$及速度$v$。

球的高度满足式子：

$x_t=x_{t-1}+v_{t-1}\tau -\frac{1}{2}g{\tau }^2$

速度满足：
$v_t=v_{t-1}-g\tau$

其中，$\tau$ 为$t-1$ 与$t$之间的时间(s)， $g$ 为重力加速度。

传感器检测高度，存在的位置协方差约为3m。

2.构建卡尔曼滤波公式

2.1 预测

• 状态值预测

${\hat{x}}_t^{-}=F\cdot {\hat{x}}_{t-1}+B\cdot u_{t-1}①$

[ h t − v t − ] = [ 1 τ 0 1 ] [ h t − 1 v t − 1 ] + [ − 1 2 τ 2 − τ ] ⋅ g \qquad\qquad

=+\cdot g [ht−​vt−​​]=[10​τ1​][ht−1​vt−1​​]+[−21​τ2−τ​]⋅g

• 状态协方差预测

$P_t^{-}=F\cdot P_{t-1}\cdot F^T+Q②$

\qquad 状态协方差预测值的初始值$P_0^{-}$为 [ 1 0 0 1 ]

[10​01​]，过程噪声的协方差 $Q$ 取 [ 0 0 0 0 ]

[0000]" role="presentation" style="position: relative;">[0000]$$\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]$$

[00​00​]。

2.2 更新

• 更新卡尔曼增益$K_t$：

$K_t=\frac{P_t^{-}\cdot H^T}{H\cdot P_t^{-}\cdot H^T+R}③$

\qquad 观测矩阵 $H$ 这里为 [ 1 0 ]

[1​0​]，$R$ 为 3.

• 融合状态估计值 ${\hat{x}}_t^{-}$以及观测量 $Z_t$，更新状态值 ${\hat{x}}_t$：

${\hat{x}}_t={\hat{x}}_t^{-}+K_t(Z_t-H\cdot {\hat{x}}_t^{-})④$

• 更新状态协方差​$P_t$：

$P_t=(1-K_t\cdot H)P_t^{-}⑤$

3.代码实现

3.1 输入值

import numpy as np
times = 40
tau = 0.1
actual = -4.9*tau**2*np.arange(times)**2
# Simulate the noisy camera data
sim = actual + 3*np.random.randn(times)

# kalman filter parameters
initial_state = np.array([[30],[10]])  
initial_current_state_covariance =np.eye(2) 
Q = np.zeros((2,2)) # process_noise_covariance
R = 3 # observation_covariance
F=np.array([[1,tau],[0,1]])
B=np.array([[-0.5*tau**2],[-tau]])
U=g=9.8
H=np.array([[1,0]])

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

3.2 pykalman包实现

from pykalman import KalmanFilter
# Set up the filter
kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,
                  initial_state_mean=initial_state.reshape(-1) ,
                  initial_state_covariance=initial_current_state_covariance ,
                  transition_matrices=F,
                  observation_matrices=H,
                  observation_covariance=R,
                  transition_covariance=Q,
                  transition_offsets=[-4.9*tau**2, -9.8*tau])

state_means, state_covs = kf.filter(sim)
1
2
3
4
5
6
7
8
9
10
11
12

注意，pykalman包中使用transition_offsets来替代 $B\cdot u_{t-1}$部分。

3.3 不使用Python包实现

current_state_covariance=None
current_state_mean=None
time=0
estimated_signal = []
for measurement in sim:
    # predict
    if time==0:
        # initialize 
        predicted_state_means=initial_state
        predicted_state_covariances=initial_current_state_covariance
    else:
        predicted_state_means = F @ current_state_mean+ B*U
        predicted_state_covariances = F @current_state_covariance @F.T + Q

    # update
    kalman_gain = predicted_state_covariances @ H.T @ np.linalg.pinv(H@predicted_state_covariances@H.T + R)
    current_state_mean = predicted_state_means  + kalman_gain * (measurement - H @ predicted_state_means)
    current_state_covariance = predicted_state_covariances - kalman_gain @ H @ predicted_state_covariances

    estimated_signal.append(current_state_mean[0,0])
    time + =1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

3.4 效果可视化

import matplotlib
matplotlib.use("TkAgg")
import matplotlib.pyplot as plt

plt.figure(figsize=(12, 6))
plt.plot(range(times), sim, label="Noisy Signal")
plt.plot(range(times), state_means[:,0], label="Kalman Signal1")

plt.plot(range(times), estimated_signal, label="Estimated Signal (Kalman Filter)")
plt.legend()
plt.title("Signal Denoising with Kalman Filter")
plt.show(block=True)

1
2
3
4
5
6
7
8
9
10
11
12
13

两种方法曲线重合在一起，说明Python实现没有问题。

注意：这里的两种实现都默认t=0时只赋初值，不进行预测。

信号去噪之卡尔曼滤波代码实现中，t=0时也进行了预测。

长期来看，效果差不多，

从图上可以看出，滤波信号与有噪声信号相比，非常平滑，同时也有很好的跟踪效果。

参考文献

[1] 信号去噪之卡尔曼滤波
[2] 卡尔曼滤波：再也不用瑟瑟发抖了
[3] https://github.com/quantopian/research_public/blob/master/notebooks/lectures/Kalman_Filters/notebook.ipynb
[4] https://github.com/pykalman/pykalman/tree/master
[5] https://pykalman.github.io/#choosing-parameters
[6] Kalman Filter and Maximum Likelihood Estimation of Linearized DSGE Models
[7] 卡尔曼滤波之一：基本概念