IMU惯性导航-编码器滤波融合-应用SLAM中的全流程

前言： 这里将从IMU是如何在EKF中使用为主流程然后去逐步讲解里面的公式推导，即从应用出发返回去理解公式

总的来说就是要用卡尔曼滤波就要去得出运动方程和观测方程才能往下做，下面的操作都是为了算出运动方程和观测方程，运动方程一般都是指预测值，通过运动方程预测出状态量

扩展卡尔曼滤波EKF公式

这里直接贴出EKF的经典五个公式，因为应用的流程都是基于这个五个公式来进行的，详细推导可以自行去《概率机器人》这本书里面看

迭代扩展卡尔曼滤波IEKF

这个IEKF的公式推导其实和上面的EKF是完全一样

1. 为什么要用迭代卡尔曼滤波
   由于EKF是对非线性模型做了线性近似化，当非线性程度越强时，误差就会越大，但是由于线性化的工作点离真值越近，线性化的误差就越小，所以通过迭代的方法逐渐找到准确的线性化点，从而提高精度
2. 与EKF的公式有哪些地方有区别
   公式流程推导保持不变，仅改变观测的线性化工作点

• 改变后的观测方程
  

• EKF中的运动方程和观测方程
  
  对比来看其实就是把 $\check{y},\check{x}$ 换成了 $y_{op,k},x_{op,k}$
  
  按照之前的方式进行推导，可以得到滤波的矫正方程为
  
  这个公式与EKF的公式唯一的区别就是 ${\hat{x}}_k$ 更新公式与EKF的更新公式不同
  滤波的过程中反复执行这两个公式，以上次的后验均值作为本次的线性化工作点，即可达到减小非线性误差的目的

1. 为什么只执行这两个公式
   因为 $G_k$ 与当前时刻相关，当 ${\hat{x}}_k$ 发生改变时 $G_k$ 也会发生改变，从而 $K_k$ 也要更新，但是其它公式与当前时刻 $k$ 的状态量无关，所以不用迭代更新它们
2. 为什么 G k G_{k}