with Application to Autonomous Vehicles

Nonlinear with Application to Autonomous Vehicles (Ph.D. Candidate) C. Silvestre (Supervisor) P. Oliveira (Co-supervisor) Institute for s and Robotics Instituto Superior Técnico Portugal January 2010

Presentation outline Nonlinear and 1. Thesis outline Main contributions 2. Kalman Filter s Embedded UAV model for INS aiding Nonlinear Complementary Kalman filter 3. Combination of and for observer navigation system 4. and Future Work

Problem statement problem Estimate attitude and of a rigid body with respect to a reference frame, using available sensor data. and criteria Stability: the state estimate converges to the real and attitude. Performance: the state estimate is obtained by exploiting the sensor readings with optimality criteria.

Thesis outline EKF/INS + Advanced Vector Aiding IMU + Landmark Detection and EKF/INS + Vehicle Model Aiding Kalman Filtering Complementary Kalman Filtering IMU + Vector Observations Nonlinear Observers The proposed navigation systems are derived using two methodologies Kalman filtering techniques; and stability theory. IMU + GPS New Stability Results using Density Functions

Main contributions: Kalman filtering Kalman Filtering (A) EKF/INS + Advanced Vector Aiding (A) EKF/INS + Vehicle Model Aiding (B) Complementary Kalman Filtering and Main contributions in the field of navigation systems based on Kalman filtering (A) Advanced aiding techniques for EKF/INS navigation systems - modelling frequency contents of vector readings, - integrating efficiently the dynamic of the vehicle. (B) Nonlinear complementary Kalman filters - endowed with stability and performance properties, - combining stochastic approach and frequency domain design.

Main contributions: Nonlinear Nonlinear Observers and (A) IMU + Landmark Detection (A) IMU + Vector Observations (A) IMU + GPS (B) New Stability Results using Density Functions Main contributions in the field of (A) Nonlinear attitude and for inertial systems - with diverse aiding sensors. - endowed with stability properties for worst-case errors. (B) New tools - based on the combination of density and functions, - for almost GAS and almost ISS analysis of systems. - that eventually yield almost ISS of the proposed.

INS/GPS aided by selective frequency contents EKF/INS navigation system architecture Aiding Sensors GPS Dynamic Vector Readings GPS, Position, Position, Magnetometer Velocity, Velocity and Gravity and Selective Bias Attitude Errors Frequency Computation Compensation Contents Inertial Sensors Rate Gyro Accelerometer INS (Error Correction Routine) Measurement Residual Calculation EKF and High-accuracy, multi-rate INS computes, velocity and attitude based on the inertial sensor readings. Aiding sensor information, such as GPS, magnetometer and gravity selective frequency contents are introduced in the EKF for error estimation and compensation. Direct-feedback error compensation.

Embedded UAV model for INS aiding Vehicle techniques: External VD aiding Aiding Sensors and (Bias Update) Inertial Sensors Thrusters Input (Error Correction Routine) INS? Vehicle Model (Error Correction Routine) Measurement Residual Calculation The external vehicle dynamics (VD) aiding is implemented using a standalone simulator. The INS and VD error derivation, estimation, and compensation techniques are similar. EKF

Embedded UAV model for INS aiding Vehicle techniques: aiding Aiding Sensors and (Bias Update) Inertial Sensors Thrusters Input (Error Correction Routine) INS Vehicle Model Measurement Residual Calculation The embedded vehicle dynamics (VD) are integrated directly in the EKF architecture. EKF The vehicle model equations are computed using the inertial estimates. The computational load of VD aiding is reduced and the implementation flexibility is added, while retaining the accuracy enhancements of VD aiding. B v ω

Embedded UAV model for INS aiding Vario X-Treme helicopter model The embedded VD aiding technique was validated for a dynamic model of the Vario X-Treme R/C Helicopter. and Vario X-treme R/C helicopter The helicopter dynamics are described using a six degree of freedom rigid body model that includes the effects of the main rotor, Bell-Hiller stabilizing bar, tail rotor, fuselage, horizontal tail plane, and vertical fin.

Embedded UAV model for INS aiding Simulation results 0.7 0.6 0.5 GPS Aiding Vario X-Treme Aiding 0.1 0.08 GPS Aiding Vario X-Treme Aiding δv y (m/s) 0.4 0.3 0.2 0.1 δb a x (m/s 2 ) 0.06 0.04 0.02 0 0 10 20 30 40 time(s) 0 0 10 20 30 40 time(s) and δb ω y (rad/s) 0.8 0.6 0.4 0.2 1 x 10-3 GPS Aiding Vario X-Treme Aiding 0 0 10 20 30 40 time(s) Bias and velocity estimation results are enhanced. The application of the embedded VD aiding to a complex and Vario X-Treme helicopter model validates the proposed technique.

