卡尔曼滤波器与DSP实现

  卡尔曼滤波器是利用系统状态方程，结合测量结果对系统状态进行进行最优估计的算法。本文介绍它的主要公式，并举例在C6000 DSP上实现。

推荐资料

• KalmanFilter.NET
• Understanding Kalman Filters
• 卡尔曼滤波与组合导航原理

“If you can’t explain it simply, you don’t understand it well enough.” —— Albert Einstein

  第一个网站是我认为的讲卡尔曼滤波讲得最好的资料！“如果你不能用简单的话把问题解释清楚，那就是理解得不够”。作者就是能用例子把问题讲得很清楚的那种人。上面举的例子和介绍的思路都非常适合初学者，即使是什么都不懂的人也能非常快速地理解。只是后面还有一些内容比如EKF、UKF等作者还没更新上去，目前只有基础的卡尔曼滤波。
  第二个链接是MathWorks给出的关于卡尔曼滤波的教学视频，还有一个配套的关于单摆的实验，可以一边动手操作一边学习，感觉挺有意思的。
  第三个链接是B站上的西北工业大学的严恭敏老师上课的录屏，严老师还有一本书《捷联惯导算法与组合导航原理》，相当于是课本，推荐想要系统学习的同学去看这个。

基础

  在正式开始介绍之前，首先得有一些基础的概念。

随机变量的期望与方差

  一维的随机变量$x$可以用它的期望$\mu$和方差${\sigma }^2$来描述；多维的随机变量$X$可以用期望$\mathrm{E}(X)$和协方差矩阵$\mathrm{V}(X)$来描述。对于一个一维的随机变量$x$，我们可以采样多个样本$x_i$来对它的期望和方差进行估计。之所以说是“估计”是因为，我们如果想获得真实的期望和方差，就需要采样无穷多的样本，这是不可能做到的。

$$\mathrm{E}(x)=\frac{1}{n}\sum _{i=0}^{n-1}{x}_i\mathrm{V}(x)=\frac{1}{n-1}\sum _{i=0}^{n-1}{\left(x_i-\mu \right)}^2$$

  上面的是公式是利用$n$个样本对随机变量$x$的期望和方差的无偏估计。之所以说是无偏是因为，如果再对上面的$\mathrm{E}(x)$和$\mathrm{V}(x)$求期望，分别可以得到$\mu$和${\sigma }^2$，它们是准确的估计。卡尔曼滤波算法中虽然不涉及这部分计算，但是最好能有这样的概念，方差可以用来衡量一个随机变量的变化程度。

期望代数（Expectation Algebra）

  因为一维随机变量$x$是多维随机变量$X$的情况，所以后面都以多维随机变量$X$进行讨论。为了和后面的推导统一符号，也用$x$表示多维随机变量。期望代数是与随机变量的期望与方差相关的一些定理公式，在卡尔曼滤波的推导中会用到。记$x$的均值和协方差矩阵分别为${\mu }_x$和$\mathrm{V}(x)$，它们满足：

$$\mathrm{V}(x)=\mathrm{E}\left(\left(x-{\mu }_x\right){\left(X-{\mu }_x\right)}^{\mathrm{T}}\right)$$

  如果对随机变量$x$进行线性变换得到$y=Fx$，$F$是$N\times N$的方阵，$x$是$N$维列向量。那么随机变量$y$的方差为：

$$\mathrm{V}(y)=F\mathrm{V}(x)F^{\mathrm{T}}$$

  还有很多简单的公式这里就不列出来了，都在这里可以找到，这是后面推导的基础。

卡尔曼滤波

  卡尔曼滤波可以概括为三个环节，初始化，更新和预测。

初始化 Initiate

  初始化状态向量${\hat{x}}_{0,0}$和表示当前状态估计不确定度的协方差矩阵$P_{0,0}$。
  ${\hat{x}}_{0,0}$可以任意，一般都初始化为0向量。在后续时刻经过多次测量后，系统的状态向量能够收敛到真实值附近。

$${\hat{x}}_{0,0}={\left[0,0,0,\cdots ,0\right]}^{\mathrm{T}}$$

  估计的状态$\hat{x}$的协方差矩阵称为估计不确定度（Estimate Uncertainty），用符号$P$表示。

