ESKF(error state Kalman filter)及其公式推导

ESKF(error state Kalman filter)介绍

ESKF(error state Kalman filter)是Kalman滤波的一种特殊形式，相比于KF或EKF,UKF等，ESKF有以下优势:

1.error state 总是接近于0，Kalman Filter工作在原点附近, 故远离奇异值、万向节锁，并且保证了线性化的合理性和有效性;
2.error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速;
3.error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）而导致协方差矩阵奇异的风险
4.error state变化平缓(因为较大的error已经集成到norminal state上)，不需要太多了correction;

常用的VIO(visual inertial odeometry)或LIO(lidar inertial odometry)中，通常采用IMU进行预积分，下面讲一下IMU预积分的公式推导。

true state kinematics

对于true state kinematics(动力学), 有:
对于IMU的真实值与测量值之间的关系:
把公式2带入公式1可得:

nominal-state kinematics

相比于true state, norminal state没有噪声和扰动(perturbations), norminal state的公式如下:

error-state kinematics

对于位置来说, 对于速度来说,
注释Rodrigues公式: ,当
时,