通俗理解kalman filter原理

时间:2021-05-03 20:58:01

  【哲学思想】即使我们对真相(真值)一无所知,我们任然可以通过研究事物规律,历史信息,当前观测而能尽可能靠近真相(真值)。

  【线性预测模型】温度的变化是线性规律的,已知房间温度真值每小时上升1度左右(用协方差R来描述高斯白噪),但具体上升1度多少不得而知。

  【线性观测模型】人用温度计读取房间温度真值时有观测误差,总是读出高出0.3度左右(用协方差Q来描述高斯白噪),但具体高出0.3度多少不得而知。

  【输入】在t时,房间温度真值不知道,最优估值是25度。

  【估值的观测值】小明为了方便直接忽略了预测噪声,估计t+1时刻的温度为26度(不准确的,因为没有考虑到预测噪声,并且基于的值也不是真值)。但是小明又幻想,真值为26度的房间,温度计读出的温度是要高0.3度左右的,由于小明不知道观测噪声具体是多少,所以就直接使用0.3度,于是小明预测出了t+1时刻估值的观测值为26.3度。

  【真值的观测值】t+1时刻到了,小明拿出温度计测量房间的温度,已知观测值总是比真值多0.3度左右,因为有观测噪声Q,所以我们任然无法得知真值。但我们确实得到了真值的观测值,小明读出温度计的温度为26.8度。

  【动态信息融合】这时小明为难了,自己预测的是26.3度,观测到的是26.8度。【创新1】两个都是靠近真值,小明不知道用哪个才好?于是小明决定融合这两个数据得到一个最优估值。【创新2】但是如何决定融合的程度呢?小明想到了用K和1-K来描述两个数据的权重。最简单的各取0.5的权重,这样简单又方便。【创新3】但是爱思考的小明想衡量K=0.5的合理性,那么用什么来衡量最优估计靠近真值的程度呢?对了,小明想到了均方差,就用这个。那么当K在0-1间变化的时候,最优估值的均方差也会变化,这表明K与均方差之间存在某种约束关联。小明在进一步研究后建立起了一个微分方程。这样小明就求出了最合理的K值,并且取名为kalman gain,当K取kalman gain时,最优估值是xx,对应均方差最小为xx。

  【输出】最终小明宣布,t+1时的最优估值是xx度,该值的可信度(均方差)是xx。

  【解放】小明意识到自己只需要知道上一时刻的最优估值和当前时刻的真值观测值,并且在已知预测和观测模型的前提下,就能计算出当前时刻的最优估值。于是小明动手写了一个计算机程序计算并存储最优估值。虽然小明从脑力劳动中解放出来了,但是观测数据仍然需要小明的手动输入。又过了一段时间小明购买了电子温度计,将其连接到计算机,计算机自动获取当前真值观测值(假设观测误差任然存在),于是小明又从这个体力劳动过程中解放了。

  【应用】提供一种在粗糙的世界中尽可能靠近柏拉图理想世界的可能性。

  【拓展到机器人SLAM】已知机器人与路标上一时刻最优估计位姿,当前传感器测量的机器人与路标位姿,机器人运动模型传感器模型。如何利用这些信息估算出当前时刻最靠近真值的最优位姿?

  【拓展到非线性系统】Tayor展开打通了线性系统与非线性系统。