$$P_{n,n}=\mathrm{E}\left(\left({\hat{x}}_{n,n}-{\mu }_{x_{n,n}}\right){\left({\hat{x}}_{n,n}-{\mu }_{x_{n,n}}\right)}^T\right)$$

  $P_{0,0}$表示在0时刻（初始时刻）对当前状态的估计不确定度。

$$P_{0,0}=\left[\begin{array}{ccccc}{\sigma }^2& 0& 0& \cdots & 0\\ 0& {\sigma }^2& 0& \cdots & 0\\ 0& 0& {\sigma }^2& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & 0\\ 0& 0& 0& 0& {\sigma }^2\end{array}\right]$$

  ${\hat{x}}_{0,0}$ 我们是随意设置的，因此0时刻状态的真实期望值${\mu }_{x_{0,0}}$很可能和我们随意设置的这个状态的估计值${\hat{x}}_{0,0}$有很大的偏差。而这个估计的不确定度自然很大，我们可以为每个维度的方差设置一个很大的值，表示初始的估计是“不靠谱”的。同样随着后续的不断测量，我们在每一时刻的估计不确定度$P_{n,n}$也会同系统状态向量的估计${\hat{x}}_{n,n}$一起不断更新。
  除了这${\hat{x}}_{0,0}$和$P_{0,0}$需要初始化以外，还有状态转移矩阵$F$，系统噪声的协方差矩阵$Q$，测量噪声的协方差矩阵$R$，观测矩阵$H$需要给出。这几个矩阵都是常量，不会随着时间变化，在后面提到了再作介绍。

预测 Predict

系统状态预测

  卡尔曼滤波的使用是建立在状态空间模型（State Space Model）之上的。状态外推方程（state extrapolate equation）用于状态的预测，它的一般形式：

$${\hat{x}}_{n+1,n}=F{\hat{x}}_{n,n}+G{u}_n+w_n$$

• ${\hat{x}}_{n+1,n}$是在$n$时刻对下一时刻的状态的估计；
• ${\hat{x}}_{n,n}$是在$n$时刻估计的$n$时刻的状态，$F$是状态转移矩阵（state transition matrix）；
• $u_n$是外部输入，是确定的，$G$是控制矩阵（control matrix）表示外部输入对系统状态的影响；
• $w_n$是过程噪声（process noise）或者称为系统噪声，无法直接测量。

估计不确定度预测

$$P_{n,n}=\mathrm{E}\left(\left({\hat{x}}_{n,n}-{\mu }_{x_{n,n}}\right){\left({\hat{x}}_{n,n}-{\mu }_{x_{n,n}}\right)}^T\right)$$

$$P_{n+1,n}=\mathrm{E}\left(\left({\hat{x}}_{n+1,n}-{\mu }_{x_{n+1,n}}\right){\left({\hat{x}}_{n+1,n}-{\mu }_{x_{n+1,n}}\right)}^T\right)$$

  把状态转移方程代入上式后可以得到下式，其中$Q$是过程噪声$w_n$的协方差矩阵。

$$P_{n+1,n}=F{P}_{n,n}{F}^T+Q$$

  因为过程噪声$w_n$是无法直接测量的，因此我们一般在初始化的时候就可以给出一个固定的过程噪声的协方差矩阵$Q$。$Q$矩阵里的值如果太小，则会对系统状态的估计更加取信于模型的预测值，而弱化了观测值的作用，导致对系统状态的估计产生滞后误差（lag error）；而如果$Q$矩阵里的值太大，导致估计的不确定度增大，则无法起到很好的滤波效果。

更新 Update

系统状态更新

  首先是状态的更新，从$n-1$时刻来到$n$时刻后，多了一个观测值/测量值$z_n$可以帮助我们更准确地进行估计。

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+K_n\left(z_n-H{\hat{x}}_{n,n-1}\right)$$

  $H$是观测矩阵，系统状态$x$中某些量我们可能是不能直接观测的，而只有部分可以直接观测，因此需要有一个观测矩阵$H$从状态向量中提出那些能够与观测值对应的部分。$z_n-H{\hat{x}}_{n,n-1}$被记作innovation，中文翻译为“新息”，可以理解为“新的部分”。当前时刻的估计是上一时刻的估计与测量值的加权，加权系数是卡尔曼增益$K_n$。卡尔曼增益的大小直接决定了新的估计值是更加侧重前一时刻的估计值还是测量值。

