自動駕駛定位系統-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...