迭代扩展卡尔曼滤波学习

论文名字《The iterated Kalman filter update as a Gauss-Newton method》

引言：

计算资源大，IKF比EKF效果更好。IKF的更新方法采用的是GaussNewton方法，用于近似解。

更新问题

当前状态，当前状态估计（预测），测量值

测量函数：
如EKF

因此，z,x预测，R，P都已知了，现在的问题就是找到一个更好的状态估计（预测）
和协方差

更新

将刚估计出的状态和协方差作为迭代的第一个值

EKF的更新公式为

而IKF的更新公式：

怎么得到的呢？或者说为啥不同捏。这是因为IKF采用的是GN方法（最大似然更新）

最大似然更新

它将当前观测和预测的状态都放到一个大的观测向量里，并得到一个测量函数。？为啥

Z服从的分布，以及它的协方差可以求得

因此问题就变为了用Z,g,Q求解迭代的状态估计（后验）
和协方差
了。
似然函数-Z的概率密度函数：（啊，是正态分布），假设是真值。

最大似然估计就是（将Z-g= Gx代入获得x的协方差）解答了fast-lio中的疑问。

相应的，最大化的L对应最小化的二次项

函数q的最小值，应该在其一阶导为0的地方

总结一下目前的疑惑点：IKF的更新公式；最大似然估计得到的协方差形式。

测量函数g可以看作其在状态估计（后验）处的一阶泰勒展开。

G是雅可比矩阵。
设测量误差为V

代入到13中

求得协方差阵

代入G,Q，得

（我没见过这种形式得卡尔曼增益。从哪来的呦—可以参考fast–lio,一种卡尔曼增益得新形式）

高斯牛顿法

高斯牛顿法更新公式–看一下非线性最小二乘问题

右上标得点代表微分，倒三角
：代表微分得转置

其中第一项，第二项高斯牛顿舍弃了第二项（二阶导）得到一个近似值。
也就是说这玩意应该用约等于≈

因此xi+1就可以写成最小二乘形式

从这里开始将上述高斯牛顿方法与IKF的更新联系起来
首先设


结合公式6，16，17 求逆引理得（就是将G Q Z 都展开）

对应到Fast-lio中就是