估计不确定度更新

$$P_{n,n}=\left(I-K_{n}H\right)P_{n,n-1}{\left(I-K_{n}H\right)}^{\mathrm{T}}+K_n{R}_n{K}_n^{\mathrm{T}}$$

  估计不确定度的更新是根据状态更新的方程得到的，可以重写一下状态更新方程：

$${\hat{x}}_{n,n}=\left(I-K_{n}H\right){\hat{x}}_{n,n-1}+K_n{z}_n$$

  再根据期望代数中的公式即可得到$P_{n,n}$的计算表达式。其中$z_n$是测量值，而$R_n$是测量噪声的协方差矩阵，

$$z_n=H{x}_n+v_n$$

  $x_n$是$n$时刻系统真实的状态值，也就是系统状态的期望值${\mu }_{x_{n,n}}$, $v_n$是测量噪声，$R_n=\mathrm{E}\left(v_n{v}_n^{\mathrm{T}}\right)$。详细的推导过程可以看这里。

卡尔曼增益计算

  卡尔曼增益是令估计不确定度$P_{n,n}$的迹最小化得到的。$P_{n,n}$的对角线是系统状态$x_{n,n}$每一个维度的方差，它的迹是这些方差之和。最小化$tr(P_{n,n})$可以通过对$K_n$求导，并令导数为零矩阵，然后得到最优的$K_n$。详细的推导过程可以看这里，最后得到如下结果：

$$K_n=P_{n,n-1}{H}^{\mathrm{T}}{\left(H{P}_{n,n-1}{H}^{\mathrm{T}}+R_n\right)}^{-1}$$

  将这一结果再代入$P_{n,n}$的计算公式，可以简化$P_{n,n}$的计算：

$$P_{n,n}=\left(I-K_{n}H\right)P_{n,n-1}$$

总结

  图片引用自这里

举例

  在C6000 DSP上实现二维坐标点(x,y)的卡尔曼滤波，源代码。
  首先实现了Matlab版本，依次给出10个二维坐标点作为观测值，然后将卡尔曼滤波后的结果输出显示；DSP上采用相同的参数设置，加载同样的10个二维坐标点，将结果输出显示后与Matlab的结果进行对比，确保结果正确后统计处理所需的时间。

匀速直线运动模型

$$state=\left[x,v_x,y,v_y\right]$$

$$F_{CV}=\left[\begin{array}{cccc}1& 1& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 1\\ 0& 0& 0& 1\end{array}\right]$$

  10次计算结果一致，DSP上Kalman滤波器初始化需要$6.2\mu \mathrm{s}$，每次更新和预测平均需要$4.4\mu \mathrm{s}$。

匀加速直线运动模型

$$state=\left[x,v_x,a_x,y,v_y,a_y\right]$$

$$F_{CA}=\left[\begin{array}{cccccc}1& 1& 0.5& 0& 0& 0\\ 0& 1& 1& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 1& 0.5\\ 0& 0& 0& 0& 1& 1\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$

  10次计算结果一致，DSP上Kalman滤波器初始化需要$7.0\mu \mathrm{s}$，每次更新和预测平均需要$5.4\mu \mathrm{s}$。

EKF & UKF

  如果系统状态的状态转移方程或者观测方程是非线性的，那么就不能简单地用固定的$F$和$H$来表示系统的状态转移和观测矩阵。这时候就需要用到EKF或者UKF了。
  EKF解决非线性的问题采用的是一阶泰勒展开来近似。这相比于KF就引入了额外的一些计算量，因为$F$和$H$不是固定的，每次系统状态改变都需要重新计算。
  UKF采用的是类似蒙特卡洛分析的方法，用几个中心对称的sigma点来估计非线性变换后的随机变量的协方差矩阵。