GNSS算法进阶（一）- 如何通俗的理解kalman滤波算法

一个简单的小例子

我们要每隔十分钟测量室内的温度，正好我们手中有一个温度计，其测量精度为0.1℃。最简单的做法是每隔十分钟用温度计测一次，然后发布出去，每次发布的温度的精度为0.1℃，可以满足我们的日常需要。

但是随着温度计使用年限过久，其精度有大幅下降，每测量一次的随机误差在1℃，也就是每次测量的结果符合标准差为1℃的正态分布。如果还是将每次温度计的读数直接发布，那可能会对我们的日常生活带来影响，因为发布的温度的精度有明显下降。

方法之一是，如果每次测量时同时有10个温度计，那么对10个结果取平均作为最后结果，那么可以提升最后结果的精度。

如果我们仅有一个温度计，那如何挖掘其他有用信息，然后提升我们的最后的精度呢？

我们知道温度不可能忽然变化，一般十分钟前和现在的温度基本上一样。这是一个很有用的信息，即使我们这个时刻没有温度计的读数，如果我们有十分钟以前的温度信息，那么我们就可以将十分钟以前的温度结果拿过来，直接作为当前温度发布出去。

这样不就不需要温度计了吗？

每次我们都可以用十分钟以前的温度结果，这样一直外推下去不就好了吗？

好像哪里不对。

其实这样一直外推也没问题，只是我们在关注结果同时，需要加上结果的精度信息。一个小时前测的温度32℃，其精度为1℃，由此我外推得到十分钟后的温度为32℃， 但其精度有所变差，变为1.5℃。由此类推，一个小时后我依然发布当前的温度为32℃，只是其精度为6℃。

有问题吗？没有问题。

kalman滤波算法公式

（公式截图来自于： https://blog.csdn.net/zhanghm1995/article/details/80862789 ）

kalman滤波的温度实时计算

接下来就需要将外推值，与当前历元的观测值进行融合，得到最终的温度结果。
简单理一下：

①我们在0时刻测了一个温度为32℃，其误差即为温度计的测量精度，1℃。由于没有历史温度信息，本次发布为，温度：32℃，精度1℃。

②在10分钟这个时刻，温度计读数为33℃，同样测量精度为1℃；但我们有十分钟以前的外推结果，外推温度为32℃，精度为1.5℃（1℃+0.5℃），其中0.5℃为我们外推的精度损失。对于两个温度结果，我们使用加权平均的方式来计算当前实际的温度。

在此，我们引入kalman滤波公式。

在本例子中，状态量X(k)就一个，就是当前时刻的温度。状态转移矩阵A也很简单，就是1. 公式(1)(2)的预测过程已经完成。只是我们上面给出的是标准差，公式需要用方差。

使用公式(3)计算kalman增益，即

Kg = (SQR(1.5))/(SQR(1.5)+SQR(1)) = 0.6923

那么10分钟时刻的最后温度：

X(k|k) = 32 + 0.6923*(33-32) = 32.6923

其相应的方差信息

P(k|k) = (1-0.6923)*SQR(1.5) = 0.6923

对其开根号，即当前时刻最后的温度精度为0.83℃，明显的该精度是优于仅使用温度计结果。

所以，我们发布10分钟时刻的温度和精度为32.69℃和0.83℃。

③在20分钟这个时刻，温度计读数为32℃，同样测量精度为1℃。上一个时刻的温度和精度为32.69℃和0.83℃，同样可以外推到当前历元的温度和精度信息。

利用kalman滤波公式，即可以计算当前历元的温度和精度信息。由此，就可以递推的计算后续的所有时刻的温度。

PVA（位置、速度、加速度）状态转移模型

RTKLIB中rtk算法kalman滤波中有PVA状态转移模型，具体见rtkpos.c->udpos函数。

我在代码中我增加了单点的PVA状态转移函数，逻辑完全一样，只是改了一个名字（pntpos.c->udpos_spp），主要是防止修改对rtk算法影响。

其中状态转移矩阵的代码逻辑如下：

tt即为相邻历元的时间变化量，很容易明白，就是位置、速度、加速度的相关关系，初中就已经掌握的知识。

for (i = 0; i < 6; i++)
{
   F[i + (i + 3) * nx] = tt;
}
for (i = 0; i < 3; i++)
{
   F[i + (i + 6) * nx] = SQR(tt) / 2.0;
}

将状态转移矩阵打印出来，看一下更直观。

另一个需要注意，我们只需要在加速度上加入过程噪声，即放大预测值的方差信息。因为从状态转移矩阵中，我们就可以知道，加速度的精度信息会传递到速度和位置。

当然，你也可以修改任何状态量的方差信息，只要你认为该状态的精度符合你修改要求。不要去修改协方差信息，尤其是对于RTKLIB这种对速度和加速度没有测量更新的模型。

代码下载

在个人公众号后台回复 git 获取。

其他推荐阅读

知乎 or csdn搜索kalman滤波

如何通俗并尽可能详细地解释卡尔曼滤波？ - 知乎

你似乎来到了没有知识存在的荒原 - 知乎

https://blog.csdn.net/zhanghm1995/article/details/80862789

公众号

有时会将代码 或者资源放在个人公众号上，有问题，在公众号后台回复，也回答的比较快一些，欢迎关注 GNSS和自动驾驶

其他相关文章链接汇总

GNSS算法学习系列教程 - 文章列表_梧桐Fighting的博客-CSDN博客