卡尔曼滤波器

1 什么是卡尔曼滤波

最近在阅读APM的源码，督导了姿态解算的部分，了解到姿态结算是依靠卡尔曼扩展滤波器（EKF）解算出位置的。然后笔者就去了解了卡尔曼滤波的相关知识。笔者的简单理解就是卡尔曼滤波就是计算出从你当前的位置到达目标位置的最优算法。比如说一个四旋翼无人机，你想让他从当前位置飞到100m外的目标地点（固定的），我们简单的认为只需要导航就可以了啊，但是实际情况下导航在引导无人机从当前位置到目标位置的过程中，会遇到很多噪音，这样可能会使无人机偏离目标地点。有人说可以依靠数学模型计算出目标位置，但实际情况中有很多例子很难建出具体的数学模型，就算能建出具体的模型，但是在迭代的过程中有误差的话，会使这个误差越来越大，偏离目标地点。所以这就引出了卡尔曼滤波，它会结合传感器的测量值与数学模型的估算值得到最优估算状态。然后区分一下卡尔曼滤波和扩展卡尔曼滤波的区别，卡尔曼滤波主要用在线性系统中，扩展卡尔曼滤波主要用在非线性系统中（在APM姿态解算中主要用到扩展卡尔曼滤波（EKF））。本文将用最简单的例子介绍一下卡尔曼滤波器。
卡尔曼滤波使用的三个前提

• (1)系统是可观测的
• (2)系统为线性系统，能够写出下一时刻的状态预测方程
• (3)假设系统的噪声统计特性可观测

2 实例分析

2.1 数学模型的创建

我们举一个最简单的例子，一个小车在水平面上做匀速直线运动和匀加速直线运动（加速度为a）两种情况，让小车到达目标位置。
首先建立匀速直线运动小车的数学模型：
$$p_k=p_{k-1}+v.\Delta t$$
$$v_k=v_{k-1}$$

• 其中状态量用$x=\left[\begin{array}{c}{x}_k\\ v_k\end{array}\right]$来表示
• 状态量估算量用$\hat{x}=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]$

匀加速直线运动小车的数学模型：
$$p_k=p_{k-1}+v.\Delta t+\frac{1}{2}\Delta t^2$$
$$v_k=v_{k-1}+a\Delta t$$

将这两个数学模型转化为线性空间形式：
$${\hat{x}}_k=F_k{\hat{X}}_{k-1}+B_k{u}_k$$
其中：

• 转换矩阵 $$F_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$
• 控制矩阵$$B_k=\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]$$
• u代表输入量，这里即为a

如果有噪声的话，且知道噪声的分布情况$w_k$~$N(0,Q_k)$
那么：
$${\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{u}_k+w_k\text{\hspace{1em}(1)}$$
$$P_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2)}$$
其中：

• $P_k$为协方差矩阵 $P_k=\left[\begin{array}{cc}{\Sigma }_{PP}& {\Sigma }_{PV}\\ {\Sigma }_{VP}& {\Sigma }_{VV}\end{array}\right]$
  如果不了解什么是协方差的话可以查看一下wiki:https://zh.wikipedia.org/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE
• 这里用到了协方差矩阵的一个基本性质：
  $$Cov(x)=\Sigma$$
  $$Cov(Ax)=A\Sigma A^T$$
• （2）式就是由（1）式两边求协方差得到的。
  这样我们就得到了小车在数学模型下的分布：
  $x$~$N({\hat{x}}_k,P_k)$
  其中：
• ${\hat{x}}_k$为期望
• $P_k$为方差

2.2 投影到传感器上

我们上面已经说过了，单靠数学模型是远远不够的，我们还要根据传感器监测到的数据，和数学模型预测出的数据进行整合，得到最优算法。那么我们首先要将传感器测得的数据与数学模型预测的数据放在一个坐标系下。
假设我们已经知道从数学模型到测量模型下的转换矩为$H_k$,可得：