and Embedded UAV model for INS aiding Simulation results GPS Aided External VD Aided Aided (ω, B v) aiding B v aiding Execution Time (s) 293 543 400 310 The embedded VD aiding is computationally more efficient than the classical VD aiding. RMS Aiding information δp (m) δv (m/s) Yaw error( o ) Pitch error( o ) Roll error( o ) GPS 2.42 3.50 10 1 1.26 10 2 9.15 10 2 7.95 10 2 Vario X-Treme Model 3.07 10 1 2.84 10 2 1.27 10 2 2.10 10 2 1.31 10 2 Vario X-Treme Model (z v only) 5.91 10 1 3.20 10 2 1.26 10 2 2.17 10 2 1.40 10 2 The VD linear velocity aiding yields about the accuracy of the full VD aiding and has a negligible computational cost with respect to GPS aiding.

Nonlinear complementary Kalman filter system architecture Euler Angles Estimate Rate Gyro Bias Estimate and The navigation system is cascade architecture of an attitude and a complementary filters. The proposed system exploits the sensor information over complementary frequency regions. Position Estimate Body Velocity Estimate The attitude and filters are endowed with stability and performance properties.

Nonlinear complementary Kalman filter Sensor fusion Position Filter Attitude Filter and Low Frequency High Frequency GPS Accelerometer Integration Vector Observations Rate Gyro Integration The complementary filters combine sensor information in the frequency domain. However, the feedback gains can be designed using a stochastic characterization of the sensors.

Nonlinear complementary Kalman filter Attitude complementary filter and The gainsk 1λ, K 2λ are computed for the LTI system [ ] [ ] [ ] [ ] [ ] xλ k+1 I T I xλ = k T I 0 wωr k +, x b k+1 0 I x b k 0 I w b k y x k = [ I 0 ] [ ] x λ k + v x λ k. b k The attitude complementary filter is uniformly asymptotically stable for bounded roll (θ<π). The complementary filter is the steady-state Kalman filter for the attitude kinematics, assuming constant roll and pitch. Performance degradation for time-varying roll and pitch is small.

Nonlinear complementary Kalman filter Position complementary filter and The gains K are computed for the LTI system [ 1p, K 2p ] [ ] [ ] [ ] [ ] xp k+1 I T I xp = k I T 2 wp + k, x v k+1 0 I x v k 0 T w v k y x k = [ I 0 ] [ ] x p k + v x p k. v k The complementary filter is uniformly asymptotically stable and is identified with the steady-state Kalman filter for the kinematics with isotropic accelerometer noise.

Nonlinear complementary Kalman filter Experimental results: DELFIMx catamaran and

and Nonlinear complementary Kalman filter Experimental results: Time domain X (m) -250-300 -350-400 -450-500 -550-600 -650-700 0 s 50 100 200 s s s TRAJECTORY ESTIMATION 300 s 350 s 400 s 550 s 600 s Comp. Filter GPS Unit 500 s 450 s 650 s 700 s -100 0 100 200 300 Y (m) YAW ESTIMATION 950 s 986 s 750 s 900 s 800 s 850 s Body Velocity (m/s) -0.5 0 200 400 600 800 Time (s) Yaw ( o ) Pitch ( o ) Roll ( o ) 3 2.5 2 1.5 1 0.5 0 260 255 250 245 10 5 0-5 5 0-5 BODY VELOCITY ESTIMATION B vx B vy B vz 1.6 1.5 1.4 1.3 1.2 0.3 0.2 0.1 0-0.1 505 510 PITCH AND ROLL ESTIMATION -10 505 506 507 508 509 510 Time (s)

Nonlinear complementary Kalman filter Experimental results: Frequency domain AIDING SENSOR INERTIAL SENSOR INTEGRATION FILTER ESTIMATE (Pendulum) (Rate Gyro) ˆθ PITCH and X-AXIS POSITION (GPS) (Accelerometer) ˆp x

based navigation system design Problem formulation: Landmark readings Objective (Landmark based observer) ˆR ˆp q r ω r v r Estimate attitude (R ) and (p) of a rigid body using landmark observations (q r ) and non-ideal angular and linear velocity measurements (w r and v r,respectively). and Rigid Body Kinematics Ṙ = R (ω), Bṗ = B v (ω) B p Velocity Measurements ω r =ω+ b ω, v r = B v+b v Landmark Measurements q r i = R L x i B p

