【学习笔记】卡尔曼滤波中的协方差矩阵

本文转载至：

添加一些自己的理解和标注的重点，算是学习笔记吧；

一. 什么是卡尔曼滤波：

首先定义问题： 对于某一个系统，知道当前状态Xt，存在以上两个问题：

1. 经过时间△t后，下一个状态如何求出？

2. 假定已求出，在t+1时刻收到传感器的非直接信息，如何对状态进行更正？

这两个问题正式卡尔曼滤波要解决的问题，形式化两个问题如下：

１．预测未来

２．修正当下

下面以一个一维的例子简单理解一下卡尔曼滤波的理论框架。

二. 卡尔曼滤波的理论框架：

以一维运动为例，假如有一个小车，开始位于x=  的位置，但是由于误差的存在，其真实分布是高斯分布，其方差是 ，即其原始位置分布是 ，当该小车经过运动，到达终点位置，但是由于运动也是不准确的（打滑等），其移动过程的分布也是高斯分布，移动分布为，那么其最终的位置分布是多少呢？

这是一个预测未来的过程，由前一时刻的位置和猜测的偏移，求新的位置，求预测位置符合全概率法则，即：

即，最终分布的均值为均值相加，方差也为方差相加，感性理解就是一个不确定的分布，经过一段不确定的移动后，其方差更大了，分布中心为两个中心和。

---

考虑另外一种情况，假入有一个小车，开始位于x= 的位置，但是由于误差的存在，其真实分布是高斯分布，其方差是 ，即其原始位置分布是，当时此时有一个传感器检测到该小车位于，分布方差为，那么小车的真实位置分布为多少呢？

这是一个感知过程，也就是通过传感器的数据修正当下，其感知过程符合贝叶斯法则，其最终分布是两个分布相乘，即

感性理解就是一个不确定位置的小车，经过传感器观测，其最终位置分布方差会更小，且位置中心位于两个分布之间。

在一维模式下，感知过程融合后的均值和方差分别为：                       

我们令                                                                                 

则，

---

总结：当一个位置小车经过移动后，且其定位和移动过程都是高斯分布时，其最终估计位置分布会更分散，即更不准确；当一个小车经过传感器观测定位，且其定位和观测都是高斯分布时，其观测后的位置分布会更集中，即更准确。

又总结：上面的两个例子总结了在一维状态下，预测过程和感知过程的均值和协方差的更新是如何更新的，也就是和，的关系，以及 之间的关系；下面的博客更清晰地讲解了预测过程和感知过程.

接下来是扩展到二维状态下的一个例子：

问题场景如下：一个机器人, 我们想知道它实时的状态, 同时也想做到预测未来和修正当下这两件事.

其状态x表示为一个一维的大小为2的向量, 元素分别表示位置信息与速度信息:

但是这个状态不一定准确, 其不确定性用协方差表示:

预测未来

只考虑机器人本身的运动, 根据物理公式,可得:

用矩阵表示如下:

在状态变化的过程中引入了新的不确定性, 根据协方差的乘积公式,

可得:

其中是系统在k-1时刻状态的不确定性;

考虑外部状态, 以引入加速度为例, 引入变量, 

同时,环境中仍然存在我们无法刻画的误差, 以表示, 最终的预测公式如下:

也就是在原本的不确定性的基础上,又加入了新的不确定性,得到了新的当前时刻状态变量的不确定度,也就是协方差;

修正当下

我们已经得到,, 下面要通过观测到的测量值  对 ,  进行更新;

因为  ,  和 的数据尺度不一定相同,例如 ,包含了笛卡尔的坐标信息, 使用雷达得到的则包含极坐标信息. 所以首先应该把两者挡在相同的尺度下去比较, 尺度转换使用 将预测信息转换为测量信息的尺度.

这样一来,得到测量尺度上的两个分布:

1. 测量值的分布

2. 预测值的分布

在修正当下的过程中,这两个分布会组成新的分布.

新的均值和方差的计算在第一部分的时候已经推导过, (姑且称之为感知变化公式)，即为:

扩展到二维情况下，公式如下(该公式也是kalman滤波的核心，)：

而在这里这种情况里,测量值对应的均值和协方差分别是,

预测未来的到的状态的均值和协方差分别是:　

根据感知变化公式，可得在这个例子里面新的均值和协方差的公式如下：Ｋ变成, 就获得了新的状态量的值，以及其对应的不确定性

总结

预测未来

修正当下

理论总结：

kalman滤波的理论框架是全概率法则和贝叶斯法则，在设定中假设预测和感知均有误差，且均服从正态分布，且预测过程(预测未来)和感知过程(修正当下)采用不同的概率更新策略，具体采取的策略如下所示：

• 预测过程符合全概率法则，是卷积过程，即采用概率分布相加；
• 感知过程符合贝叶斯法则，是乘积过程，即采用概率分布相乘；

参考：(建议按照顺序阅读下面的博客，对于理解卡尔曼滤波很重要)

史上zui好的卡尔曼滤波基础讲解

史上zui全的卡尔曼滤波讲解：从一到三

上面的博文是参考了大佬们的博客，总结在一起的，前面一维的公式推导都很好理解，只是二维那边省略了一些公式推导过程，下面是我的推导过程，比较详细，最后的结果稍微有一点不一样，大家可以参考一下，有问题的话也欢迎提出来告诉我～～

我最后的推导结果中，和上面彩色的公式区别如下:

1. K和上面彩色的多一个

2. 也就是 跟彩色的 也差左乘一个

3.  也就是 与上面彩色的 也是差一个左乘一个，右乘一个