手撕卡尔曼滤波器

卡尔曼滤波器（Kalman Filter），从字面意思上来看，“Filter滤波器”一词并不能很好地体现其特性。卡尔曼滤波器用一句话来说就是“Optimal Recursive Data-Processing Algorithm”，即为“最优化 递归 数字处理 算法”，它更像是一种观测器，而不是一般意义上的滤波器。卡尔曼滤波器的应用非常广泛，尤其是在导航中。它的广泛应用是因为世界中存在大量的不确定性，当我们描述一个系统时，这个不确定性主要体现在三个方面：

1. 不存在完美的数学模型
2. 系统的扰动不可控，也很难建模
3. 测量传感器存在误差

递归算法

下面看一个例子，多次用同一把尺子测量同一枚硬币的直径，用 zk 表示第k次的测量结果。由于种种误差，测量得到：z1=50.1mmz2=50.4mmz3=50.2mm

此时如果要估计真实结果，自然而然地会想到取平均值。用 x^k 表示第k次的估计值，可以得到：x^k=1k(z1+z2+⋯+zk)=1k(z1+z2+⋯+zk−1)+1k(zk)=k−1k1k−1(z1+z2+⋯+zk−1)+1k(zk)=k−1kx^k−1+1k(zk)=x^k−1+1k(zk−x^k−1)

上式中，第三行 1k−1(z1+z2+⋯+zk−1) 就是 k−1 次的平均值 x^k−1

观察最后一行结论，k↑,1k−1→0,x^k→x^k−1 ，也就是说，随着k的增加，此时拥有了大量的数据，对估计的结果就比较有信心了，测量的结果就不是很重要了。相反，如果k比较小， 1k−1 就会比较大，测量结果 zk 就会起到很大的作用，尤其是测量结果和估计值差距比较大的时候。

令 Kk=1k−1 ，则此时公式可以表示为：x^k=x^k−1+Kk(zk−x^k−1)

上式表示的含义为：当前的估计值 = 上一次的估计值 + 系数 * ( 当前测量值 - 上一次的估计值 ) ，其中的 Kk 就是卡尔曼增益/因数（Kalman Gain），通过这个公式可以看出，新的估计值 x^k 与上一次的估计值 x^k−1 有关，上一次的又与上上次的有关，这就是一种递归思想（Recursive），这也是卡尔曼滤波器的优势，他不需要追溯很久以前的数据，只需要上一次的就可以。下面来讨论一下这个 Kk ：

引入两个误差：

1. 估计误差 eEST （e代表误差error，EST代表估计estimate）
2. 测量误差 eMEA （e代表误差error，MEA代表测量measurement）

则 Kk 可以表示为Kk=eESTk−1eESTk−1+eMEAk

这个公式是卡尔曼滤波中的核心公式，具体的推导后文会讲到。下面对这个公式进行讨论。在k时刻，

1. 当 eESTk−1≫eMEAk ， Kk→1 ，此时 x^k=zk ，这说明当第k-1次的估计误差远大于第k次的测量误差时，第k次的估计值很趋近于测量值。（估计的误差大，测量的误差小，更信任测量值）
2. 当 eESTk−1≪eMEAk ， Kk→0 ，此时 x^k=x^k−1 ，这说明当第k-1次的估计误差远小于第k次的测量误差时，第k次的估计值很趋近于测量值。（估计的误差小，测量的误差大，更信任估计值）

运用以上知识，解决一个实际问题可以分为三步：

1. 计算卡尔曼增益 Kk ，公式见前文
2. 计算估算值 x^k ，公式见前文
3. 更新估计误差 eESTk=(1−Kk)eESTk−1 ，此公式推导见后文。

再看前文测量硬币直径的例子，实际长度 x=50mm ，对于第一次测量：x^0=40mmeEST0=5mmz1=51mmeMEAk=3mm

估计值是是随便估计的一个值；估计误差随便给一个数；测量值是实际测量得到的；由于测量工具不变，测量误差是一个恒定值。接下来进行递归：

蓝色是测量值 zk ，红色为估计值 x^k ，可以看到经过反复迭代，估计值越来越接近实际值。这就是卡尔曼滤波器的递归思想。

数学基础

数据融合

下面举例说明数据融合（Data Fusion）

分别用两个称称一个东西，得到两个结果，分别为z1=30mmz2=32mm

两个称都有误差，两个称的标准差（Standard Deviation）分别为：σ1=2gσ2=4g

他们均符合正态分布/高斯分布（Natural/Gaussin Distribution）

如果用这两个结果去估计真实值 z^=? ，可以用到上一节的思想，则z^=z1+K(z2−z1),k∈[0,1]k=0,z^=z1k=1,z^=z2

