也谈卡尔曼滤波（上）

最新推荐文章于 2024-04-02 22:05:15 发布

_小庄

最新推荐文章于 2024-04-02 22:05:15 发布

阅读量3.9k

点赞数 5

分类专栏： math

本文链接：https://blog.csdn.net/junshen1314/article/details/49185815

版权

math 专栏收录该内容

15 篇文章

订阅专栏

关于kalman Filter, 网上有很多大牛总结的很好，结合自己这两天的学习，进行总结：

一、历史

卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。注意这里的测量和估计，是我们应用此算法非常重要的一部分。

二、公式推导

首先给出卡尔曼滤波的 五条公式，即

这五条公式是我们应用卡尔曼滤波解决问题的核心思想。（公式截图自【1】，这是关于卡尔曼滤波器的一篇经典的引用超过6000次的tutorial。约翰霍普金斯大学的一个学生将其翻译成了中文【2】）

看着这一团公式可能会有点眼晕，接下来我们将通过一个例子，一点点解释这些公式的来历和应用（例子、图片公式等来自【1】【3】和译文【4】，讲的都挺好的，非常值得参考）。

如图，我们希望测量一辆小车的位移。目前我们手头能掌握的信息，一个是小车收到的力f和小车的质量m，另一个是放置在路上的一台测距仪器。现在我们开始工作：

根据初中物理知识，假如我们知道小车的受力，我们应当能知道某段时间内小车的位移（这一点牛顿大神已经告诉我们怎么做了，也即：小车运动的模型已知），另外，小车的位移也可以通过测距仪测出来。所以目前，要获得小车位移信息，我们手头有两个量：1.根据运动模型的估计位移，2.用测距仪测出来的位移。按道理我们应该非常自信自己能够获得精确的小车位移，毕竟我们手头有两中测量方式，但问题是，1.运动模型基本是基于理想情况的，完全根据运动模型计算出来的位移，没有考虑噪声影响，一般是不准的。2. 测距仪是传感器，传感器也有测量噪声，完全依赖传感器测出来的位移，也是不准的。所以手头有两个测量位移，并没有什么卵用。最要紧的是我们可以用一种方法把这两种数据结合起来。聪明的同学已经想到了，求平均值，再不行求加权平均，这两种方法其实也是可以的，但再实践过程中，我们会发现单纯这样处理的效果并不是很好，用卡尔曼的话来说，并没有完全利用好两种测量所能给出的全部的信息。

那么卡尔曼是怎么处理这两种信息的呢？

在这里我们首先要对我们刚才的测量和估计，两个过程建一下模。

卡尔曼滤波模型假设这一时刻的状态由上一时刻过渡而来，单纯这么说可能有点费解，x表示状态向量，z表示测量量。A为状态转移矩阵，B为输入转换矩阵，H为测量转换矩阵。w和v分别代表过程和测量噪声。结合我们小车测距的例子：我们知道小车的质量m ,小车受力f,那么由牛顿第二定律，小车的加速度为uf=f/m,我们用x表示小车的位移，x加个点表示小车的速度（导数一般用变量加点表示，公式不好打），那么我们有：

把这两个式子写成向量的形式，有：

对比（1.1），很容易看出A=[1, Delt(t); 0 ,1],B=[(Delta (t))^2 /2 ; Delta(t)]

另外，我们还可以知道：小车的位移测量值等于（佳哥的关于卡尔曼滤波推导和使用的日志，总结的非常好【5】）

对比（1.2）式，我们知道H=[1, 0 ]。

到目前为止，我们已经明确，式（1.1）和式（1.2）就是我们关于预测和测量的模型。既然我们已经建立好模型，那么我们开始处理刚才提出的问题：如何把这两个量联系，或者说有效的融合到一起？

我们希望得到一个这样的形式：

,也即，下一时刻的输出状态，等于这一时刻测量值和预测值的某种组合，格外要注意，这里的加^的xt,代表的是根据先验知识推测出来的这一时刻的值，在我们上面的例子中，他是用式（1.1）算出来的位移值。Z也就是测量值了。当然如前述所说，我们可以给a,b找个值，那么输出状态就是我们手头的测量值和预测值的加权平均了，但是卡尔曼没这么干，他选的a,b是经过了充分的考虑的。

根据最优估计和类似于一种反馈的思想，我们可以推得：

具体推导过程不再详细解释，具体见【11】。这里用^和-表示根据先验知识（例子中先验知识就是我们用的牛顿运动定律）推得的状态。K 叫做卡尔曼增益。这里我们重点介绍一下这个K 如何推导。鉴于佳哥已经总结的非常好【5】，这里直接拿来用了。

重申一下变量命名规则：

，

和

分别表示根据先验知识算出的先验状态（根据牛顿公式算出的速度），引入测量值之后的（混合了测量值和先验值，如式1.7）后验状态，和真实的状态。

定义

为先验估计误差，

位后验估计误差。则先验估计协方差为：

（1）后验估计协方差为：

（2）。这里为什么要提协方差呢？我们知道变量的方差刻画了变量的离散程度，那么协方差——也即多个变量的方差，即刻画了多个变量之间的相关特性。这里使用协方差的目的，是为我们进行最小均方误差估计（LMS-least mean square）做准备。关于最小均方误差估计，或称最优估计内容希望查一下相关资料。总之这里就是希望我们的误差ek能达到最小，在这个设想下，我们希望找到一个合适的增益K的值。推导的思路为：以军方误差公式（1）和（2）为基础，结合公式1.7和公式1.2，推导出协方差表达式，然后对表达式的迹（trace,这个矩阵的迹代表的是各个变量的方差，刻画的是每个变量的离散程度）求导数，求出导数为零的时候的K的值即为最优的估计值。具体的推导过程【5】介绍的非常详细，这里不再赘述。直接给出K的值：