卡尔曼滤波 —— 各路大神和自己的总结

426
0
2020年8月7日 09时25分

卡尔曼滤波 —— 各路大神和自己的总结插图

 

首先分成两个Part吧:直观公式。

 

预备知识

 

1. 协方差矩阵是对称矩阵,对角线上是各维度本身的方差,其他是不同维度间的协方差;

 

2. 贝叶斯公式:卡尔曼滤波 —— 各路大神和自己的总结插图(1)

3. 多维高斯分布:卡尔曼滤波 —— 各路大神和自己的总结插图(2)

 

4. 期望和协方差矩阵: 在[公式] 时,[公式]

卡尔曼滤波 —— 各路大神和自己的总结插图(5)


 

直观部分

 

先来说一说直观的。借鉴了这篇的图文:https://courses.engr.illinois.edu/ece420/sp2017/UnderstandingKalmanFilter.pdf

 

对一个小车的状态进行估计,假设无外力作用,即保持初速度持续不断的运动。假设马尔科夫性,即 [公式] 只与 [公式] 有关,给出一个递推方程 [公式] 。理论上可以通过这个方程求出任意时刻的状态,但是事实上会受到不确定因素的影响。

 

我们假设每个状态分量受到的不确定因素(噪声)服从正态分布,那么小车的状态分量也就服从正态分布,现在对小车进行状态估计。

 

卡尔曼滤波 —— 各路大神和自己的总结插图(9)

 

上图是t=0时小车的位置服从的状态分布。

 

卡尔曼滤波 —— 各路大神和自己的总结插图(10)

 

通过公式预测出小车在t=1时刻位置的正态分布(如上图)。

 

因为加了噪声,所以不确定度增加了,分布也变宽了。这时为了避免不确定度带来的偏差,用传感器测量一次小车的位置,如下图深色分布:

 

卡尔曼滤波 —— 各路大神和自己的总结插图(11)

 

当然,传感器测量同样有不确定因素的干扰,这时,你该相信哪个呢?传感器也可能出错,估计方程也有偏差,这里就需要加权,卡尔曼滤波的K,也就是卡尔曼增益就是这个权重。这里捎带着提一下 [公式] 后面会在公式部分进行推导,这里的 [公式] 就是方程推算是一个后验概率, [公式] 和 [公式] 都是传感器观测方程的参数。

 

卡尔曼滤波 —— 各路大神和自己的总结插图(16)

 

加权后的结果就是图中绿色部分了。

 


公式推导部分

 

这里我使用的是视觉SLAM十四讲中的最大后验概率推导,可能比书上看的明朗些,因为高博喜欢不写中间过程或者不给出之前从哪得到过的结论,直接拿来用。

 

声明: [公式] 是后验概率, [公式] 是先验概率

 

状态估计和观测模型的方程:① [公式]

 

可以看出是线性方程。

 

[公式] 先验与 [公式] 的方程:② [公式] , [公式] 的均值是0,故不加在 [公式] 上了。

 

要求算的方程: [公式]

 

意义:后验是由先验信息 [公式] 和其他信息 [公式] 综合得出的

 

等式右侧都符合高斯分布,为啥子?因为噪声都是高斯分布的且是以0为均值,加上估计值当然是以估计值为均值的高斯分布了。

 

那么我们可以这样写:③ [公式] ,在②式中给出了 [公式] 的均值、协方差矩阵的求法。

 

随后我们对③式中高斯分布的二次项进行计算:

 

高斯分布公式:(看到exp后面的了吧)[公式]

 

推导:

 

卡尔曼滤波 —— 各路大神和自己的总结插图(28)

 

①这个式子后面我们会用到!

 

②步将 [公式] 这就是卡尔曼增益

 

这样就有了 [公式] ,可以注意到这里的卡尔曼增益是用了 [公式] 算出来的,感觉有点转圈圈,就是要求的东西放到求解的式子里了好像没有移项。

 

这里我们使用刚才推导给出的①式对 [公式] 继续推:

就有了:

卡尔曼滤波 —— 各路大神和自己的总结插图(31)

 

根据SMW(Sherman-Morrison-Woodbury)恒等式:

 

卡尔曼滤波 —— 各路大神和自己的总结插图(32)

 

上述的最后一个恒等式,可得: [公式] 。

 

好了现在要求的后验概率已经求出了协方差,还有均值要求解,对③式中高斯分布的一次项进行计算:

卡尔曼滤波 —— 各路大神和自己的总结插图(34)

 

其中式①是将等号左右均求转置,由于协方差矩阵是对称矩阵,所以不变。式②使用了 [公式] 和 [公式] 。最终得到了 [公式] 即如何递归的求解均值。

 

到这里公式部分就结束了。

 

发表评论

后才能评论