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

文章目录

• Overview
• System State Vector
• ENU Coordinate
• State Prediction (IMU-driven system kinematics in discrete time)
• The nominal state kinematics
• The error-state Jacobian and perturbation matrices
• ESKF Measurement Update (Fusing IMU with GPS data)
• Results
• Reference

Overview

• 代码：imu_gnss_fusion

System State Vector

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

$\mathbf{x} = \begin{bmatrix} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{b}_a \\ \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{16 \times 1} \quad \delta \mathbf{x} = \begin{bmatrix} \delta \mathbf{p} \\ \delta \mathbf{v} \\ \delta \boldsymbol{\theta} \\ \delta \mathbf{b}_a \\ \delta \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15 \times 1}$x=⎣⎢⎢⎢⎢⎡​pvqba​bg​​⎦⎥⎥⎥⎥⎤​∈R16×1δx=⎣⎢⎢⎢⎢⎡​δpδvδθδba​δbg​​⎦⎥⎥⎥⎥⎤​∈R15×1

the true-state

$\hat{\mathbf{x}}_{t}=\mathbf{x} \oplus \hat{\delta} \mathbf{x}$x^t​=x⊕δ^x

ENU Coordinate

• using ENU coordinate
• in ENU coordinate, $\mathbf{g} = \begin{bmatrix} 0 & 0 & -9.81007 \end{bmatrix}^T$g=[0​0​−9.81007​]T
• extrinsic between IMU and GPS: ${{}^{I} \mathbf{p}}_{G p s}$IpGps​

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

The nominal state kinematics

$\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 Jacobian and perturbation matrices

$\begin{array}{l} \hat{\delta \mathbf{x}} \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\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^←Fx​(x,um​)⋅δx^P←Fx​PFx⊤​+Fi​Qi​Fi⊤​​

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

where

$\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}}= \left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\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]$Fi​=∂i∂f​∣∣∣∣​x,um​​=⎣⎢⎢⎢⎢⎡​0I000​00I00​000I0​0000I​⎦⎥⎥⎥⎥⎤​

$\mathbf{Q}_{\mathbf{i}}= \left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \boldsymbol{\Theta}_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right] , \quad \text{with} \quad \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{\boldsymbol{\omega}}_{n}}^{2} \Delta t^{2} \mathbf{I} & {\left[r a d^{2}\right]} \\ \mathbf{A}_{\mathbf{i}}=\sigma_{\mathbf{a}_{w}}^{2} \Delta t \mathbf{I} & {\left[m^{2} / s^{4}\right]} \\ \boldsymbol{\Omega}_{\mathbf{i}}=\sigma_{\boldsymbol{\omega}_{w}}^{2} \Delta t \mathbf{I} & {\left[r a d^{2} / s^{2}\right]} \end{array}$Qi​=⎣⎢⎢⎡​Vi​000​0Θi​00​00Ai​0​000Ωi​​⎦⎥⎥⎤​,withVi​=σa~n​2​Δt2IΘi​=σω~n​2​Δt2IAi​=σaw​2​ΔtIΩi​=σωw​2​ΔtI​[m2/s2][rad2][m2/s4][rad2/s2]​

ESKF Measurement Update (Fusing IMU with GPS data)

GPS measurement

$\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}_{G p s} = {}^{G} \mathbf{p}_{I} + {}_{I}^{G} \mathbf{R} \cdot {}^{I} \mathbf{p}_{G p s}$h(x^t​)=GpGps​=GpI​+IG​R⋅IpGps​

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}$K=PH⊤(HPH⊤+V)−1δ^x←K(y−h(x^t​))P←(I−KH)P​

where

$\mathbf{H} \left.\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}} = \left[\begin{array}{lll} \mathbf{I} & \mathbf{0} & -{}_{I}^{G} \mathbf{R} \lfloor {{}^{I} \mathbf{p}}_{G p s} \rfloor _{\times} & \mathbf{0} & \mathbf{0} \end{array}\right]$H≜∂δx∂h​∣∣∣∣​x​=[I​0​−IG​R⌊IpGps​⌋×​​0​0​]

the best true-state estimate

$\hat{\mathbf{x}}_{t}=\mathbf{x} \oplus \hat{\delta} \mathbf{x}$x^t​=x⊕δ^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