求k使得 σz^ 最小，也就是使得方差 Var(z^) 最小σz^2=Var(z^)=Var(z1+K(z2−z1))=Var(z1−Kz1+Kz2)=Var((1−K)z1+Kz2)=Var((1−K)z1)+Var(Kz2)=(1−K)2Var(z1)+K2Var(z2)=(1−K)2σ12+K2σ22

可以看到，第四行中 (1−K)z1 和 Kz2 是互相独立的，因为两个称的结果不会互相影响，所以由于方差的性质可以写成两个独立的方差。要求这个式子的最小值，就要对K求导并令导数等于0。可以解出K：dσz^2dK=0−2(1−K)σ12+2Kσ22=0K=σ12σ12+σ22=2222+42=0.2

将K带入上面的式子，得到 z^=30.4 。此时为最优解。计算此时的标准差 σz^=1.79 ，绘出正态分布图：

得到比两个图形更高更瘦的图形，这个过程就叫数据融合。

协方差矩阵

协方差矩阵（Covarince Matrix）是把方差和协方差在一个矩阵中表示出来，体现了变量间的联动关系。下面举例说明：

令身高为x，体重为y，年龄为z，分别计算平均值，方差和协方差（拿x举例）：σx2=13((179−180.3)2+(187−180.3)2+(175−180.3)2)=24.89σxσy=13((179−180.3)(74−75)+(187−180.3)(80−75)+(175−180.3)(71−75))=18.7=σyσx

观察协方差的每一项，如果两个括号内都为负数，相乘为正数；两个括号内都为正数，相乘仍为正数；但一正一负相乘得到负数。所以最后加在一起的结果如果是正数，说明这两个变量的变化方向是一样的；如果是负数，说明这两个变量的变化方向是相反的。

协方差矩阵表示形式为：P=[σx2σxσyσxσzσyσxσy2σyσzσzσxσzσyσz2]

如果需要编程实现，可以通过以下方法求得P（其中a为过渡矩阵）a=[x1y1z1x2y2z2x3y3z3]−13[111111111][x1y1z1x2y2z2x3y3z3]P=13aTa

多取一些数据，得到协方差矩阵，可以利用协方差矩阵分析各个数据之间的关系；

从协方差矩阵中可以看到，对角线上的数为方差，这些数据比较大，说明了这些变量之间跨度比较大。剩下的数据为协方差，体重和身高的协方差比较大，说明他们是正相关的，身高增加体重也增加；而年龄和其余两者的协方差比较小，说明他们之间的相关性比较小。

状态空间方程

状态空间表达（State Space Representation），现代控制理论就是以状态空间方程为基础的。以弹簧振动阻尼系统为例：

动态方程表达式为：mx¨+Bx˙+kx=F

将F定义为u，也就是系统的输入（Input）。将其转化成状态空间表达形式，定义两个状态（State）变量 x1=x ， x2=x˙ ，则x1˙=x2x2˙=x¨=1mu−Bmx˙−kmx=1mu−Bmx2−kmx1

这样就用两个一阶微分方程表达出来了。定义两个测量（Meansurement）变量，位置 z1=x=x1 ，速度 z2=x˙=x2

将上面的式子改写成矩阵形式：[x1˙x2˙]=[01−km−Bm][x1x2]+[01m]u[z1z2]=[1001][x1x2]

归纳出状态空间的表达形式：x˙(t)=Ax(t)+Bu(t)z(t)=Hx(t)

这是一种连续的表达形式， x˙(t) 为x对时间的导数，体现了x随时间的变化。

如果写成离散形式(本节不深入讲解离散型，只做了解)，其中下标 k−1,k,k+1 里面的1代表一个时间单位，即为采样时间（Sample Time），这种形式体现了上一步到这一步的一种变化：xk=Axk−1+Buk−1zk=Hxk

如果增加一些开头提到的不确定性，其中 wk−1 为过程噪音（Process Noise）， vk 为测量噪音（Meansurement Noise）：xk=Axk−1+Buk−1+wk−1zk=Hxk+vk

也就是说当估计结果 xk 不准确，测量结果 zk 也不准确的情况下，如何估计一个精确的 x^k ？这就是卡尔曼滤波器所要解决的问题。

卡尔曼增益数学推导

在上文的状态空间方程中， xk 为状态变量，A为状态矩阵，B为控制矩阵， uk 为控制， wk−1 为过程噪音， vk 为测量噪音，其中噪声是不可测的，是系统不确定性的表现。但过程噪声可以假设其符合正态分布 P(w)∼N(0,Q) ，其中0为期望，Q为协方差矩阵：Q=E(wwT)=E([w1w2][w1w2])=E([w12w1w2w2w1w22])=[E(w12)E(w1w2)E(w2w1)E(w22)]=[σw12σw1σw2σw2σw1σw22]

