Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear Navigation System Designwith Application to Autonomous Vehicles
J.F. Vasconcelos (Ph.D. Candidate)C. Silvestre (Supervisor) P. Oliveira (Co-supervisor)
Institute for Systems and RoboticsInstituto Superior Técnico
Portugal
January 2010
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Presentation outline
1. Introduction
� Thesis outline
� Main contributions
2. Kalman Filter Based Navigation Systems
� Embedded UAV model for INS aiding
� Nonlinear Complementary Kalman filter
3. Lyapunov Based Navigation System Design
● Attitude and position nonlinear observers
● Combination of Lyapunov and density functions for nonlinear observer stability analysis
● Nonlinear GPS/IMU navigation system
4. Conclusions and Future Work
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
IntroductionProblem statement
• Design criteria
—Stability: the state estimate converges to the real position andattitude.
—Performance: the state estimate is obtained by exploiting the sensor readings with optimality criteria.
Estimate attitude and position of a rigid body with respect to a reference frame, using available sensor data.
Navigation problem
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
IntroductionThesis outline
• The proposed navigation systems are derived using two methodologies
—Kalman filtering techniques;
—Lyapunov and density functions stability theory.
Kalman
Filtering
EKF/INS
+
Advanced
Vector Aiding
EKF/INS
+
Vehicle Model
Aiding
Complementary
Kalman
Filtering
Nonlinear
Observers
IMU
+
Landmark
Detection
IMU
+
Vector
Observations
IMU
+
GPS
New Stability Results
using Density Functions
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
IntroductionMain contributions: Kalman filtering
Kalman
Filtering
(A)
EKF/INS
+
Advanced
Vector Aiding
(A)
EKF/INS
+
Vehicle Model
Aiding
(B)
Complementary
Kalman
Filtering
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.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
IntroductionMain contributions: Nonlinear observers
Nonlinear
Observers
(A)
IMU
+
Landmark
Detection
(A)
IMU
+
Vector
Observations
(A)
IMU
+
GPS
(B) New Stability Results using Density Functions
Main contributions in the field of nonlinear observers
(A) Nonlinear attitude and position observers for inertial systems
- with diverse aiding sensors.
- endowed with stability properties for worst-case errors.
(B) New stability analysis tools
- based on the combination of density and Lyapunov functions,
- for almost GAS and almost ISS analysis of nonlinear systems.
- that eventually yield almost ISS of the proposed observers.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Presentation outline
1. Introduction
� Thesis outline
� Main contributions
2. Kalman Filter Based Navigation Systems
� Embedded UAV model for INS aiding
� Nonlinear Complementary Kalman filter
3. Lyapunov Based Navigation System Design
● Attitude and position nonlinear observers
● Combination of Lyapunov and density functions for nonlinear observer stability analysis
● Nonlinear GPS/IMU navigation system
4. Conclusions and Future Work
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Rate GyroAccelerometer
(Error Correction Routine)
INSEKF
MeasurementResidual
Calculation
GPSDynamic
Vector Readings
Rate GyroAccelerometer
(Error Correction Routine)
INSEKF
MeasurementResidual
Calculation
GPSDynamic
Vector Readings
Rate GyroAccelerometer
(Error Correction Routine)
INSEKF
MeasurementResidual
Calculation
GPSDynamic
Vector Readings
GPS, Magnetometer and GravitySelective Frequency Contents
Position, Velocity andAttitude Computation
Position, Velocity, Attitude andBias Errors Compensation
� High-accuracy, multi-rate INS computes position, 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.
Inertial Sensors
Aiding Sensors
Rate GyroAccelerometer
(Error Correction Routine)
INSEKF
MeasurementResidual
Calculation
GPSDynamic
Vector Readings
INS/GPS aided by selective frequency contentsEKF/INS navigation system architecture
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
INS
(Error Correction Routine)
Inertial Sensors
Aiding Sensors
(Bias Update)EKF
MeasurementResidual
Calculation
Vehicle ModelThrusters Input
(Error Correction Routine)
Vehicle Model??
Embedded UAV model for INS aidingVehicle model aiding techniques: External VD aiding
•The external vehicle dynamics (VD) aiding is implemented using a standalone simulator.
•The INS and VD error derivation, estimation, and compensation techniques are similar.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
INS
(Error Correction Routine)
Inertial Sensors
Aiding Sensors
(Bias Update)EKF
MeasurementResidual
Calculation
Vehicle ModelThrusters Input
Embedded UAV model for INS aidingVehicle model aiding techniques: Embedded VD aiding
•The embedded vehicle dynamics (VD) are integrated directly in the EKF architecture.
•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.
Bv ω
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Vario X-treme R/C helicopter
The embedded VD aiding technique was validated for a nonlinear dynamic model of the 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 aidingVario X-Treme helicopter model
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
• Bias and velocity estimationresults are enhanced.
• The application of the embeddedVD aiding to a complex andnonlinear Vario X-Tremehelicopter model validates theproposed technique.
Embedded UAV model for INS aidingSimulation results
0 10 20 30 400
0.1
0.2
0.3
0.4
0.5
0.6
0.7
time(s)
| δv y| (
m/s
)
GPS AidingVario X-Treme Aiding
0 10 20 30 400
0.02
0.04
0.06
0.08
0.1
time(s)
| δb a
x| (m
/s2 )
GPS AidingVario X-Treme Aiding
0 10 20 30 400
0.2
0.4
0.6
0.8
1x 10-3
time(s)
| δb ω
y| (
rad/
s)
GPS AidingVario X-Treme Aiding
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Embedded UAV model for INS aidingSimulation results
� The embedded VD aiding is computationally more efficient than the classical VD aiding.
GPS Aided External VD Aided Embedded VD Aided
(ω,Bv) aiding Bv aiding
Execution Time (s) 293 543 400 310
� 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.
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 (zv only) 5.91 × 10−1 3.20 × 10−2 1.26 × 10−2 2.17 × 10−2 1.40 × 10−2
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
� The navigation system is cascade architecture of an attitude anda position complementary filters.
� The proposed system exploits the sensor information over complementary frequency regions.
� The attitude and position filters are endowed with stability and performance properties.
Euler Angles Estimate
Rate Gyro Bias Estimate
Position Estimate
Body Velocity Estimate
Nonlinear complementary Kalman filterNavigation system architecture
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
AccelerometerIntegration
GPS
Position Filter
Rate GyroIntegration
High Frequency
Vector Observations
Low Frequency
Attitude Filter
Nonlinear complementary Kalman filterSensor fusion
� The complementary filters combine sensor information inthe frequency domain.
� However, the feedback gains can be designed using a stochastic characterization of the sensors.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear complementary Kalman filterAttitude complementary filter
� The gains are computed for the LTI system
� The attitude complementary filter is uniformly asymptoticallystable 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.
K1λ, K2λ[
xλ k+1
xb k+1
]
=
[
I −T I
0 I
] [
xλ k
xb k
]
+
[
−T I 0
0 I
] [
wωr k
wb k
]
,
yx k =[
I 0]
[
xλ k
xb k
]
+ vλ k .
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear complementary Kalman filterPosition complementary filter
� The gains are computed for the LTI system
� The position complementary filter is uniformly asymptoticallystable and is identified with the steady-state Kalman filterfor the position kinematics with isotropic accelerometer noise.
[
xp k+1
xv k+1
]
=
[
I T I
0 I
] [
xp k
xv k
]
+
[
I −T 2
0 −T
] [
wp k
wv k
]
,
yx k =[
I 0]
[
xp k
xv k
]
+ vp k.
K1p, K2p
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear complementary Kalman filterExperimental results: DELFIMx catamaran
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
245
250
255
260
Yaw
(o)
-5
0
5
10
Pitc
h (o )
505 506 507 508 509 510-10
-5
0
5R
oll (
o )
Time (s)
0 200 400 600 800-0.5
0
0.5
1
1.5
2
2.5
3
Time (s)
Bod
y V
eloc
ity (
m/s
)
BvxBvyBv
z
1.2
1.31.4
1.5
1.6
505 510-0.1
0
0.10.2
0.3
-100 0 100 200 300-700
-650
-600
-550
-500
-450
-400
-350
-300
-250
0 s 50 s100 s200 s
300 s350 s 400 s
450 s
500 s550 s
600 s650 s
700 s750 s
800 s
850 s900 s
950 s
986 s
Y (m)
X (
m)
Comp. FilterGPS Unit
TRAJECTORY ESTIMATION BODY VELOCITY ESTIMATION
PITCH AND ROLL ESTIMATIONYAW ESTIMATION
Nonlinear complementary Kalman filterExperimental results: Time domain
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos AIDING SENSOR INERTIAL SENSOR INTEGRATION FILTER ESTIMATE
(Rate Gyro)
(Accelerometer)
(Pendulum)
(GPS)
PIT
CH
X-A
XIS
PO
SIT
ION
Nonlinear complementary Kalman filterExperimental results: Frequency domain
θ
px
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Presentation outline
1. Introduction
� Thesis outline
� Main contributions
2. Kalman Filter Based Navigation Systems
� Embedded UAV model for INS aiding
� Nonlinear Complementary Kalman filter
3. Lyapunov Based Navigation System Design
● Attitude and position nonlinear observers
● Combination of Lyapunov and density functions for nonlinear observer stability analysis
● Nonlinear GPS/IMU navigation system
4. Conclusions and Future Work
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system designProblem formulation: Landmark readings
Estimate attitude (R ) and position (p) of a rigid body using landmark observations (q
r) and non-ideal angular
and linear velocity measurements (wrand v
r,respectively).
Rigid Body Kinematics
Objective (Landmark based observer)
R pqr
ωr vr
R = R (ω)×,
Bp = B
v − (ω)×
Bp
Velocity Measurements
ωr = ω + bω , vr =Bv + bv
Landmark Measurements
qr i = R′Lxi −
Bp
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system designProblem formulation: Vector observations
Estimate attitude ( R) of a rigid body using vector observations (h
r) and non-ideal angular velocity
measurements (wr).
Rigid Body Kinematics
Objective (Vector based attitude observer)
R
ωr
Velocity Measurements
Body Frame Vectors (measured)
hr
Reference Vectors (known)
Hr =[
hr 1 . . . hr n
]
=[
Bh1 . . .
Bhn
]
= R′H.
H =[
Lh1 . . . L
hn
]
,
{L} → local frame
{B} → body frame
ωr = ω + bωR = R (ω)×
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system designObserver design technique
• Define a Lyapunov function V(x) based on the observation error of the aiding sensors (landmarks/vector readings).
• Derive a feedback law such that V(x)<0.
• Analyze the stability properties of the closed loop system using suitable Lyapunov based tools.
Observer Design Methodology
V (x)
Design 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 position and orientation are unknown.
• Velocity readings can be distorted by bias and/or noise.
SE(3)
V (x) ≤ 0Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system designObserver design: Landmark based observer
• The synthesis Lyapunov Function is a linear combination of landmark readings and bias estimation errors
where
Vb = γϕ∑n−1
i=1 ‖Bui −Bui‖
2 + γp‖Bun − Bun‖
2 + γbω b′
ωbω + γbv b′
vbv
Bun = − 1n
∑n
i=1 qi.Buj=1..(n−1) =∑n−1
i=1 aij(qi+1 − qi),
• The obtained observer kinematics are given by
where
that are a function of the sensor readings (output feedback).
˙R = R (ω)
×, B ˙p = Bv − (ω)
×
Bp,
˙bω = γb(γϕsω − γp
(
Bp)
×sv),
˙bv =
γp
γbsv,
ω = ωr − bω − kωsω,B v = vr − bv +
(
(
ωr − bω
)
×
− kvI
)
sv + kω(
Bp)
×sω,
sω =
n∑
i=1
(R′XDXAXei) × (QDXAXei), sv = B p +1
n
n∑
i=1
qi,
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system design Observer design: Vector based observer
• The synthesis Lyapunov Function is a linear combination of vector readings and bias estimation error
where
• The obtained observer kinematics are given by
where
that are a function of the sensor readings (output feedback).
Buj =∑n
i=1 aijhi.
Vb =∑n−1
i=1 ‖Bui −Bui‖
2 + γbω b′
ωbω
ω = R′HAHA′
HH′
r
(
ωr − bω
)
− kωsω , sω =
n∑
i=1
(R′HAHei) × (HrAHei),
˙R = R (ω)
×,
˙bω = kbωsω,
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Lyapunov based navigation system design Properties of the derived observers
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 formulationThe derived feedback law is an explicit function of the sensor measurements.
� Sensor setup characterizationNecessary and sufficient landmark/vector geometry for pose estimation is derived. Directionality of the estimation errors is analyzed.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Combination of Lyapunov and density functionsAlmost ISS analysis
Proposition (Almost ISS)If the system is locally ISS and weakly almost ISS, then the system is almost ISS, with
Step 1 (Local ISS)Find a region where
, yielding
Analysis MethodObtain almost ISS by combining local ISS with weakly almost ISS.
Step 2 (Weakly almost ISS)Find a density function ρ such that for , which implies
∀u∀a.a.x(t0) lim inft→∞ |x(t)| ≤ γ2(‖u‖∞).
∀u∀a.a.x(t0) lim supt→∞|x(t)| ≤ γ1(‖u‖∞).
∀u∀|x(t0)| < r lim supt→∞|x(t)| ≤ γ1(‖u‖∞).
γ1(‖u‖∞) < |x(t)| < r
V < 0
div(ρf ) > 0|x(t)| > γ2(‖u‖∞)
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Combination of Lyapunov and density functionsAlmost GAS analysis
Analysis MethodGiven the largest invariant set M in , a density function ρ can show that is unstable, yielding aGAS of .
The stability of an equilibrium point can be excluded using a density function analysis.
Theorem 1 Suppose there exists a density function that is C1 and integrable in a neighborhood U of . If in U then the global inset of has zero measure.
Proposition Let the M be the largest invariant set in 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 , then the origin is almost GAS.
{x : V (x) = 0}
M \ {0} {0}
ρ : Rn → R+
xu ∈ Rn div(ρf ) > 0 xu
Almost GAS of the origin is obtained by combining Theorem 1 with LaSalle's invariance principle.
{x : V (x) = 0}
xu ∈ M \ {0}
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Combination of Lyapunov and density functionsApplication to Nonlinear Observers
• If rate gyro noise is considered, the proposed stability analysis tools show that the nonlinear attitude observer is almost ISS with respect to , that is
0 1 2 3 4 5 6 7 8 9 1010
-7
10-6
10-5
10-4
10-3
10-2
10-1
100
101
Time (s)
||I−
R||2
||I − R (t0)||2 < r(umax)
||I − R (t0)||2 > r(umax)
r(umax)
γ1(umax)
R = I
• Almost GAS of a nonlinear observer with biased velocity readings is obtained, namely almost all the trajectories of the system
converge to the set
θ = − sin(θ) + b, b = − sin(θ)
{(θ, b) = (2πk, 0), k ∈ Z}.
lim supt→∞
‖I −R(t)‖2 ≤ γ1(‖u‖∞).
∀u∀a.a.R(t0)Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear GPS/IMU navigation systemProblem formulation
Estimate attitude (R ) and position (p) of a rigid body using pseudorange observations (pij) and non-ideal angular velocity and acceleration measurements (wr and vr,respectively).
Rigid Body Kinematics
ObjectiveR p
ωr
Inertial Measurements
Pseudorange Measurements
ρij
ar
R = R (ω)×, p = v, v = a
ωr = ω + bω + nω ,
ar = RT (a − g) + na
ρij = ‖pj − pS i‖ + bc
(satellite i, receiver j)
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear GPS/IMU navigation systemObserver configuration
� The navigation system is described by a cascade of an attitude observer and a position observer.
� Three GPS receivers onboard the vehicle are necessary for attitude estimation. In alternative, vector readings can be adopted.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear GPS/IMU navigation systemProperties 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 noiseThe estimation errors converge to a known neighborhood of the origin.
� Output feedback formulationThe derived feedback law is an explicit function of the sensor measurements.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
0 2 4 6 8 10 12 14 16 18 200
0.5
1
1.5
2
2.5
3
3.5
Time (s)
||Ep|| (m)
namax = 0.3m/s2, nωmax = 0rad/s
namax = 3m/s2, nωmax = 0rad/s
namax = 0.3m/s2, nωmax = 10π
180rad/s
0 2 4 6 8 10 12 14 16 18 200
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
Time (s)
||I−R||
namax = 0.3m/s2, nωmax = 0rad/s
namax = 0.3m/s2, nωmax = 10π
180rad/s
0 2 4 6 8 10 12 14 16 18 200
0.5
1
1.5
2
2.5
Time (s)
||Ev|| (m/s)
namax = 0.3m/s2, nωmax = 0rad/s
namax = 3m/s2, nωmax = 0rad/s
namax = 0.3m/s2, nωmax = 10π
180rad/s
0 2 4 6 8 10 12 14 16 18 200
1
2
3
4
5
6
7
8
9
10
Time (s)
namax = 0.3m/s2, nωmax = 0rad/s
namax = 3m/s2, nωmax = 0rad/s
namax = 0.3m/s2, nωmax = 10π
180rad/s
GRAVITY COMPENSATION ERROR VELOCITY ESTIMATION ERROR
ATTITUDE ESTIMATION ERRORPOSITION ESTIMATION ERROR
Nonlinear GPS/IMU navigation systemSimulation results
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Presentation outline
1. Introduction
� Thesis outline
� Main contributions
2. Kalman Filter Based Navigation Systems
� Embedded UAV model for INS aiding
� Nonlinear Complementary Kalman filter
3. Lyapunov Based Navigation System Design
● Attitude and position nonlinear observers
● Combination of Lyapunov and density functions for nonlinear observer stability analysis
● Nonlinear GPS/IMU navigation system
4. Conclusions and Future Work
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Conclusions and future work
Kalman
Filtering
EKF/INS
+
Advanced
Vector Aiding
EKF/INS
+
Vehicle Model
Aiding
Complementary
Kalman
Filtering
Nonlinear
Observers
IMU
+
Landmark
Detection
IMU
+
Vector
Observations
IMU
+
GPS
New Stability Results
using Density Functions
Performance driven design
� Integrated advanced aiding techniques;
� Accounted for a variety of sensor non-idealities and disturbances.
Stability driven design
� Guaranteed stability with some sensor non-idealities;
� Accounting for extra non-idealities implies system redesign.
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
boot
Nonlinear Navigation
System Design
J.F. Vasconcelos
Journal publications
� Kalman Filter Based Navigation Systems
� Lyapunov Based Navigation System Design
� Magnetometer Calibration in Sensor Frame
Control Engineering Practice, Elsevier- “Embedded UAV model and Laser Aiding for inertial Navigation Systems”
Transactions on Control Systems Technology, IEEE
- “A nonlinear position and attitude observer on SE(3) using landmark measurements”
Transactions on Aerospace and Control Systems, IEEE
In preparation
Systems & Control Letters, Elsevier
- “Discrete-time complementary filters for attitude and position: Design, analysis and experimental validation”
- “Combination of Lyapunov and density functions for stability analysis”- “Nonlinear GPS/IMU navigation system”
Introduction
KF Based Nav. SystemsEmbedded VD model aiding
Nonlinear CKF
LyapunovBased Nav. SystemsAttitude and position nonlinear observers
Lyapunov and density functionsstability analysis
Nonlinear GPS/IMU
Conclusions
Nonlinear Navigation
System Design
J.F. Vasconcelos
Nonlinear Navigation System Designwith Application to Autonomous Vehicles
J.F. Vasconcelos (Ph.D. Candidate)C. Silvestre (Supervisor) P. Oliveira (Co-supervisor)
Institute for Systems and RoboticsInstituto Superior Técnico
Portugal
January 2010