Python卡尔曼滤波模块：filterpy

文章目录

• • 模型
  • 代码

模型

【filterpy】用于卡尔曼滤波。

卡尔曼滤波是一种递归的最优估计算法，可通过含有噪声的观测来估计系统的状态，记其状态方程与观测方程为

• 状态方程 ${\mathbf{x}}_k={\mathbf{F}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k$
• 观测方程 ${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$

式中

• ${\mathbf{x}}_k$
• ${\mathbf{z}}_k$
• ${\mathbf{F}}_k$
• ${\mathbf{H}}_k$
• ${\mathbf{w}}_k\sim N(0,{\mathbf{Q}}_k)$
• ${\mathbf{v}}_k\sim N(0,{\mathbf{R}}_k)$
• ${\mathbf{B}}_k{u}_k$

现假设有一目标在做匀速运动，对于某个时间步长

δ

t

\\delta_t

δt​而言，其状态转移矩阵可以记作

F

=

[

1

δ

t

0

1

]

F=\\begin{bmatrix}1&\\delta_t\\\\0&1\\end{bmatrix}

F=[10​δt​1​]

则对于某一时刻的状态

[

x

k

−

1

v

k

1

]

\\begin{bmatrix}x_{k-1}\\\\v_{k_1}\\end{bmatrix}

[xk−1​vk1​​​]而言，其下一时刻的状态为

[

x

k

v

k

]

=

F

[

x

k

−

1

v

k

−

1

]

=

[

x

k

−

1

+

v

k

−

1

δ

t

v

k

−

1

]

\\begin{bmatrix}x_k\\\\ v_k\\end{bmatrix}=F\\begin{bmatrix}x_{k-1}\\\\ v_{k-1}\\end{bmatrix}= \\begin{bmatrix} x_{k-1}+v_{k-1}\\delta_t\\\\ v_{k-1} \\end{bmatrix}

[xk​vk​​]=F[xk−1​vk−1​​]=[xk−1​+vk−1​δt​vk−1​​]

如果我们只有位置传感器，那么观测矩阵可以记作

[

1

0

]

\\begin{bmatrix}1&0\\end{bmatrix}

[1​0​]。

设观测时会额外引入一些噪声，令

Q

=

[

0.025

0.05

0.05

1

]

Q=\\begin{bmatrix}0.025&0.05\\\\0.05&1\\end{bmatrix}

Q=[0.0250.05​0.051​]，观测噪声的协方差为

5

5

5，则卡尔曼滤波前后其误差变化如下

代码

import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter

kf = KalmanFilter(dim_x=2, dim_z=1) # 状态维度：[位置, 速度]

dt = 1.0 # 时间步长
kf.F = np.array([[1, dt], [0, 1]])
kf.H = np.array([[1, 0]]) # 观测矩阵 H (只观测位置)
kf.x = np.array([[0.], [0.]]) # 初始速度

kf.P *= 1000. # 初始协方差（不确定性）
kf.Q = Q = np.array([[dt**4/4, dt**3/2],
[dt**3/2, dt**2]]) * 0.1
kf.R = np.array([[5.]]) # 位置测量噪声标准差 sqrt(5) ≈ 2.24

np.random.seed(42)
ts = np.arange(50)
pos = 0.1 * ts
meas = pos + np.random.randn(50)* np.sqrt(5)

fpos = []
fvel = []

for z in meas:
kf.predict()
kf.update(z)
fpos.append(kf.x[0, 0])
fvel.append(kf.x[1, 0])

plt.subplot(121)
plt.plot(pos, label='True Position', linewidth=2)
plt.scatter(range(len(meas)), meas, label='Noisy Measurements', s=10, color='red')
plt.plot(fpos, label='Kalman Filter Estimate', linewidth=2)
plt.legend()
plt.grid(True)

plt.subplot(122)
plt.plot(np.abs(fpos – pos), label="Errors of Kalman Filter")
plt.plot(np.abs(meas – pos), label="Errors of Measurement", lw=0, marker='.')
plt.legend()
plt.grid(True)

plt.show()

【KalmanFilter】即为Kalmann滤波类，其输入参数为

• dim_x 滤波器的状态维度，在我们的模型中包括位置和速度，故而维度为2
• dim_z 测量值的维度，由于我们只观测位置，故而维度为1
• dim_u 默认为0，表示控制输入的维度

在创建滤波器对象后，根据状态方程与观测方程，分别设置状态转移矩阵和观测矩阵

F

,

H

F,H

F,H；

Q

,

R

Q,R

Q,R则分别关联过程噪声和观测噪声。