通过Q这个协方差矩阵可以表示出过程噪声的方差，亦可以表示出过程噪声之间的关系。

对于测量噪声也同样认为符合正态分布 P(v)∼N(0,R) ， R=E(vvT) 同样为协方差矩阵，形同Q。

但建模的时候噪音是不知道的，所以我们只能测得除掉噪声其余的项，表示为 x^k− ，这是一个估计值所以要加一个hat。此时我们没有做任何处理，只是根据上面的式子去掉噪声得来，所以在上面加一个负号，代表先验估计。x^k−=Ax^k−1+Buk−1zk=Hxk→x^kmea=H−1zk

上式中第一行 x^k− 为算出来的结果，第二行 x^kmea 为测出来的结果，但他们都不具备测量噪声这一项，他们都是不太准确的。这时可以运用卡尔曼滤波器通过两个不太准确的结果得到一个准确的结果。

回忆之前数据融合的概念，对于最终的估计值， x^k （后验估计） 可以表示为：x^k=x^k−+G(H−1zk−x^k−),G∈[0,1]

• 当 G=0 ， x^k=x^k− ，此时更相信计算结果
• 当 G=1 ， x^k=H−1zk ，此时更相信测量结果

在许多教材中会令 G=KkH ，卡尔曼滤波器可以表示为：x^k=x^k−+Kk(zk−Hx^k−),Kk∈[0,H−1]

• 当 Kk=0 ， x^k=x^k− ，此时更相信计算结果
• 当 Kk=H−1 ， x^k=H−1zk ，此时更相信测量结果

接下来的目标就是寻找 Kk ，使得误差最小，也就是说使得估计值 x^k 趋近于实际值 xk 。很明显， Kk 的取值与计算误差和测量误差息息相关，当测量误差特别大时会更相信计算出来的结果，当计算误差特别大时会更相信测量出来的结果。

令误差 ek=xk−x^k ，其同样符合正态分布 P(ek)∼N(0,P)P=E(eeT)=[σe12σe1σe2σe2σe1σe22]

如果新估计出的 xk 距离实际值越小，说明误差的方差越小，说明越接近期望值0。而方差之和为P的迹 tr(P)=σe12+σe22 ，所以要想让方差最小，接下来的目标就变成了选取合适的 Kk ，使得协方差矩阵P的迹最小P=E(eeT)=E((xk−x^k)(xk−x^k)T)

下面求 xk−x^k ，其中的 zk 是真实测量的结果，所以 zk=Hxk+vk 。因为 ek=xk−x^k ，所以可以定义先验误差 ek−=xk−x^k−xk−x^k=xk−(x^k−+Kk(zk−Hx^k−))=(I−KkH)(xk−x^k−)−Kkvk=(I−KkH)ek−−Kkvk

前文提到， ek− 和 ek− 的方差都是0，且令先验误差的协方差矩阵 Pk−=E(ek−ek−T) 。此时第k步的 Pk 可以整理为Pk=E((xk−x^k)(xk−x^k)T)=E(((I−KkH)ek−−Kkvk)((I−KkH)ek−−Kkvk)T)=Pk−−KkHPk−−(KkHPk−)T+KkHPk−HTKkT+KkRKkT

此时可以计算 Pk 的迹tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)

寻找k使得 tr(Pk) 有最小值，对k求导并寻找极值点（求导法则略）dtr(Pk)dk=0−2(HPk−)T+2KkHPk−HT+2KkR=0Kk=Pk−HTHPk−HT+R

至此，我们推出的 Kk 就是卡尔曼增益。这也是卡尔曼滤波器中最核心的公式。

其中的R是测量噪声的协方差矩阵，R的大小代表了测量噪声方差的大小，也就是 测量噪声的大小。分析 Kk ：

• 当R很大， Kk→0 ， x^k=x^k− ，此时更相信计算结果
• 当R很小， Kk=Pk−HTHPk−HT=H−1,x^k=H−1zk ，此时更相信测量结果

误差协方差矩阵数学推导

现在来推导卡尔曼增益 Kk 中的先验误差的协方差矩阵 Pk− 。

根据前文的结论，我们可以得到真实值 xk ，先验估计 x^k− ，后验估计 x^k ，卡尔曼增益 Kkxk=Axk−1+Buk−1+wk−1x^k−=Ax^k−1+Buk−1x^k=x^k−+Kk(zk−Hx^k−)Kk=Pk−HTHPk−HT+R

根据 Pk− 的定义， Pk−=E(ek−ek−T) ，其中误差为真实值减估计值，ek−=xk−x^k−=Axk−1+Buk−1+wk−1−Ax^k−1−Buk−1=A(xk−1−x^k−1−)+wk−1=Aek−1+wk−1

