道不尽的卡尔曼！通俗易懂详细解释卡尔曼滤波
点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达
本文只做学术分享，如有侵权，联系删文
概述
总的来说，卡尔曼滤波器是一个状态估计器，它利用传感器融合、信息融合来提高系统的精度。通常，我们要观测一个系统的状态，有两种手段。一种是通过系统的状态转移方程，并结合上一时刻的状态推得下一时刻的状态。一种是借助辅助系统（量测系统）的测量得到系统状态。这两种方式都有各自的不确定性，卡尔曼滤波可以将这两者做到最优结合（加权平均），使得我们估计的状态的不确定性小于其中任何一种。所以权重的选择至关重要，它意味着我们更信任哪一种方式得出的状态（当然是更加信任不确定性较小的状态）。
建模
比如，我们观测一辆小车的速度和位置（所以我们要观察的状态就是速度 v 和位置 p ，即状态 x=(v,p) ,这里的 p,v 都是向量）,当我们有了 k-1 时刻的状态 =(,) 后，那么如果速度不变，我们就可以得到下一时刻的状态 =(,) ,其中
当然，我们也可以对小车进行控制，比如让小车加速、拐弯等等，这些都可以用一个加速度来表示： ，这样，下一时刻状态 =(,) 的表示变为：
其中：
但是，现实的情况并没有那么理想，小车可能会受到外界的各种扰动，比如，轮胎打滑，地面崎岖等等，都会使得状态转移的过程中混入噪音干扰（过程噪声），而且这个噪音往往是不可被测量的，也就是说，没办法通过建模消除噪音项。不过，往往我们可以认为这个噪音是服从零均值的高斯分布的。那么（1）式重写为
上面的状态转移方程为我们提供了观察系统状态的一种方式，同时，我们可能有一些辅助系统，比如对于这个小车，我们可以用GPS作为得到其状态的另一种方式。
那样的话，GPS每一时刻都可以提供一次当前状态的观测值 ,它与真实状态 的关系为，=+ （3）
其中的 是观测模型，它把系统真实状态空间映射成观测空间， 是噪声项，称为观测噪声——因为任何的测量系统都是有误差的，所以观测值实际上是真实值与噪声的叠加。我们同样可以认为此噪声是服从0均值的高斯分布的。即
现在我们比较这两种方式得到的系统状态，容易想到，状态转移方程得到的系统状态在演变时会非常平滑，而它的不确定度会随着迭代的进行而逐渐增大，因为误差会在一次次迭代的过程中不断累积（具体反映为估计状态的方差越来越大）。相反，由量测系统得到的状态不存在累积误差，但演变时也会很不平滑。这时我们就需要将两者得到的状态有效结合起来。这就是卡尔曼滤波做的事情了。
卡尔曼滤波
然后我们要算出测量残差：
第一项为0，因为 代表k时刻系统状态的真实值，是一个固定值，没有协方差。所以简化为：
补充内容：
另外，直观上理解：协方差矩阵代表的是数据分布的一种不确定度，这种不确定度本身不具有所谓的正负号，不可能因为加减而有所抵消。
交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~