一个简单的小例子
我们要每隔十分钟测量室内的温度，正好我们手中有一个温度计，其测量精度为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和自动驾驶