Nonlinear Navigation System Design J.F. Vasconcelos Nonlinear Navigation System Design with Application to Autonomous Vehicles J.F. Vasconcelos (Ph.D. Candidate) C. Silvestre (Supervisor) P. Oliveira (Co-supervisor) Institute for Systems and Robotics Instituto Superior Técnico Portugal January 2010
38
Embed
Nonlinear Navigation System Design with Application …users.isr.ist.utl.pt/.../jfvasconcelos_thesis_presentation_handout.pdf · Nonlinear Navigation System Design with Application
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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
� 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.
� 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
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
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