从零开始学习自动驾驶系统(六)-Error State卡尔曼滤波

1.Error State EKF

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

其中

是True State，是Nominal State，是Error State。

ES-EKF直接估计Error State,然后用它矫正Nominal State。在整个滤波过程中，我们实际上修正的变量是,这点一定要清楚!!

Predection

Measurement Update

2.Error State Extended Kalman Filter的执行流程

1、Update Nominal State With Motion Model

注意，公式中的是当前能获取的最优的State的估计值。可能是前一次Prediction产生的State值(连续多次使用Motion Model)，也可能是Measurement Update后State值。

2、Propagete Uncertainty

同样的，在更新不确定性的过程中，需要根据所使用的state不同使用不同的, Prediction阶段产生的State使用Motion Model的,Measurement Update阶段产生的State使用测量阶段的.

在接收到其它传感器的测量结果进行Measurement Update之前，步骤1)和步骤2)可以不断循环执行。

3、If A Measurement Available

3.1 Compute Kalman Gain

3.2 Computer Error State

3.3 Correct Nominal State

3. Error State Extended Kalman Filter的典型应用场景

在自动驾驶系统，主流的定位方案往往采用多传感器融合的定位方案,其中IMU(惯性测量单元)由于无须依赖外部信号,并且具有更高的更新频率,因此成为自动驾驶的标配传感器.

图片来源:https://www.sohu.com/a/230577278_455835

IMU的航迹推演(dead-reckoning)随着时间的推移,误差不断累积,为了避免定位位置出现偏移,需要每隔一段时间,都需要将IMU信息与GPS测量信息或者视觉定位信息进行融合.ES-EKF就是融合这些多传感器信息的有效方法之一.

4. Error State Extended Kalman Filter的优势

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.

参考链接

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