• $u_{expedicted}=H_k{\hat{x}}_k$
• ${\Sigma }_{expected}=H_k{P}_k{H}_k^T$

可得：
$X_{expedicted}$~$N(H_k{\hat{x}}_k,H_k{P}_k{H}_k^T)$

而传感器上测得的状态量符合：
$Y_{measurment}$~$N(z_k,R_k)$ (其中Noise:$v_k$~$N(0,R_k)$)

2.3 应用卡尔曼得到最优估计

由2.2我们得到了传感器和数学模型两个分布，接下来到了最重要的环节，就是利用卡尔曼滤波将两个分布整合，得到一个最优分布。具体做法就是$将两个正太分布相乘$。下面我们要补充一个知识点，就是如何将两个正态分布相乘：

• X~$N({\mu }_1,{\sigma }_1^2)$
• Y~$N({\mu }_2,{\sigma }_2^2)$

对于二者相乘新的分布为：

• k=$\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$
• ${\mu }^{{}^{\prime }}={\mu }_1+k({\mu }_2-{\mu }_1)$
• ${\sigma }_1^{{}^{\prime }2}={\sigma }_1^2+k{\sigma }_1^2$

转化为矩阵形式：

• $K^{{}^{\prime }}$=$\frac{{\Sigma }_1}{{\Sigma }_1+{\Sigma }_2}$
• ${\vec{\mu }}^{{}^{\prime }}={\vec{\mu }}_1+K^{{}^{\prime }}({\vec{\mu }}_2-{\vec{\mu }}_1)$
• ${\Sigma }_1^{{}^{\prime }}={\Sigma }_1+K^{{}^{\prime }}{\Sigma }_1^{{}^{\prime }}$

由$K^{{}^{\prime }}$=$\frac{{\Sigma }_1}{{\Sigma }_1+{\Sigma }_2}$可得$K^{{}^{\prime }}=\frac{H_k{P}_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k})$
所以相乘后的均值：

• $H_k{x}_k^{{}^{\prime }}=H_k{\hat{x}}_k+K^{{}^{\prime }}(z_k-H_k{\hat{x}}_k)$

相乘后的新方差：

• $H_k{P}_k{}^{\prime }{H}_k^T=H_k{P}_k{H}_k^T+K^{{}^{\prime }}{H}_k{P}_k{H}_k^T$

由上面两个式子可得：

• $x_k^{{}^{\prime }}={\hat{x}}_k+\frac{K^{{}^{\prime }}}{H_k}(z_k-H_k{\hat{x}}_k)$
• $P_k{}^{\prime }=P_k+K^{{}^{\prime }}{P}_k$

我们引入一个新的概念，$卡尔曼增益K$:
$$K=\frac{K^{{}^{\prime }}}{H_k}=\frac{P_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k}$$
由此得到经过卡尔曼滤波后的最优状态估计：

$${\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K(z_k-H_k{\hat{x}}_k)$$
$$P_k^{{}^{\prime }}=P_k+K{H}_k{P}_k$$

3 总结

卡尔曼滤波器程序步骤

1. 从当前状态计算下一步状态（估算）
   ${\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{u}_k+w_k$
   $P_k=F_k{P}_{K-1}{F}_k^T+Q$
   $X_{predict}$~$N(H_k{\hat{x}}_k,H_k{P}_k{x}_k^T)$
2. 根据传感器计算均值和方差
   $Y_{measurent}$ ~ $N(z_k,R_k)$
3. 计算卡尔曼滤波
   $K=\frac{K^{{}^{\prime }}}{H_k}=\frac{P_k{H}_k^T}{H_k{P}_k{H}_k^T+P_k}$
   ${\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K(Z_k-H_k{\hat{x}}_k)$
   $P_k^{{}^{\prime }}=P_k+K{H}_k{P}_k$
4. 将计算出的${\hat{x}}_k^{{}^{\prime }}$与$P_k^{{}^{\prime }}$带入第一步迭代，计算下一步最优状态。