自动驾驶定位系统-Error State Extend Kalman Filter

1. Error State EKF

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

$$ x = \hat{x} + \delta x $$

其中$x$是True State，$\hat{x}$是Nominal State，$\delta x$是Error State。

ES-EKF直接估计Error State,而后用它矫正Nominal State。在整个滤波过程当中，咱们实际上修正的变量是$\delta x$,这点必定要清楚!!

Predection

$$ \begin{aligned} \underbrace{x_{k} - f(\hat{x}_{k-1}, u_k, 0)}_{\delta k} = F_k(\underbrace{x_{k-1} - \hat{x}_{k-1}}_{\delta_{k-1}}) + L_kw_{k} \\ \end{aligned} $$

$$ \Downarrow $$

$$ \delta_k=F_k\delta_{k-1} + L_k w_k $$

Measurement Update

$$ \begin{aligned} y_{k} = & h( \check{x}_k, 0) + H_k (\underbrace{x_k - \check{x}_k}_{\delta x_k}) + M_k v_k \end{aligned} $$

$$ \Downarrow $$

$$ \begin{aligned} y_{k} = & h( \check{x}_k, 0) + H_k {\delta x_k} + M_k v_k \end{aligned} $$

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

一、Update Nominal State With Motion Model

$$ \check{x}_k = f({x}_{k-1}, u_k, 0) $$

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

二、Propagete Uncertainty

$$ \check{P}_k = F_k {P}_{k-1} F_k^T + L_k Q_k L_k^T $$

一样的，在更新不肯定性的过程当中，须要根据所使用的state不一样使用不一样的$P_{k-1}$, Prediction阶段产生的State使用Motion Model的$P_{k-1}$,Measurement Update阶段产生的State使用测量阶段的$P_{k-1}$.

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

三、If A Measurement Available

3.1 Compute Kalman Gain

$$ K_k = \check{P}_k H_k^T(H_k \check{P}_kH_k^T + M_k R_k M_k^T)^{-1} $$

3.2 Computer Error State

$$ \delta\hat{x}_k = K_k (y_k - h_k(\check{x}_k,0)) $$

$$ \hat{P}_k = (1 - K_k H_k) \check{P}_k $$

3.3 Correct Nominal State

$$ \hat{x}_k = \check{x}_k + \delta \hat{x}_{k} $$

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

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

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.

相关文章

自动驾驶定位系统-扩展卡尔曼滤波Extend Kalman Filter

自动驾驶定位系统-卡尔曼滤波Kalman Filter

自动驾驶系统定位与状态估计- Recursive Least Squares Estimation

自动驾驶系统定位与状态估计- Weighted Least Square Method

自动驾驶定位系统-State Estimation & Localization

自动驾驶定位算法-直方图滤波定位

---

我的网站地址: http://www.banbeichadexiaojiu...