传感器融合：基于ESKF的IMU+GPS数据融合

Overview

欢迎访问 持续更新： https://spatial-ai.net/sensorfusion-imu-gnss.html

• 代码：imu_gnss_fusion

System State Vector

the nominal-state $\mathbf{x}$ and the error-state $\delta \mathbf{x}$

$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{b}}_a\\ {\mathbf{b}}_g\end{array}\right]\in {\mathbb{R}}^{16\times 1}\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_g\end{array}\right]\in {\mathbb{R}}^{15\times 1}$$

the true-state

$${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \hat{\delta }\mathbf{x}$$

ENU Coordinate

• using ENU coordinate
• in ENU coordinate, $\mathbf{g}={\left[\begin{array}{ccc}0& 0& -9.81007\end{array}\right]}^T$
• extrinsic between IMU and GPS: ${{}^I\mathbf{p}}_{Gps}$

State Prediction (IMU-driven system kinematics in discrete time)

The nominal state kinematics

p ← p + v Δ t + 1 2 ( R ( a m − a b ) + g ) Δ t 2 v ← v + ( R ( a m − a b ) + g ) Δ t q ← q ⊗ q { ( ω m − ω b ) Δ t } a b ← a b ω b ← ω b \begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t^{2} \\ \mathbf{v} \leftarrow \mathbf{v}+\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \leftarrow \boldsymbol{\omega}_{b} \end{array} p←p+vΔt+21​(R(am​−ab​)+g)Δt2v←v+(R(am​−ab​)+g)Δtq←q⊗q{(ωm​−ωb​)Δt}ab​←ab​ωb​←ωb​​

The error-state kinematics

δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b + δ g ) Δ t + v i δ θ ← R ⊤ { ( ω m − ω b ) Δ t } δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i δ g ← δ g \begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \\ \delta \mathbf{g} & \leftarrow \delta \mathbf{g} \end{aligned} δpδvδθδab​δωb​δg​←δp+δvΔt←δv+(−R[am​−ab​]×​δθ−Rδab​+δg)Δt+vi​←R⊤{(ωm​−ωb​)Δt}δθ−δωb​Δt+θi​←δab​+ai​←δωb​+ωi​←δg​

where

$$\begin{array}{ll}{\mathbf{V}}_{\mathbf{i}}={\sigma }_{{\tilde{\mathbf{a}}}_n}^2\Delta t^2\mathbf{I}& \left[m^2/s^2\right]\\ {\Theta }_{\mathbf{i}}={\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2\Delta t^2\mathbf{I}& \left[ra{d}^2\right]\\ {\mathbf{A}}_{\mathbf{i}}={\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& \left[m^2/s^4\right]\\ {\mathbf{\Omega }}_{\mathbf{i}}={\sigma }_{{\mathbf{\omega }}_w}^2\Delta t\mathbf{I}& \left[ra{d}^2/s^2\right]\end{array}$$

The error-state Jacobian and perturbation matrices

The error-state system is now

$$\delta \mathbf{x}\leftarrow f\left(\mathbf{x},\delta \mathbf{x},{\mathbf{u}}_m,\mathbf{i}\right)={\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \delta \mathbf{x}+{\mathbf{F}}_{\mathbf{i}}\cdot \mathbf{i}$$

The ESKF prediction equations are written:

$$\begin{array}{l}\widehat{\delta \mathbf{x}}\leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \widehat{\delta \mathbf{x}}\\ \mathbf{P}\leftarrow {\mathbf{F}}_{\mathbf{x}}\mathbf{P}{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{i}}{\mathbf{Q}}_{\mathbf{i}}{\mathbf{F}}_{\mathbf{i}}^{\top }\end{array}$$

δ x ∼ N { δ ^ x , P } \delta \mathbf{x} \sim \mathcal{N}\{\hat{\delta} \mathbf{x}, \mathbf{P}\} δx∼N{δ^x,P}

where

F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 0 0 R ⊤ { ( ω m − ω b ) Δ t } 0 I Δ t 0 0 0 I 0 0 0 0 0 I ] \mathbf{F}_{\mathbf{x}} = \left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}} = \left[\begin{array}{ccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 \\ 0 & 0 & \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} & 0 & \mathbf{I} \Delta t \\ 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right] Fx​=∂δx∂f​∣∣∣∣​x,um​​=⎣⎢⎢⎢⎢⎡​I0000​IΔtI000​0−R[am​−ab​]×​ΔtR⊤{(ωm​−ωb​)Δt}00​0−RΔt0I0​00IΔt0I​⎦⎥⎥⎥⎥⎤​

$${\mathbf{F}}_{\mathbf{i}}={\frac{\partial f}{\partial \mathbf{i}}|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{llll}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]$$

$${\mathbf{Q}}_{\mathbf{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\Omega }_{\mathbf{i}}\end{array}\right]$$

ESKF Measurement Update (Fusing IMU with GPS data)

GPS measurement

y = h ( x t ) + v , v ∼ N { 0 , V } \mathbf{y}=h\left(\mathbf{x}_{t}\right)+v , \quad v \sim \mathcal{N}\{0, \mathbf{V}\} y=h(xt​)+v,v∼N{0,V}

convert the GPS raw data which is in WGS84 coordinate to ENU by GeographicLib

$$h\left({\hat{\mathbf{x}}}_t\right)={}^G{\mathbf{p}}_{Gps}={}^G{\mathbf{p}}_I+{}_I^G\mathbf{R}\cdot {}^I{\mathbf{p}}_{Gps}$$

the filter correction equations

$$\begin{array}{l}\mathbf{K}=\mathbf{P}{\mathbf{H}}^{\top }{\left(\mathbf{H}\mathbf{P}{\mathbf{H}}^{\top }+\mathbf{V}\right)}^{-1}\\ \hat{\delta }\mathbf{x}\leftarrow \mathbf{K}\left(\mathbf{y}-h\left({\hat{\mathbf{x}}}_t\right)\right)\\ \mathbf{P}\leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}\end{array}$$

where

$$\mathbf{H}{\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}=\left[\begin{array}{lllcc}\mathbf{I}& 0& -{}_I^G\mathbf{R}\lfloor {{}^I\mathbf{p}}_{Gps}{\rfloor }_{\times }& 0& 0\end{array}\right]$$

the best true-state estimate

$${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \hat{\delta }\mathbf{x}$$

Results

path on ROS rviz and plot the result path(fusion_gps.csv & fusion_state.csv) on google map:

Reference

• Quaternion kinematics for the error-state Kalman filter