Nonlinear Filter Design for Pose and IMU Bias Estimation Glauco - PowerPoint PPT Presentation

Nonlinear Filter Design for Pose and IMU Bias Estimation Glauco Garcia Scandaroli, Pascal Morin. Glauco.Scandaroli@inria.fr , Pascal.Morin@inria.fr May 12, 2011.

Introduction • Context: • High quality pose estimation ✦ orientation and position. • Data fusion between different sources... maximizes virtues and minimizes drawbacks of each sensor! • Extreme case of fast dynamics, e.g. MUAVs. • IMU (Inertial Measurement Unit): • High frequency : 50 to 1 ❦ [ ❍③ ] . • Incremental measurements. • angular rate gyroscopes , accelerometers . • Drawbacks: measurement high frequency noise and additive offset . • Recover pose by IMU integration: drifts quickly . • Pose measurements: • Advantages: no drift. • Low frequency : generally from 1 up to 25 [ ❍③ ] . G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 1/10

Nonlinear observer design Orientation and Position and gyro bias dynamics accelerometer bias dynamics ✭ d ✽ d dt R = RS ( ✦ � ✦ b ) ❀ ❃ dt p = v ❀ ❃ ❁ (1) d d dt ✦ b = 0 ✿ dt v = R ( a � a b )+ g ■ ❀ (2) ❃ ❃ ✿ d dt a b = 0 ✿ Measurements y = ( ✦❀ a ❀ R ❀ p ) ✿ (3) Orientation and Position and gyro bias observer accelerometer bias observer ✽ ✭ d dt ❜ R = ❜ d ❃ p = ❜ v + ☛ p ❀ RS ( ✦ � ❜ ✦ b + ☛ R ) ❀ dt ❜ ❃ ❁ (4) d d dt ❜ v = R ( a � ❜ a b )+ g ■ + ☛ v ❀ (5) dt ❜ ✦ b = ☛ ✦ ✿ ❃ ❃ ✿ d a b = ☛ a ✿ dt ❜ G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 2/10

Nonlinear observer design Error definition R � R ❜ ❡ R T ❀ ❡ ✦ b � ✦ b � ❜ ✦ b ❀ p � p � ❜ v � v � ❜ a b � a b � ❜ p ❀ ❡ v ❀ ❡ a b ✿ ❡ • The innovation terms should make ( ❡ R , ❡ ✦ b , ❡ p , ❡ v , ❡ a b )= ( I 3 ,0 ❀ 0 ❀ 0 ❀ 0 ) an asymptotically stable equilibrium point of: Error dynamics ✭ d dt ❡ R = ❡ RS ( � ❜ ✦ b � ❜ R ❡ R ☛ R ) ❀ (6) d dt ❡ ✦ b = � ☛ ✦ ❀ ✽ d ❃ p = ❡ v � ☛ p ❀ dt ❡ ❃ ❁ d (7) dt ❡ v = � R ❡ a b � ☛ v ❀ ❃ ❃ ✿ d a b = � ☛ a ✿ dt ❡ G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 3/10

Nonlinear observer design Orientation and gyro bias estimation • Orientation and gyro bias dynamics is independent from position and accelerometer bias. • Several solutions with semi -global stability... • Passive complementary filter on SO ( 3 ) (Mahony, Hamel & Pflimlin 2008). Lemma Let � P a ( ❡ ✁ ❀ ☛ ✦ = � k 2 ❜ � P a ( ❡ ✁ ❀ ☛ R = k 1 ❜ R T vex R T vex R ) R ) with k 1 ❀ k 2 ❃ 0 . Then, concerning the dynamics (6) : 1) All solutions converge to E s ❬ E u with E s = ( I 3 ❀ 0 ) , and ♥ ♦ ☞ ✁ = � 1 �❡ ( ❡ ☞ tr E u = R ❀ ❡ ✦ b ) R . 2) ( ❡ R ❀ ❡ ✦ b ) = ( I 3 ❀ 0 ) is a locally exponentially stable equilibrium. G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 4/10

