惯导3-卡尔曼滤波的理解

有关卡尔曼滤波的文章非常多，我在这里就只写写学习卡尔曼滤波器的时候遇到的坑。

什么是滤波器

所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。
所谓估计就是根据测量得出的与状态$X(t)$有关的数据$Z(t)=HX(t)+V(t)$解算出$X(t)$的值。估计的方法有很多，但每个方法都有一个准则。例如最小二乘估计的指标是：使各次量测$Z_i$与由估计$X$确定的量测的估计$Z=HX$之差的平方和最小。
而卡尔曼滤波估计是最小方差估计，

量测方程

看了三天的姿态融合，才明白量测方程，这个坑还挺大的。
看网上的通俗的理解，有一个估计房间温度的例子，需要一个自己的估计值和一个温度计的测量值，两个值都不太准，所以需要卡尔曼滤波综合考虑每个值，最后给出一个最优估计值。
这个例子其实很好，但是总给人一种感觉，就是我想要估计的状态量必须也能测量$Z=X$，也就是我必须能够测量温度，来当做量测值。但这其实是不对的，实际情况下，我要估计的值并不能量测出来，不信你看量测方程$$Z(t)=HX(t)+V(t)$$这里还有个$H$，如果$H$不是单位矩阵，那么$Z$和$X$就不相等。
拿惯导融合做例子：
INS和GNSS融合时，状态量是$\Delta X$，而量测值是$X_I-X_G$，这时你可能就不理解了，其实，真正的量测值是$\Delta X_I-\Delta X_G$，这个很容易推导的，那为什么是$\Delta X_I-\Delta X_G$呢，因为$\Delta X_I-\Delta X_G$，可以用$\Delta X$线性表示，不信可以看$H$中的具体值是什么。到这里如果还想继续深入理解，就只能看量测方程的推导了。

状态量

又遇到了两个坑，都是状态量的理解，主要还是大$F$矩阵没有理解，导致的理解偏差。
状态量有15个，包含：速度偏差、位置偏差、姿态角偏差、陀螺仪偏差、加速度计偏差。

1. 速度和位置偏差。读代码的时候发现，每次调用卡尔曼滤波，速度和位置偏差的初值都设为0，表示完全不理解，不应该是用上一次的数据吗？
   原来，每次计算完误差，都会去校正，所以就认为：校正完之后，位置和速度是准确的，既然是准确的，那么位置和速度的偏差当然是0啦。
2. 姿态角、陀螺仪、加速度计的偏差都是单次的，所以计算误差要累加。

参考

1. 卡尔曼滤波器介绍-Greg Welch， Gary Bishop