Error State Extended Kalman Filter (ES-EKF)

ES-EKF是EKF的一种变种，它的基本思想就是把State区分为两部分:Nominal State和Error State。如下所示:

其中x是True State，  是Nominal State，  是Error State。

ES-EKF 的优点

1.The orientation error-state is minimal (i.e., it has the same number of parameters as degrees of freedom), avoiding issues related to over-parametrization (or redundancy) and the consequent risk of singularity of the involved covariances matrices, resulting typically from enforcing constraints.
2.The error-state system is always operating close to the origin, and therefore far from possible parameter singularities, gimbal lock issues, or the like, providing a guarantee that the linearization validity holds at all times.
3.The error-state is always small, meaning that all second-order products are negligible. This makes the computation of Jacobians very easy and fast. Some Jacobians may even be constant or equal to available state magnitudes.
4.The error dynamics are slow because all the large-signal dynamics have been integrated in the nominal-state. This means that we can apply KF corrections (which are the only means to observe the errors) at a lower rate than the predictions.

题目 (Coursera : State Estimation and Localization for Self-Driving Cars)

通过融合IMU输入，GNSS、Lidar观测来估计真实的轨迹、姿态、速度

  

代码

https://github.com/a-l-a-n/Coursera-State-Estimation-and-Localization-for-Self-Driving-Cars

参考

1. Quaternion kinematics for the error-state Kalman filter
2. Madyastha, Venkatesh, et al. "Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation." AIAA Guidance, Navigation, and Control Conference. 2011.
3. Coursera : State Estimation and Localization for Self-Driving Cars
4. https://zhuanlan.zhihu.com/p/360677963