Kalman Filter

前段时间在知乎上看到的《卡尔曼滤波器及其在云台控制中的应用》这篇文章,中间提到了Estimate的三个境界:smoothing->filtering->prediction。为了解决我在机器人比赛中的预测问题,也为了迈入Estimate的大门,就从Kalman Filter开始入手吧。

概述

卡尔曼滤波器是在估计线性系统状态的过程中,以最小均方误差为目的而推导出的5个递推数学等式,即“根据当前的测量值和上一刻的预测量、误差,计算当前的最优量,再预测下一刻的量”。误差是不断变化而且独立存在, 始终不受测量数据的影响。
从概率论贝叶斯模型的观点来看,系统预测的结果是先验,系统测量的结果是后验。
核心思想:预测 + 测量反馈。

状态方程

  • $A$ 为转换矩阵,大小 [n x n]
  • $x$ 是系统的状态向量,大小 [n x 1]
  • $B$ 是将输入转换为状态的矩阵,大小为 [n x k]
  • $u$ 为系统输入,大小为 [k x 1]
  • $w$ 是系统噪声

观测方程

  • $Z$ 是测量值$y_t$,大小为 [m x 1]
  • $H$ 是状态变量到测量的转换矩阵,大小为 [m x n]
  • $v$ 是测量噪声

概率推导

其中,更新为 $P(x_t|y_1,…,y_t)$,预测为 $P(x_t|y_1,…,y_{t-1})$

最后更新有了递归 $P(x_{t-1}|y_1,…,y_{t-1})$
重复描述这个过程:
$t=1$:
更新:$P(x_1|y_1)\sim N(\hat{\mu}_1,\hat{\Sigma}_1)$
$t=2$:
预测:$P(x_2|y_1)\sim N(\bar{\mu}_2,\bar{\Sigma}_2) $
更新:$P(x_2|y_1,y_2)\sim N(\hat{\mu}_2,\hat{\Sigma}_2) $

$t=t$:
预测:$P(x_t|y_1,…,y_{t-1})\sim N(\bar{\mu}_t,\bar{\Sigma}_t)$
更新:$P(x_t|y_1,…,y_t)\sim N(\hat{\mu}_t,\hat{\Sigma}_t) $

由带噪声的预测和测量得到估计

每次更新状态是相互独立的,状态测量虽然是一个序列,但是由观测方程,每次测量也是相互独立的。对噪声计算协方差的平方矩阵就是Q和R。

预测理论值$x_k$(先验)有了,测量实际值$z_k$(后验)也有了,假设一个$K$,然后得到估计值

  • $\hat{x}_k’$ 是先验的预测
  • $\hat{z}_k$ 是后验的预测
  • $\hat{x}_k$ 是最后的估计值
  • $(z_k-H\hat{x}_k’)$ 是残差,即测量实际值$z_k$和预测理论值$\hat{z}_k$的差,即后验和先验的差

推导卡尔曼增益K

预测值$\hat{x}_k’$和真实值$x_k$之间有系统噪声带来的误差,计算协方差矩阵:

估计值$\hat{x}_k$和真实值$x_k$之间两种噪声都有,计算协方差矩阵:

代入前面得到的估计值$\hat{x}_k$化简:

因为系统状态变量和测量噪声之间是相互独立的,所以继续推导:

接下来求矩阵$P_k$的迹,即协方差矩阵的对角线元素之和,即均方差:

寻找均方差的最小值,对未知量$K$求导,令导函数等于0得到$K$的计算式:

上面这个式子,转换矩阵$H$是常数,测量噪声协方差$R$也是常数。因此$K$的大小只和$P_k’$有关。
不妨进一步假设,上面式子中的矩阵大小都为 [1 x 1],并假设$H=1$、$P_k’\ne0$。那么$K$可以写成:

所以$P_k’$越大,那么$K$就越大,权重将更加重视反馈。如果$P_k’=0$,即预测值和真实值相等,那么$K=0$,估计值就等于预测值。

将计算出的这个$K$反代入$P_k$中,就能简化$P_k$:

因此递推公式中每一步的K就计算出来了,同时每一步的估计协方差也能计算出来。最后一个未知量$P_k’$。它的递推计算如下:

由此也得到了$P_k’$的递推公式。因此我们只需设定最初的$P_k$,就能不断递推下去。

总结 Kalman 计算流程

  1. 计算预测值
  2. 计算系统误差 $P_k’ = AP_{k-1}A^T+Q$
  3. 计算卡尔曼增益 $K_k=P_k’H^T(HP_k’H^T+R)^{-1}$
  4. 计算估计值 $\hat{x}_k=\hat{x}_k’+K_k(z_k-H\hat{x}_k’)$
  5. 递推更新 $P_k = (I-K_kH)P_k’$


简化 Kalman Filter

最简单的情况是一阶,全部是标量计算,调节Q/R的比值来控制滤波效果。当然可以是更复杂的,二阶三阶都可以。

  1. 计算预测值 $\hat{x}^-_k=\hat{x}_{k-1}$
  2. 计算系统误差 $P^-_{k}=P_{k-1}+Q$
  3. 计算卡尔曼增益 $K_k=\frac{P^-_k}{P^-_k+R}$
  4. 计算估计值 $\hat{x}=\hat{x}^-_k+K_k(z_k-\hat{x}^-_k)$
  5. 递推更新 $P_k=(1-K_k)P^-_k$

坐标系和运动学模型的选择

参考自动驾驶中常用于车辆状态估计的车辆运动模型,主要包含以下几类:

  • 恒定速度模型(Constant Velocity, CV)
  • 恒定加速度模型(Constant Acceleration, CA)
  • 恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)
  • 恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

参考

https://blog.csdn.net/heyijia0327/article/details/17487467
https://blog.csdn.net/heyijia0327/article/details/17667341
https://zhuanlan.zhihu.com/p/38745950
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits