论文名字《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中就是