推荐资料

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

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

基础

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

随机变量的期望与方差

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

E(x)=1n∑i=0n−1xi      V(x)=1n−1∑i=0n−1(xi−μ)2{\rm{E} }(x) = {1 \over n}\sum\limits_{i = 0}^{n - 1} { {x_i}\;\;\;{\rm{V} }(x)} = {1 \over {n - 1} }\sum\limits_{i = 0}^{n - 1} { { {\left( { {x_i} - \mu } \right)}^2} }

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

期望代数（Expectation Algebra）

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

V(x)=E((x−μx)(X−μx)T){\rm{V} }(x) = {\rm{E} }\left( {\left( {x - {\mu _x} } \right){ {\left( {X - {\mu _x} } \right)}^{\rm T} } } \right)

  如果对随机变量xx进行线性变换得到y=Fxy=Fx，FF是N×NN\times N的方阵，xx是NN维列向量。那么随机变量yy的方差为：

V(y)=FV(x)FT{\rm{V} }(y) = F{\rm{V} }(x){F^{\rm T} }

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

卡尔曼滤波

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

初始化 Initiate

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

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

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

Pn,n=E((x^n,n−μxn,n)(x^n,n−μxn,n)T){P_{n,n} } = {\rm{E} }\left( {\left( { { {\hat x}_{n,n} } - {\mu _{ {x_{n,n} } } } } \right){ {\left( { { {\hat x}_{n,n} } - {\mu _{ {x_{n,n} } } } } \right)}^T} } \right)

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

P0,0=[σ200⋯00σ20⋯000σ2⋯0⋮⋮⋮⋱00000σ2]{P_{0,0} } = {\begin{bmatrix} { {\sigma ^2} }&0&0& \cdots &0\\ 0&{ {\sigma ^2} }&0& \cdots &0\\ 0&0&{ {\sigma ^2} }& \cdots &0\\ \vdots & \vdots & \vdots & \ddots &0\\ 0&0&0&0&{ {\sigma ^2} } \end{bmatrix} }

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

预测 Predict

系统状态预测

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

x^n+1,n=Fx^n,n+Gun+wn{ {\hat x}_{n + 1,n} } = F{ {\hat x}_{n,n} } + G{u_n} + {w_n}

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

估计不确定度预测

Pn,n=E((x^n,n−μxn,n)(x^n,n−μxn,n)T){P_{n,n} } = {\rm{E} }\left( {\left( { { {\hat x}_{n,n} } - {\mu _{ {x_{n,n} } } } } \right){ {\left( { { {\hat x}_{n,n} } - {\mu _{ {x_{n,n} } } } } \right)}^T} } \right)

Pn+1,n=E((x^n+1,n−μxn+1,n)(x^n+1,n−μxn+1,n)T){P_{n + 1,n} } = {\rm{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)

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

Pn+1,n=FPn,nFT+Q{P_{n + 1,n} } = F{P_{n,n} }{F^T} + Q

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

更新 Update

系统状态更新

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

x^n,n=x^n,n−1+Kn(zn−Hx^n,n−1){ {\hat x}_{n,n} } = { {\hat x}_{n,n - 1} } + {K_n}\left( { {z_n} - H{ {\hat x}_{n,n - 1} } } \right)

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

估计不确定度更新

Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT{P_{n,n} } = \left( {I - {K_n}H} \right){P_{n,n - 1} }{\left( {I - {K_n}H} \right)^{\rm{T} } } + {K_n}{R_n}K_n^{\rm{T} }

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

x^n,n=(I−KnH)x^n,n−1+Knzn{ {\hat x}_{n,n} } = \left( {I - K_nH} \right){ {\hat x}_{n,n - 1} } + {K_n}{z_n}

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

zn=Hxn+vn{z_n} = H{x_n} + {v_n}

  xnx_n是nn时刻系统真实的状态值，也就是系统状态的期望值μxn,n{\mu _{ {x_{n,n} } } }, vnv_n是测量噪声，Rn=E(vnvnT){R_n} = {\rm{E} }\left( { {v_n}v_n^{\rm{T} } } \right)。详细的推导过程可以看这里。

卡尔曼增益计算

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

Kn=Pn,n−1HT(HPn,n−1HT+Rn)−1{K_n} = {P_{n,n - 1} }{H^{\rm{T} } }{\left( {H{P_{n,n - 1} }{H^{\rm{T} } } + {R_n} } \right)^{ - 1} }

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

Pn,n=(I−KnH)Pn,n−1{P_{n,n} } = \left( {I - {K_n}H} \right){P_{n,n - 1} }

总结

举例

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

匀速直线运动模型

state=[x,vx,y,vy]state = \left[ {x,{v_x},y,{v_y} } \right]

FCV=[1100010000110001]{F_{CV} } = {\begin{bmatrix} 1&1&0&0\\ 0&1&0&0\\ 0&0&1&1\\ 0&0&0&1 \end{bmatrix} }

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

匀加速直线运动模型

state=[x,vx,ax,y,vy,ay]state = \left[ {x,{v_x},{a_x},y,{v_y},{a_y} } \right]

FCA=[110.5000011000001000000110.5000011000001]{F_{CA} } = {\begin{bmatrix} 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{bmatrix} }

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

EKF & UKF

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