则 Pk− 可以整理为：Pk−=E(ek−ek−T)=E((Aek−1+wk−1)(Aek−1+wk−1)T)=AE(ek−1−ek−1−T)AT+E(wk−1−wk−1−T)=APk−1AT+Q

根据上式就可以利用卡尔曼滤波器估计状态变量的值了。分为以下步骤

• 预测
  1. 先验估计x^k−=Ax^k−1+Buk−1
  2. 先验误差协方差矩阵Pk−=APk−1AT+Q
• 校正
  1. 卡尔曼增益Kk=Pk−HTHPk−HT+R
  2. 后验估计x^k=x^k−+Kk(zk−Hx^k−)

根据以上四步就可以得到最优估计值，也就是后验估计值 x^k 。

先验误差协方差矩阵 Pk− 中包含上一次的 Pk−1− 项，每次矫正厚需要更新先验误差协方差矩阵。将卡尔曼增益带入可以求得：Pk=Pk−−KkHPk−−(KkHPk−)T+KkHPk−HTKkT+KkRKkT=(I−KkH)Pk−

所以第五步为

1. 更新先验误差协方差Pk=(I−KkH)Pk−

以上就是完整的卡尔曼滤波器的五个公式。

可以看到，每次预测都会用到上一次的结果，所以在最开始要赋予初值 x^0 和 P0 ，初值的选取会在下文提及。

扩展卡尔曼滤波

前文讲到，卡尔曼滤波器在线性系统里可以得到最优估计值。对于非线性系统，可以将其线性化再进行处理，这种滤波器叫做扩展卡尔曼滤波器（Extend Kalman Filter），简称EKF。

线性系统可以表示为：xk=Axk−1+Buk−1+wk−1zk=Hxk+vk

而非线性系统，无法用线性的状态空间方程表达，而是可以表示为：xk=f(xk−1,uk−1,wk−1)zk=h(xk,vk)

其中f为过程方程，h为测量方程，这是两个非线性的表达形式。注意无论是线性还是非线性，误差v和w都是符合正态分布的。但正态分布的随机变量通过非线性系统以后就不再是正态分布的了。所以如果还想对非线性系统进行卡尔曼滤波，需要对其线性化（Linearization），用泰勒级数展开，不赘述过程只给出结论：f(x)=f(x0)+∂f∂x(x−x0)

如果需要线性化一个系统，需要找到一个点（Operating Point），在这个点附近进行线性化。对于非线性系统来说最好的线性化的点就是它的真实点，但由于系统有误差，无法知道真实值，所以无法在真实点进行线性化，所以过程方程 f(xk) 只能在 x^k−1 附近，也就是k-1时刻（上一次）的后验估计附近进行线性化。

由于不知道误差 wk−1 是多少，所以将其假设为0，定义 f(x^k−1,uk−1,0)=x~k ，可以得到xk=f(x^k−1,uk−1,0)+Ak(xk−1−x^k−1)+Wkwk−1Ak=∂f∂x|x^k−1,uk−1Wk=∂f∂w|x^k−1,uk−1

例：x1=x1+sinx2=f1x2=x12=f2Ak=∂f∂x|x1^k−1,x2^k−1=[∂f1∂x1∂f1∂x2∂f2∂x1∂f2∂x2]|x1^k−1,x2^k−1=[1cosx22x10]|x1^k−1,x2^k−1=[1cosx2^k−12x1^k−10]

可以看出，A矩阵随着K的变化而变化，所以每次要重新计算A矩阵。

同理对于测量方程， zk 在 x~k 附近进行线性化。由于不知道误差 vk ，所以将其假设为0，定义 h(x^k,0)=z~kzk=h(x~,0)+Hk(xk−x~k)+VkvkHk=∂h∂x|x^kVk=∂h∂v|x^k

这样就把非线性系统线性化了：xk=x~k+Ak(xk−1−x^k−1)+Wkwk−1zk=z~k+H(xk−x~k)+Vvk

其中的 Wkwk−1 和 Vvk 也都是正态分布（ WQWT 相当于矩阵中W的平方）：P(w)∼N(0,R)P(Wwk−1)∼N(0,WQWT)P(Vvk)∼N(0,VRVT)

将卡尔曼滤波器的线性化部分替换成对应非线性的部分，就可以得到扩展卡尔曼滤波器的五个公式：

• 预测
  1. 先验估计x^k−=f(x^k−1,uk−1,0)
  2. 先验误差协方差矩阵Pk−=APk−1AT+WQWT
• 校正
  1. 卡尔曼增益Kk=Pk−HTHPk−HT+VRVT
  2. 后验估计x^k=x^k−+Kk(zk−h(x^k,0))

1. 更新先验误差协方差Pk=(I−KkH)Pk−

文章作者: Fan Ziqi

文章链接: https://www.robotsfan.com/posts/b4727fbe.html