卡尔曼滤波系列1——卡尔曼滤波
作者简介：秃头小苏，致力于用最通俗的语言描述问题
往期回顾：霍夫直线检测原理详解 霍夫直线检测代码实战
近期目标：拥有2000粉丝
支持小苏：点赞、收藏⭐、留言
原创不易，转载请附上原文链接
文章目录
- 卡尔曼滤波系列1——卡尔曼滤波
-
- 写在前面
- 卡尔曼滤波宏观理解
- 卡尔曼滤波引导小例
- 卡尔曼滤波数据融合小例
- 卡尔曼滤波公式
- 卡尔曼公式应用举例
- 结语
- 参考链接
卡尔曼滤波系列1——卡尔曼滤波
写在前面
终于开始写卡尔曼滤波系列了，有点小激动呢其实在之前的文章中我也简单介绍过卡尔曼滤波，enmmm是的，只是简单的介绍了一下，主要是帮大家直观理解卡尔曼滤波，并没有对卡尔曼滤波的公式进行讲解和推导，那么这一系列呢，我将着重于一些公式的推导来和大家唠唠卡尔曼滤波
谈到公式推导大家可能多少有点抗拒，从我之前写的文章我也发现，实战的文章似乎总是比理论文章的阅读量高很多，但是理论也确实是我们不可缺少的，我们应坦然的面对这篇文章呢，我打算介绍一些理论上的问题，但也会通过一些例子来带入，卡尔曼的核心推导我将给出推导的视频链接，这样可能比文字描述更加有效。【这样其实就意味着这篇文章其实推导的过程也并不多，但会介绍一些重要的概念，对推导过程感兴趣的可以观看视频进行了解】
卡尔曼滤波宏观理解
首先先来对卡尔曼滤波做一个宏观上的理解，即我们使用卡尔曼滤波能干什么？卡尔曼滤波是用来对事物的状态进行估计的，为什么要对事物的状态进行估计呢？那肯定是我们无法准确的知道事物的当前状态，这时候我们就要进行估计了【但是我们不可能是随便的估计，往往是根据一些经验来进行估计，所以估计的结果是相对准确的】；另一方面，为了估计一个事物的状态，我们还可以借助一些仪器来进行测量，但是用仪器进行测量就会存在一定的测量误差。这时候就会产生一个问题，我们应该相信我们的估计结果还是相信测量结果呢？我想大概率人们都会这么想：如果我们使用的测量仪器是从某个小厂家手里花五毛钱买的劣质仪器，那么我们会更相信估计结果；如果买的仪器价值千金，那我们则有理由相信测量结果。其实呢，我们往往都会给这两种结果分配不同的比例，表示我们更信任哪种结果。那么卡尔曼滤波就是结合事先的估计和仪器的测量这两种手段来估计事物状态的方法。
我们举个例子来说明上文所要表达的含义。如下图所示，我们用炉子给水壶烧水，现在水沸腾了，我们结合初中知识，估计水的温度是95.4°C【受大气压影响，没有100度】，即Xk=95.4°C。这时候我们用一个温度计测量水的温度，发现其值为yk=94.1°C。发现xk和yk不一致，该相信谁呢？我们假设温度计的可信度为0.6，估计的为0.4，则最终的的水温=0.6*94.1+0.4*95.4=94.62。
卡尔曼滤波引导小例
通过上文你可能对卡尔曼滤波有了一个大概的了解，即它是用来对一些不确定的物体的状态进行估计的。下面举一个生活中的小例子来引入卡尔曼滤波：现有一枚硬币、一把尺子，目的是尽可能准确的得出硬币的尺寸，共测量k次，每次的测量结果记为Z1，Z2，… Zk 。我们如何估计硬币的尺寸呢？很自然的一个想法是取平均值，计算过程如下图：
通过这个例子我想表达什么观点呢，我们可以聚焦到上例的最终结果： X ^ k = X ^ k − 1 + 1 k ( Z k − X ^ k − 1 ) {{\rm{\hat X}}_k}{\rm{ = }}{{\rm{\hat X}}_{k - 1}} + \frac{1}{k}({Z_k} - {{\rm{\hat X}}_{k - 1}}) X^k=X^k−1+k1(Zk−X^k−1)。可以发现进行k次测量后的估计值 = 进行（k-1）次测量后的估计值 + 1 k \frac{1}{k} k1（第k次的测量结果 - 进行（k-1）次测量后的估计值）。不知道大家有没有嗅到一些函数递归的味道，即估计某次测量的结果是和估计上次的结果有关系的，其实卡尔曼滤波就含有这种递归的思想再来分析一下上式，若随着测量的次数k增大, 1 k \frac{1}{k} k1将趋于0，上式将为： X ^ k = X ^ k − 1 {{\rm{\hat X}}_k}{\rm{ = }}{{\rm{\hat X}}_{k - 1}} X^k=X^k−1，即随着k的增大，第k次的估计值只和第（k-1）次的估计值有关，而和第k次的测量值无关，也即随着k增大，测量结果不在重要。其实这也非常好理解，当我们测量了很多次，前面的估计值其实已经非常接近真实值了，所以再多测一次的意义就变得不大了；相反的，若k非常小，即k=1（k表示测量次数，k=0无意义）时，上式将为： X ^ k = Z k {{{\rm{\hat X}}}_k}{\rm{ = }}{Z_k} X^k=Zk，即此时的估计值完全由测量值决定。其实上式类似于卡尔曼滤波中的一个公式，当然这是后话了，这里我们加以注意即可。进一步的，我们可以将上式中的 1 k \frac{1}{k} k1替换为Kk表示，Kk就是卡尔曼系数。
这里我还想给出Kk的计算公式，如下图所示：
当然，这里不需要理解为什么，有一个大概的印象就好。我们从宏观上来理解这个公式：
- 当 e E S T ≫ e M E A {e_{EST}} \gg {e_{MEA}} eEST≫eMEA时，即估计误差远大于测量误差时，Kk趋于1， X ^ k {{{\rm{\hat X}}}_k} X^k趋向于 Z k {\rm}{Z_k} Zk，表示相信测量值。
- 当 e E S T ≪ e M E A {e_{EST}} \ll {e_{MEA}} eEST≪eMEA时，即估计误差远小于测量误差时，Kk趋于0， X ^ k {{{\rm{\hat X}}}_k} X^k趋向于 X ^ k − 1 {{\rm{\hat X}}_{k - 1}} X^k−1，表示相信估计值。
卡尔曼滤波数据融合小例
大家既然看了卡尔曼滤波，那么肯定是知道卡尔曼滤波有数据融合的作用了，实际上，卡尔曼滤波就起到了一个数据融合的作用，从这篇文章的写在前面可以看出，卡尔曼滤波不就是把估计值和测量值进行了融合而得到了一个新的结果嘛读到这里不知道读者有没有疑惑，我们不是还没介绍卡尔曼滤波的过程嘛，怎么就直接跳到卡尔曼滤波的应用了。确实是这样的，但我想的是通过一些例子来过渡到卡尔曼公式的推导，当然这些例子都是非常简单的样例，所以就让我们一起来看看这个例子吧
现有一个电子秤和一个普通秤，同时测一个物体，电子秤测得的重量结果为Z1=30，标准差为 σ 1 = 2 {\sigma _1} = 2 σ1=2；普通秤测得的重量结果为Z2=32，标准差为 σ 1 = 4 {\sigma _1} = 4 σ1=4。现希望通过这两个结果来得到一个最优的估计值
首先我们可以先画出两种秤测得物体重量的分布图，如下：【假设它们是满足正态分布】
上图Z1表示电子秤测量结果分布情况，Z2表示普通秤测量结果分布情况。在没进行计算之前，我觉得我们可以先猜一下真实值。由上可知，电子秤的标准差只有2，而普通秤的标准差为4，这表示电子秤测量结果更加稳定，这样测量结果可能更加准确，最终我们融合的结果可能就更加偏向于电子秤测得的结果，即更接近于30。当然这些仅是我的猜测结果，现我们通过计算看看最优的估计值是否和我们的猜测一致。
还是先来明确一下我们的目标：根据两个测量结果估计真实值 Z ^ {{{\rm{\hat Z}}}} Z^。直接来说思路叭，在卡尔曼滤波引导小例中，我们也对硬币的真实尺寸做了一个估计，这里我们能否借鉴呢？在引导小例中，我们最后得到了一个硬币估计值的公式： X ^ k = X ^ k − 1 + 1 k ( Z k − X ^ k − 1 ) {{\rm{\hat X}}_k}{\rm{ = }}{{\rm{\hat X}}_{k - 1}} + \frac{1}{k}({Z_k} - {{\rm{\hat X}}_{k - 1}}) X^k=X^k−1+k1(Zk−X^k−1)。这个公式所表达的含义为：当前估计值 = 上一次的估计值 + 系数*(当前测量值 - 上一次的估计值)。那么这个结论能否被我们所利用，可能有人说这个例子中又没有估计值啊，这说明你的思维被禁锢了，我们为什么不能把我们的某次测量值就当成是估计值呢现我们将第一次测量结果Z1作为估计值，Z2作为测量值，这样我们就可以得到估计的真实值 Z ^ {{{\rm{\hat Z}}}} Z^的表达式，如下：
Z ^ = Z 1 + k ( Z 2 − Z 1 ) \hat Z = {Z_1} + k({Z_2} - {Z_1}) Z^=Z1+k(Z2−Z1)
得到这个公式我们又能做什么呢，而且还有一个未知系数k，我们第一步肯定是要求出这个K。我们的目的是求出测量的最优结果 Z ^ {{{\rm{\hat Z}}}} Z^，若我们使 Z ^ {{{\rm{\hat Z}}}} Z^的标准差 σ Z ^ {\sigma _{\hat Z}} σZ^最小，也即方差 v a r ( Z ^ ) {\mathop{\rm var}} (\hat Z) var(Z^)最小，会使 Z ^ {{{\rm{\hat Z}}}} Z^的结果最优。那么我们现在要做的就是求未知数k,使得 v a r ( Z ^ ) {\mathop{\rm var}} (\hat Z) var(Z^)最小，推导过程如下：
上图我们已经得到了 σ Z ^ 2 = ( 1 − k ) 2 σ 1 2 + k 2 σ 2 2 {\sigma _{\hat Z}}^2 = {{\rm{(1 - k)}}^2}{\sigma _1}^2 + {k^2}{\sigma _2}^2 σZ^2=(1−k)2σ12+k2σ22，现需求出k使 σ Z ^ 2 {\sigma _{\hat Z}}^2 σZ^2最小，则令上式对k求导等于0找出极值点即可，求导如下图所示：
至此我们得到了最终的估计测量值和标准差，可以看到这个结果和我们之前猜测的一样更加的接近30，而且可以发现这个结果的标准差比之前的两次测量的标准差都要小。这里我画出三次结果的分布图供大家直观感受一下【after_merge表示融合后的结果，可以看出融合后的结果分布更高更瘦了，其实就是效果更加好了】
卡尔曼滤波公式
这一小节将为大家带来卡尔曼公式还是通过一个小例子来进行讲解。如下图所示，有一辆小车在马路上行驶，其速度为s，加速度为a，当前时刻的位置用P表示。
根据初中物理匀加速直线运动的知识，可以得到公式：
{ P k = P k − 1 + S k − 1 Δ t + 1 2 a ⋅ Δ t 2 S k = S k − 1 + a ⋅ Δ t \left\{ \begin{array}{l}{P_k} = {P_{k - 1}} + {S_{k - 1}}\Delta t + \frac{1}{2}a \cdot \Delta {t^2}\\ {S_k} = {S_{k - 1}} + a \cdot \Delta t \end{array} \right. {Pk=Pk−1+Sk−1Δt+21a⋅Δt2Sk=Sk−1+a⋅Δt
其中Pk表示k时刻小车的位置，Pk-1表示k-1时刻小车的位置，Sk表示k时刻小车的速度，Sk-1表示k-1时刻小车的速度， Δ t \Delta t Δt表示时间间隔。对于上式我们可以将其写成向量的形式，如下：
[ P k S k ] = [ 1 Δ t 0 1 ] [ P k − 1 S k − 1 ] + [ 1 2 Δ t 2 Δ t ] ⋅ a \left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right] =\left[\begin{array}{cc} 1 & {\Delta t} \\ 0 & 1 \end{array}\right]\left[ \begin{array}{l} {P_{k - 1}}\\ {S_{k - 1}} \end{array} \right] + \left[ \begin{array}{l} \frac{1}{2}\Delta {t^2}\\ \Delta t \end{array} \right] \cdot a [PkSk]=[10Δt1][Pk−1Sk−1]+[21Δt2Δt]⋅a
我们令 X k = [ P k S k ] {X_k} = \left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right] Xk=[PkSk]，则 X k − 1 = [ P k − 1 S k − 1 ] {X_{k - 1}} = \left[ \begin{array}{l} {P_{k - 1}}\\ {S_{k - 1}} \end{array} \right] Xk−1=[Pk−1Sk−1]。令 A = [ 1 Δ t 0 1 ] A = \left[\begin{array}{cc} 1 & {\Delta t} \\ 0 & 1 \end{array}\right] A=[10Δt1]、 B = [ 1 2 Δ t 2 Δ t ] B = \left[ \begin{array}{l} \frac{1}{2}\Delta {t^2}\\ \Delta t \end{array} \right] B=[21Δt2Δt]、 U k − 1 = a {U_{k - 1}} = a Uk−1=a 。Xk称为状态向量，A称为状态转移矩阵，B称为控制矩阵，Uk-1称为状态控制向量。【注意这里的运动为匀加速运动，即a不变，则Uk=Uk-1=a】。则上式可变成如下形式：
X k = A ⋅ X k − 1 + B ⋅ U k − 1 {X_k} = A \cdot {X_{k - 1}} + B \cdot {U_{k - 1}} Xk=A⋅Xk−1+B⋅Uk−1
大家注意，我们上述所述的所有公式都是在理想状态下成立的，但是我们知道理想状态在现实中是不可能存在的，因此我们需要在上述的公式中都加入噪声，则变换之后的公式如下：【注意这里我们认为噪声都是满足正态分布的】
这里再复写一遍我们得到的带噪声的公式： X k = A ⋅ X k − 1 + B ⋅ U k − 1 + W k − 1 {X_k} = A \cdot {X_{k - 1}} + B \cdot {U_{k - 1}} + {W_{k - 1}} Xk=A⋅Xk−1+B⋅Uk−1+Wk−1。上式因为带有噪声Wk-1，是未知量，因此我们无法对其进行建模，故在我们的估计模型中是不能加入Wk-1的，但去掉Wk-1后模型必然是不准确的，于是我们用 X ^ k {{\hat X}_k} X^k代表原来的 X k {X_k} Xk，加了^表示是估计值，这样我们建立的模型如下：
这样我们就得到了一个我们估计的数学模型了： X ^ k − = A ⋅ X ^ k − 1 + B ⋅ U k − 1 {{\hat X}_k}^ - = A \cdot {{\hat X}_{k - 1}} + B \cdot {U_{k - 1}} X^k−=A⋅X^k−1+B⋅Uk−1
上文已经得到了我们的估计模型，还是之前那辆小车，现天上有个小卫星可以测量小车的速度，如下图所示：
因为卫星可以直接获取小车的位置和速度【哈哈哈不知道卫星能不能直接获取速度哈，这个影响不大不用在意】，所以有以下方程：
{ Z p , k = P k Z s , k = S k \left\{ \begin{array}{l} {Z_{p,k}} = {P_k}\\ {Z_{s,k}} = {S_k} \end{array} \right. {Zp,k=PkZs,k=Sk
改写成向量形式，如下：
[ Z p , k Z s , k ] = [ 1 0 0 1 ] [ P k S k ] \left[ \begin{array}{l} {Z_{p,k}}\\ {Z_{s,k}} \end{array} \right] = \left[ {\begin{array}{cc} 1&0\\ 0&1 \end{array}} \right]\left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right] [Zp,kZs,k]=[1001][PkSk]
令 Z k = [ Z p , k Z s , k ] {Z_k} = \left[ \begin{array}{l} {Z_{p,k}}\\ {Z_{s,k}} \end{array} \right] Zk=[Zp,kZs,k]、 H = [ 1 0 0 1 ] H = \left[ {\begin{array}{cc} 1&0\\ 0&1 \end{array}} \right] H=[1001]、 X k = [ P k S k ] {X_k} = \left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right] Xk=[PkSk]。则上式变为： Z k = H ⋅ X k {Z_k} = H \cdot {X_k} Zk=H⋅Xk
同样的，测量也是存在误差的，也认为其满足正态分布，在上述公式中加入误差，结果如下：
这里也复写一遍带噪声的观测方程： Z k = H ⋅ X k + V k {Z_k} = H \cdot {X_k} + {V_k} Zk=H⋅Xk+Vk同样因为其带有未知数Vk，我们无法建模。舍去噪声的观测方程建模结果如下：
这样我们就得到了我们测量的数学模型了： X ^ k = H − 1 Z k {{\hat X}_k} = {H^{ - 1}}{Z_k} X^k=H−1Zk
至此，我们得到了一个估计模型，一个观测模型，这时候我们就可以使用上文引导小例和融合小例中的结论，将估计值和观测值进行融合，其结果如下：
我们可以对上式进行一个简单的分析：
- 当Kk=0时， X ^ k = X ^ k − {{\hat X}_k} = {{\hat X}_k}^ - X^k=X^k−，即后验估计值=先验估计。
- 当Kk=H-1时， X ^ k = H − 1 Z k {{\hat X}_k} = {H^{ - 1}}{Z_k} X^k=H−1Zk，即后验估计值=测量值。
得到了 X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) {{\hat X}_k} = {{\hat X}_k}^ - + {K_k}({Z_k} - H{{\hat X}_k}^ - ) X^k=X^k−+Kk(Zk−HX^k−)这个式子，Kk是未知的，我们可以用融合小例中的方法求Kk，即找出使 X ^ k {{\hat X}_k} X^k的方差最小的Kk，则Kk即为所求。这里我就不进行推导了，大家感兴趣可以观看此视频。最后我们得到了Kk的表达式： K k = P t − H T ( H P t − H T + R ) − 1 {K_k} = {P_t}^ - {H^T}{(H{P_t}^ - {H^T} + R)^{ - 1}} Kk=Pt−HT(HPt−HT+R)−1其中上式中的 Pt-为t时刻过程噪声协方差的先验值，其等于: P t − = A P t − 1 A T + Q {P_t}^ - = A{P_{t - 1}}{A^T} + Q Pt−=APt−1AT+Q其中Q为测量噪声的协方差。Pt-为先验值，后续需更新，更新后为 P t {P_t} Pt， P t = ( 1 − K k H ) P t − {P_t} = (1 - {K_k}H){P_t}^ - Pt=(1−KkH)Pt−
卡尔曼滤波最重要的就是五个公式，两个预测和三个更新，这些我在前文其实已经全部提到，现做一些整理，如下图所示：
至此，卡尔曼公式就介绍完啦，希望大家都有所收获
卡尔曼公式应用举例
这一部分其实网上的资料特别多，我之前的文章中也做过一个小例子，这里还没想好再补充什么例子，后续有时间再更新应用部分叭
2022.04.08更新
在网络上看到了一个非常有意思物体轨迹预测的小实验，当然是利用卡尔曼滤波实现的详情点击传送门进行了解。
结语
这篇文章的确花费了很多时间，公式什么的太多了，但也收获很大，希望可以让大家对卡尔曼滤波有一个较为清晰的认识，当然文中确实有一些表述不是特别清晰的地方，深感抱歉可以结合其它文章进行了解。
初步计划后续会更新扩展卡尔曼滤波和无际卡尔曼滤波，冲冲冲
参考链接
DR_CAN：https://www.bilibili.com/video/BV1hC4y1b7K7/?spm_id_from=333.788
421施公队：https://www.bilibili.com/video/BV1Rh41117MT?spm_id_from=333.337.search-card.all.click
如若文章对你有所帮助，那就
咻咻咻咻~~duang~~点个赞呗