kalman 滤波器
引言
最近在做激光雷达的建图LIO-SAM，数据是跑起来了，但是细节还不大清楚，这篇文章简单梳理kalman的使用，没有整理繁杂的推导过程。细节可参看参考文献部分，特别是第一个，化繁为简，写的很好。
一句话理解 kalman：
- 使用上一次的最优结果预测当前值，并使用当前观测值进行修正，得到最优结果。
1 kalman 流程
- 初始估计值
- 预测：根据上一次的最优估计值估算当前阶段的状态值与不确定性
- 更新：基于观测值与估计值进行修正，得到最优估计
- 符号说明
- F: 转移矩阵
- H：测量矩阵
- Q：过程噪声
- R：测量噪声
- P：协方差 (更新阶段，此值有简化)
- G: 是将输入转换为状态的矩阵
- 完整的协方差计算方法如下：
2 构建过程噪声矩阵Q
有了递推的公式，在实际使用过程中，一直对噪声的设置不大明白，看到第一篇参考文献的介绍才知道有具体的方法。
过程噪声方差对卡尔曼滤波性能有非常大的影响。过小的 q会造成滞后误差。而如果q的值过高，卡尔曼滤波会完全相信测量值，造成估计值噪声过大。
不同状态变量对应的过程噪声可以是独立的。这种情况下，过程噪声协方差矩阵 Q 是对角阵：
过程噪声也可以是相关的。比如匀速运动模型假设零加速度（a=0），但实际加速度的随机噪声 \(\sigma^2_a\) 会造成速度和位置产生对应的扰动。这种情况下，过程噪声在状态变量间就是存在互相关的。环境过程噪声有两种模型：
- 离散噪声模型
- 连续噪声模型
2.1 离散噪声模型-协方差构建
离散噪声模型假设噪声在每个采样点是不同的，但是在采样点之间是相同的（零阶保持）。
对匀速运动模型，过程噪声协方差具有下述形式：
我们以模型的随机加速度方差 \(σ^2_a\)来表示位置和速度的协方差。矩阵元素计算：
现在我们可以把结果填入 Q 矩阵：
2.2 离散噪声模型-输入转移矩阵投影法
如果动态模型包含输入，我们可以更快捷地算出 Q矩阵。我们可以用输入转移矩阵把加速度方差 \(σ^2_a\) 投影到我们的动态模型里。
式中 G 是控制矩阵（或输入转移矩阵）。
对匀速运动模型，G 矩阵为：
2.3 连续噪声模型
连续噪声模型假设噪声随时间连续改变。
为了推导连续噪声模型的过程噪声协方差矩阵Qc，我们需要把离散过程噪声的协方差矩阵 Q 对时间进行积分。
2.4 噪声模型的选择
回答这个问题之前，你需要选择合适的过程噪声方差。你可以用统计和随机领域的公式去计算，或者根据经验手动选择一个合适的值（这种比较推荐）。
在雷达中，\(\sigma_{a}^{2}\) 依赖目标的特性和模型完整性。对可以机动的目标，比如飞机，\(\sigma_{a}^{2}\) 应该相对高；对不能机动的目标，比如火箭，可以用较小的 \(\sigma_{a}^{2}\)。模型完整性也是选择过程噪声方差的因素之一。如果你的模型包含了环境扰动，例如空气阻力，那么过程噪声的比例会比不包含要小。（译注：目标特性和模型完整性是一回事，一句话说就是过程噪声就是模型不准确性，模型近似程度越高就越不准确，噪声方差就要选得越大，大到一定程度KF就只相信测量值了，就起不到滤波效果了。）
一旦选定了过程噪声方差的值，接下来就要选择噪声模型了。应该选离散的还是连续的？
这个问题没有明确的答案。我推荐两种模型都尝试一下，实测看看哪种模型在你的滤波器中效果最好。当 \(\Delta t\) 非常小时，可以选取离散噪声模型，反之则最好选用连续噪声模型。
3 例程1 二维运动滤波
3.1 预测
经过时间 \(\Delta{t}\) 后的预测状态量为：
对于匀速运行，加速度为0，不会对预测造成影响，故
对于匀加速运动模型，引入加速度\(a_x,a_y\),u变为：
- 第一个预测公式为：
- 第二个预测公式
该公式中，P被称为状态协方差矩阵（state covariance matrix），表示系统的不确定程度，P在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会逐渐减小；
Q是过程噪声协方差矩阵（process noise covariance matrix），即无法用 \(\tilde{x} = F\hat{x}+u\) 表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的。
- P的初始化
以激光雷达为例，激光雷达只能测量点的位置，无法测量点的速度，因此对于激光雷达的协方差矩阵来说，对于位置信息，其测量较准确，不确定度较低；对于速度信息，不确定度较高。因此可以认为此处的 P为：
- Q 的初始化
3.2 更新
- 第一个更新公式
这个公式计算的是实际观测量 z 与状态量预测值 \(\tilde{x}\) 之间的差值 y。不同传感器的观测量一般不同，比如激光雷达测量的位置信号为 x方向和 y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态量左乘一个矩阵H，才能与观测量进行相应的运算，H被称为测量矩阵（MeasurementMatrix）。激光雷达的观测量为：
其中 \(x_m\) 和 \(y_m\) 分别表示 x 方向和 y方向上的测量值。由于 \(\tilde{x}\) 是一个 4x1的列向量，如果要与一个 2x1 的列向量 z 进行减运算，需要左乘一个 2x4 的矩阵：
意即，对于激光雷达而言，测量矩阵 H为：
- kalman gain更新
对于公式：
R是测量噪声协方差矩阵（measurement noise covariance matrix），表示的是测量值与真值之间的差值。一般情况下，传感器厂家会提供该值
4 例程2
4.1 背景
假设车辆X方向移动的速度为20m/s,Y方向为10m/s，持续20s观察
import numpy as np
import matplotlib.pyplot as plt
m = 200 # Measurements
vx = 20 # in X
vy = 10 # in Y
mx = np.array(vx + np.random.randn(m)*1)
my = np.array(vy + np.random.randn(m)*1)
measurements = np.vstack((mx,my))
# initial object state (px,py, vx, vy)
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
# 物件的不确定因素: 先验估计的协方差矩阵
P = np.diag([1000.0,
1000.0,
1000.0,
1000.0])
# 量测的时间间隔 𝑑𝑡 假设100ms
dt = 0.1 # time step between filter step
# 状态转移矩阵 𝐅
A = np.matrix([
[1.0, 0.0, dt, 0.0],
[0.0, 1.0, 0.0, dt],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
])
# 观测矩阵 𝐇
H = np.matrix([
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]
])
# 量测noise的协方差矩阵 𝐑
ra = 0.9
R = np.matrix([[ra, 0.0],
[0.0, ra]])
# 过程噪声的协方差矩阵 𝐐,输入转移矩阵投影法构建
sv = 0.5 # m/s^2
G = np.matrix([[0.5 * dt ** 2],
[0.5 * dt ** 2],
[dt],
[dt]])
Q = G * G.T * sv ** 2
I = np.eye(4)
class States_Kalman():
def __init__(self):
self.initial()
def savestates(self, x, Z, P, R, K):
self.xt.append(float(x[0]))
self.yt.append(float(x[1]))
self.dxt.append(float(x[2]))
self.dyt.append(float(x[3]))
self.Zx.append(float(Z[0]))
self.Zy.append(float(Z[1]))
self.Px.append(float(P[0, 0]))
self.Py.append(float(P[1, 1]))
self.Pdx.append(float(P[2, 2]))
self.Rdy.append(float(P[3, 3]))
self.Rdx.append(float(R[0, 0]))
self.Rdy.append(float(R[1, 1]))
self.Kx.append(float(K[0, 0]))
self.Ky.append(float(K[1, 0]))
self.Kdx.append(float(K[2, 0]))
self.Kdy.append(float(K[3, 0]))
def initial(self):
self.xt, self.yt = [], []
self.dxt, self.dyt = [], []
self.Zx, self.Zy = [], []
self.Px, self.Py = [], []
self.Pdx, self.Pdy = [], []
self.Rdx, self.Rdy = [], []
self.Kx, self.Ky = [], []
self.Kdx, self.Kdy = [], []
def plot_x(states_kalman):
fig = plt.figure(figsize=(16, 9))
plt.step(range(len(measurements[0])), measurements[0], label='$measured v_x$')
plt.step(range(len(measurements[0])), measurements[1], label='$measured v_y$')
plt.axhline(vx, color='k', label='$ture v_x$')
plt.axhline(vy, color='k', label='$ture v_y$')
plt.step(range(len(measurements[0])), states_kalman.dxt, label='$estimate v_x$', linewidth=2)
plt.step(range(len(measurements[0])), states_kalman.dyt, label='$estimate v_y$', linewidth=2)
plt.xlabel('Filter Step')
plt.ylabel('Velocity')
plt.title('Estimate (Elements from State Vectir $x$)')
plt.legend(loc='best', prop={'size': 11})
plt.show()
states_kalman = States_Kalman()
for n in range(len(measurements[0])):
# Time update (Prediction)
# 1. project the state ahead
x = A * x
# 2. Project the error covariance ahead
P = A * P * A.T + Q
# Measurement update (Correction)
# 1. Compute the Kalman Gain
S = H * P * H.T + R
K = (P * H.T) * np.linalg.pinv(S)
# 2. update the estimate by z
Z = measurements[:, n].reshape(2, 1)
y = Z - H * x
x = x + K * y
# 3. update the error covariance
P = (I - K * H) * P
## Save states (for plot results)
states_kalman.savestates(x, Z, P, R, K)
plot_x(states_kalman)
4.2 参数调试
若只更改Q为单位阵，协方差矩阵偏大，则更相信量测值，起不到很好的滤波作用，效果如下：
浙公网安备 33010602011771号