Nonlinear observer design Position and accelerometer bias estimation • First: R and ✦ ❇ are available. • Position and acceleration estimation: Theorem Let k 3 S ( ✦ ❇ )) R T ❡ p ❀ ☛ a = � k 5 ( I 3 + 1 ☛ p = k 3 ❡ p ❀ ☛ v = k 4 ❡ p with k 3 ❀ k 4 ❀ k 5 ❃ 0 such that k 5 ❁ k 3 k 4 . Then, ( ❡ p ❀ ❡ v ❀ ❡ a b ) = ( 0 ❀ 0 ❀ 0 ) is a globally exponentially stable equilibrium point of the position estimation error dynamics (7) . • This theorem yields a globally asymptotically stable estimator . • Stability is achieved for any angular velocity ✦ ❇ . • The initial assumption can be relaxed. G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 5/10

Nonlinear observer design Full pose observer Corollary Let ✭ d dt ❜ R = ❜ RS ( ✦ � ❜ ✦ b + ☛ R ) ❀ d dt ❜ ✦ b = ☛ ✦ ❀ ✽ d ❃ dt ❜ p = ❜ v + ☛ p ❀ ❃ ❁ d v = g ■ + ❜ dt ❜ R ( a � ❜ a b )+ ☛ v ❀ ❃ ❃ ✿ d a b = ☛ a ❀ dt ❜ � P a ( ❡ ✁ ❀ ☛ ✦ = � k 2 ❜ � P a ( ❡ ✁ ❀ with ☛ R = k 1 ❜ R T vex R T vex R ) R ) R T ❡ p ❀ ☛ a = � k 5 ( I 3 + 1 ✦ b )) ❜ ☛ p = k 3 ❡ p ❀ ☛ v = k 4 ❡ k 3 S ( ✦ � ❜ p ✿ Assume that k 1 ❀ ✁✁✁ ❀ k 5 ❃ 0 and k 5 ❁ k 3 k 4 . Then, 1) The origin ( ❡ R ❀ ❡ p ❀ ❡ v ❀ ❡ a b ) = ( I 3 ❀ 0 ❀ 0 ❀ 0 ❀ 0 ) is a locally ✦ b ❀ ❡ exponentially stable equilibrium. 2) If ❡ R converges asymptotically to I 3 , then ( ❡ p ❀ ❡ v ❀ ❡ a b ) converges ✦ b ❀ ❡ asymptotically to zero. G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 6/10

Nonlinear observer design Tuning the innovation gains • How to tune the innovation gains? • Rationale: analyze the error dynamics and each gain’s effect. • Using this procedure, one can define 5 settling times ✜ i ❃ 0, Gain tuning for the nonlinear observers: 1 k 1 = 3 ✜ 1 + ✜ 2 ✜ 1 ✜ 2 ❀ k 2 = 9 ✜ 1 ✜ 2 ❀ 27 k 3 = 3 ✜ 3 ✜ 4 + ✜ 3 ✜ 5 + ✜ 4 ✜ 5 ❀ k 4 = 9 ✜ 3 + ✜ 4 + ✜ 5 ✜ 3 ✜ 4 ✜ 5 ❀ k 5 = ✜ 3 ✜ 4 ✜ 5 ✿ ✜ 3 ✜ 4 ✜ 5 • This definition of k 3 , k 4 , and k 5 satisfies k 5 ❁ k 3 k 4 . G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 7/10

Results Enhancing visual pose estimation using data fusion – Visual estimation at 40 [ ❍③ ] . Pose estimation using (Benhimane & Malis 2007). Visual update at 40 [ ❍③ ] . G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 8/10

Results Enhancing visual pose estimation using data fusion – Visual–IMU fusion at 200 [ ❍③ ] . The same visual pose estimation using the proposed filter. IMU at 200 [ ❍③ ] , visual update at 10 [ ❍③ ] . G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 9/10

Results Enhancing visual pose estimation using data fusion – Visual–IMU fusion at 200 [ ❍③ ] . The same visual pose estimation using the proposed filter. IMU at 200 [ ❍③ ] , visual update at 10 [ ❍③ ] . B C 0.05 1 B ω b B C C 0 � B C B z C −0.05 0.5 B C 0 B C B a b C −0.2 0 � 0.5 −0.4 W 0 1 0 10 20 30 40 50 60 70 0.5 −0.5 t [ s ] 0 y x G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 9/10

Last Remarks Conclusion • Design of nonlinear observers to estimate pose with online calibration of IMU bias. • Semi-global stability is achieved. • Gain tuning method based on errors settling times. Future work • Evaluation of time varying innovation gains , and the use of gain matrices, also relating with measurement and estimate uncertainties . • Extension for coordinate system parameter estimation , e.g. camera-to-IMU orientation and translation. G. G. Scandaroli, P. Morin ICRA – Shanghai, China. May 12, 2011. 10/10

Bibliography Benhimane, S. & Malis, E. (2007). Homography-based 2D visual tracking and servoing, Intl. Journal of Robotics Research 26 : 661–676. Mahony, R., Hamel, T. & Pflimlin, J.-M. (2008). Nonlinear complementary filters on the special orthogonal group, IEEE Trans. on Automatic Control 53 : 1203–1218.