IMU and VO Loose Fusion based on ESKF

1. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA & Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . IMU and VO Loose Fusion based on ESKF Hongchen Gao cggos@outlook.com 2021.07.19 Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 1 / 9

2. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA & Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . System State the nominal-state x =       p v q ba bg       ∈ R16×1 the error-state δx =       δp δv δθ δba δbg       ∈ R15×1 the true-state xt = x ⊕ δx Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 2 / 9

3. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA & Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . State Prediction IMU-driven system kinematics in discrete time The nominal-state kinematics p ← p + v∆t + 1 2 (R (am − ab) + g) ∆t2 v ← v + (R (am − ab) + g) ∆t q ← q ⊗ q {(ωm − ωb) ∆t} ab ← ab ωb ← ωb The error-state kinematics δp ← δp + δv∆t δv ← δv + −R [am − ab]× δθ − Rδab + δg ∆t + vi δθ ← R⊤ {(ωm − ωb) ∆t} δθ − δωb∆t + θi δab ← δab + ai δωb ← δωb + ωi Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 3 / 9

4. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . State Prediction The error-state Jacobian and perturbation matrices The error-state system is δx ← f (x, δx, i) = Fx (x) · δx + Fi · i The ESKF prediction equations are written ˆ δx ← Fx (x) · ˆ δx P ← FxPF⊤ x + FiQiF⊤ i where Fx = ∂f ∂δx

5. x =    I I∆t 0 0 0 0 I −R [am − ab]× ∆t −R∆t 0 0 0 R⊤ {(ωm − ωb) ∆t} 0 −I∆t 0 0 0 I 0 0 0 0 0 I    Fi = ∂f ∂i

6. x =   0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I   Qi = Vi 0 0 0 0 Θi 0 0 0 0 Ai 0 0 0 0 Ωi # with        Vi = σ2 ãn ∆t2I m2/s2 Θi = σ2 ω̃n ∆t2I rad2 Ai = σ2 aw ∆tI m2/s4 Ωi = σ2 ωw ∆tI rad2/s2 Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 4 / 9

7. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Function h(x̂) ←− Tc0cm · Tcb · T−1 b0bm | {z } Tvw ·Tb0bn · T−1 cb =Tvw · T · T−1 cb = RvwRRT cb Rvw(t + Rtbc) + Rc0cm tcb + tc0cm 0 1 Measurement Residual r = z ⊖ h(x̂) = tz − t̂ θz ⊖ θ̂ = tz − t̂ 2 [q̂∗ ⊗ qz]vec ∈ R6 Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 5 / 9

8. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Measurement Update (Fusing IMU with VO data) Measurement Jacobian Matrix Measurement Jacobian Matrix w.r.t Error-State H = ∂h(x) ∂δx

9. x=x̂ = − ∂r ∂δx

10. x=x̂ ht w.r.t δt ∂ht ∂δt = Rvw ht w.r.t δθ ∂ht ∂δθ = ∂RvwRtbc ∂δθ = Rvw · ∂Rtbc ∂δθ = −Rvw · R · t∧ bc hθ w.r.t δθ ∂hθ ∂δθ = ∂θ{RvwRRT cb} ∂δθ = ∂2 qvw ⊗ q ⊗ qT cb vec ∂δθ = 2 [0 I] · ∂qvwqqT cb ∂δθ = 2 [0 I] · ∂qvw ⊗ (q ⊗ δq) ⊗ qT cb ∂δq · ∂δq ∂δθ = 2 [0 I] · ∂L(qvw ⊗ q) · R(qT cb) · δq ∂δq · ∂ h 1 1 2 δθ i ∂δθ = [0 I]3×4 · L(qvw ⊗ q) · R(qT cb) · h 0 I i 4×3 Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 6 / 9

11. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Measurement Update (Fusing IMU with VO data) Filter Correction the Kalman Gain K = PHT (HPHT + R)−1 the error-state δx ←− Kr the covariance matrix P ←− (I − KH)P the best true-state estimation xt = x ⊕ δx −→ p = p̂ + δp v = v̂ + δv R = R̂ · ∆R{δθ} ba = ˆ ba + δba bg = ˆ bg + δbg Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 7 / 9

12. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . QA Challenges 1 Measurement Function 2 Measurement Jacobian Matrix of ESKF 3 Rotation Perturbation in the Formula of Residual and Correction 4 EKF Tuning: R, Q, P Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 8 / 9

13. IMU and VO Fusion based on ESKF Hongchen Gao System State State Prediction IMU-driven system kinematics in discrete time The error-state Jacobian and perturbation matrices Measurement Update (Fusing IMU with VO data) Measurement Models Measurement Jacobian Matrix Filter Correction QA Challenges ThankYou . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ThankYou Thank You! Hongchen Gao IMU and VO Fusion based on ESKF 2021.07.19 9 / 9

IMU and VO Loose Fusion based on ESKF

You are reading a preview.

Check these out next

IMU and VO Loose Fusion based on ESKF

More Related Content

Slideshows for you (20)

Similar to IMU and VO Loose Fusion based on ESKF (20)

Recently uploaded (20)