based navigation system design Problem formulation: Vector observations Objective (Vector based attitude observer) ˆR Estimate attitude ( R) of a rigid body using vector observations (hh r ) and non-ideal angular velocity measurements (w r ). ω r and Rigid Body Kinematics Body Frame Vectors (measured) H r = [ h r1... h r n ] = [B h 1... B h n ] Ṙ =R (ω) =R H. Velocity Measurements ω r =ω+b ω Reference Vectors (known) H = [L ] h 1... L h n, {L} local frame {B} body frame

and based navigation system design Observer design technique Observer Methodology Define a function V V(x) based on the observation error of the aiding sensors (landmarks/vector readings). Derive a feedback law such that V(x)<0. V 0 Analyze the stability properties of the closed loop system using suitable based tools. Issues Topological obstacles to global stability on manifolds, namely SE(3). Relaxation to almost global stability and almost ISS frameworks is adopted. The feedback law must be a function of the sensor readings. The true and orientation are unknown. Velocity readings can be distorted by bias and/or noise.

and based navigation system design Observer design: Landmark based observer The synthesis Function is a linear combination of landmark readings and bias estimation errors V b = γ ϕ n 1 i=1 B û i B u i 2 + γ p B û n B u n 2 + γ bω b ω bω + γ bv b v bv where B u j=1..(n 1) = n 1 i=1 a ij(q i+1 q i ), B u n = 1 n n i=1 q i. The obtained observer kinematics are given by ˆR = ˆR (ˆω), B ˆp = B ˆv (ˆω) B ˆp, (B ˆb ω = γ b (γ ϕ s ω γ p ˆp ) s v), ˆb v = γ p s v, γ b n where s ω = ( ˆR XD X A X e i ) (QD X A X e i ), s v = B ˆp + 1 n q i, ( ( ˆω =ω r ˆb i=1 ω k ω s ω, B ˆv = v r ˆb v + ω r ˆb ) ) n i=1 (B ω k v I s v + k ω ˆp ) s ω, that are a function of the sensor readings (output feedback).

and based navigation system design Observer design: Vector based observer The synthesis Function is a linear combination of vector readings and bias estimation error where The obtained observer kinematics are given by where B u j = n i=1 a ijh i. ˆω = ˆR HA H A H H r V b = n 1 i=1 B û i B u i 2 + γ bω b ω bω ˆR = ˆR ( ˆω), (ω r ˆb ) ω k ω s ω, s ω = ˆb ω = k bω s ω, n ( ˆR HA H e i ) (H r A H e i ), that are a function of the sensor readings (output feedback). i=1

and based navigation system design Properties of the derived Observer Properties Almost GAS with exponential convergence Almost all initial conditions converge to the origin for ideal velocity measurements. Dynamic bias estimation with exponential convergence for worst-case initial conditions Almost all initial conditions converge to the origin for the case of biased velocity readings. Output feedback formulation The derived feedback law is an explicit function of the sensor measurements. Sensor setup characterization Necessary and sufficient landmark/vector geometry for pose estimation is derived. Directionality of the estimation errors is analyzed.

and Combination of and Almost ISS analysis Analysis Method Obtain almost ISS by combining local ISS with weakly almost ISS. Step 1 (Local ISS) Find a region γ 1 ( u ) < x(t) < r V < 0, yielding Pro (Almost ISS) where u x(t 0 ) < r lim sup t x(t) γ 1 ( u ). Step 2 (Weakly almost ISS) Find a density function ρ such that div(ρf) > 0 for x(t) > γ 2 ( u ), which implies u a.a.x(t 0 ) lim inf t x(t) γ 2 ( u ). If the system is locally ISS and weakly almost ISS, then the system is almost ISS, with u a.a.x(t 0 ) lim sup t x(t) γ 1 ( u ).

and Combination of and Almost GAS analysis Analysis Method Given the largest invariant set M in that M \ {0} is unstable, yielding agas of {0}. {x : V (x) = 0}, a density function ρ can show The stability of an equilibrium point can be excluded using a density function analysis. Theorem 1 Suppose there exists a density function ρ :R n R + that is C 1 and integrable in a neighborhood U of x u R n. If div(ρf) > 0 in U then the global inset of x u has zero measure. Almost GAS of the origin is obtained by combining Theorem 1 with LaSalle's invariance principle. Pro Let the M be the largest invariant set in {x : V (x) = 0} and assume that M is a countable union of isolated points. If there is a density function ρ that satisfies the conditions of Theorem 1 for all x u M \ {0}, then the origin is almost GAS.

and Combination of and Application to Nonlinear Observers If rate gyro noise is considered, the proposed tools show that the attitude observer is almost ISS with respect to R =I, that is u a.a.r(t 0 ) I R 2 lim sup t I R(t) 2 γ 1 ( u ). 10 1 10 0 10-1 10-2 10-3 10-4 10-5 10-6 10-7 I R(t 0 ) 2 < r(u max ) I R(t 0 ) 2 > r(u max ) r(u max ) γ 1 (u max ) 0 1 2 3 4 5 6 7 8 9 10 Time (s) Almost GAS of a observer with biased velocity readings is obtained, namely almost all the trajectories of the system θ = sin(θ) + b, converge to the set ḃ = sin(θ) {(θ, b) = (2πk, 0), k Z}.

and navigation system Problem formulation Objective Estimate attitude (R ) and (p) of a rigid body using pseudorange observations (p ρ ij ) and non-ideal angular velocity and acceleration measurements (w r and v r,respectively). ω r a r ˆR ˆp Rigid Body Kinematics Ṙ =R (ω), ṗ = v, v = a Inertial Measurements ω r =ω+ b ω + n ω, a r = R T (a g) + n a Pseudorange Measurements ρ ij = p j p S i + b c (satellite i, receiver j)

navigation system Observer configuration and The navigation system is described by a cascade of an attitude observer and a observer. Three GPS receivers onboard the vehicle are necessary for attitude estimation. In alternative, vector readings can be adopted.

and navigation system Properties of the derived observer GPS/IMU Observer Properties Almost GAS with exponential convergence Almost all initial conditions converge to the origin for ideal inertial measurements. Dynamic bias estimation with exponential convergence for worst-case initial conditions Almost all initial conditions converge to the origin for the case of biased velocity readings. Almost ISS with inertial sensor noise The estimation errors converge to a known neighborhood of the origin. Output feedback formulation The derived feedback law is an explicit function of the sensor measurements.

navigation system Simulation results 10 9 8 7 GRAVITY COMPENSATION ERROR n a max = 0.3m/s 2, n ω max = 0rad/s n a max = 3m/s 2, n ω max = 0rad/s n a max = 0.3m/s 2, n ω max = 10 π 180 rad/s 2.5 2 VELOCITY ESTIMATION ERROR n a max = 0.3m/s 2, n ω max = 0rad/s n a max = 3m/s 2, n ω max = 0rad/s n a max = 0.3m/s 2, n ω max = 10 π 180 rad/s 6 5 4 3 2 E ṽ (m/s) 1.5 1 0.5 and E p (m) 1 3.5 3 2.5 2 1.5 0 0 2 4 6 8 10 12 14 16 18 20 1 Time (s) POSITION ESTIMATION ERROR n a max = 0.3m/s 2, n ω max = 0rad/s n a max = 3m/s 2, n ω max = 0rad/s n a max = 0.3m/s 2, n ω max = 10 π 180 rad/s I R 0 0 2 4 6 8 10 12 14 16 18 20 1.8 1.6 1.4 1.2 1 0.8 0.6 0.4 Time (s) ATTITUDE ESTIMATION ERROR n a max = 0.3m/s 2, n ω max = 0rad/s n a max = 0.3m/s 2, n ω max = 10 π 180 rad/s 0.5 0.2 0 0 2 4 6 8 10 12 14 16 18 20 Time (s) 0 0 2 4 6 8 10 12 14 16 18 20 Time (s)

and future work EKF/INS + Advanced Vector Aiding IMU + Landmark Detection and EKF/INS + Vehicle Model Aiding Kalman Filtering Complementary Kalman Filtering Performance driven design Integrated advanced aiding techniques; Accounted for a variety of sensor non-idealities and disturbances. IMU + Vector Observations Nonlinear Observers IMU + GPS New Stability Results using Density Functions Stability driven design Guaranteed stability with some sensor non-idealities; Accounting for extra non-idealities implies system redesign.

Journal publications Nonlinear and Kalman Filter s - Embedded UAV model and Laser Aiding for inertial s Control Engineering Practice, Elsevier - Discrete-time complementary filters for attitude and :, analysis and experimental validation Transactions on Control s Technology, IEEE - A and attitude observer on SE(3) using landmark measurements s & Control Letters, Elsevier - Combination of and for - navigation system In preparation Magnetometer Calibration in Sensor Frame Transactions on Aerospace and Control s, IEEE