Upload
hoangnhi
View
217
Download
3
Embed Size (px)
Citation preview
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 1
Chapter 8
State Estimation
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 2
Architecture
Path planner
Path manager
Path following
Autopilot
Unmanned Vehicle
Waypoints
On-board sensors
Position error
Tracking error
Status
Destination, obstacles
Servo commands State estimator
Wind
Path Definition
Airspeed, Altitude, Heading,
commands
Map
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 3
Why estimate states?
• State information needed to control aircraft
• We measure many things
– accelerations, angular rates, pressure altitude, dynamic
pressure (airspeed), magnetic heading (sometimes),
GPS position
• Don’t have direct measurements of everything we
needState Measured directly or estimated?
pn Measured (GPS) and smoothed
pe Measured (GPS) and smoothed
pd Measured (absolute pressure, GPS)
u Estimated with EKF
v Estimated with EKF
w Estimated with EKF
� Estimated with EKF
✓ Estimated with EKF
Estimated with EKF
p Measured (rate gyro)
q Measured (rate gyro)
r Measured (rate gyro)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 4
Benchmark Maneuver
0 5 10 15 20 25 30 35 4090
95
100
105
110
hc (m)
t (s)
Commanded altitude
0 5 10 15 20 25 30 35 40−40
−20
0
20
40
�a (deg/s)
t (s)
Commanded course
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 5
Inverting Sensor Measurement Model
• Several states can be estimated by inverting
sensor measurement model
– Angular rates, altitude, airspeed
– LPF denotes low pass filter
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 6
0 5 10 15 20 25 30 35 40−100
0
100p
(deg
/s)
t (s)
Roll rate estimation: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 40−200
0
200
q (d
eg/s
)
t (s)
Pitch rate estimation: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 40−100
0
100
r (de
g/s)
t (s)
Yaw rate estimation: Solid−true, Dashed−estimate
Model Inversion - p, q, r
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 7
Model Inversion - h, Va
0 5 10 15 20 25 30 35 40−20
0
20
40
60
80
100
h (m)
t (s)
Altitude estimation: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 409
9.5
10
10.5
11
11.5
12
Va (m/s)
t (s)
Airspeed estimation: Solid−true, Dashed−estimate
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 8
Inverting Sensor Measurement Model
Assuming steady, level flight (no acceleration), we can also invert accelerometer
measurements to determine pitch and roll. Accelerometer outputs are
yaccel,x
= u+ qw � rv + g sin ✓ + ⌘accel,x
yaccel,y
= v + ru� pw � g cos ✓ sin�+ ⌘accel,y
yaccel,z
= w + pv � qu� g cos ✓ cos�+ ⌘accel,z
In unaccelerated flight, u = v = w = p = q = r = 0, and
LPF (yaccel,x
) = g sin ✓
LPF (yaccel,y
) = �g cos ✓ sin�
LPF (yaccel,z
) = �g cos ✓ cos�
Solving for � and ✓:
ˆ�accel
= tan
�1
✓LPF (y
accel,y
)
LPF (yaccel,z
)
◆
ˆ✓accel
= sin
�1
✓LPF (y
accel,x
)
g
◆
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 9
0 5 10 15 20 25 30 35 40−60
−40
−20
0
20
40
φ (d
eg)
t (s)
Roll estimation: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 40−10
0
10
20
30
θ (d
eg)
t (s)
Pitch estimation: Solid−true, Dashed−estimate
Model Inversion - �, ✓
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 10
Inverting Sensor Measurement Model
• Can also invert sensor models for GPS signals:
pn = LPF (yGPS,n)
pe = LPF (yGPS,e)
� = LPF (yGPS,�)
Vg = LPF (yGPS,Vg )
• Update rate too slow ( 1 Hz)
• Need to fill in samples between measurement updates
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 11
0 5 10 15 20 25 30 35 40−20
0
20N
orth
erro
r (m
)
t (s)
North position estimation error
0 5 10 15 20 25 30 35 400
20
40
East
erro
r (m
)
t (s)
East position estimation error
0 5 10 15 20 25 30 35 40−50
0
50
� (deg)
t (s)
Course: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 4010
15
20Vg (m/s)
t (s)
Ground speed: Solid−true, Dashed−estimate
Model Inversion - pn, pe, �, Vg
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 12
Dynamic Observer Theory
Given linear time-invariant model
x = Ax+Bu
y = Cx.
A continuous-time observer for this system is given by
˙x = Ax+Bu| {z } + L (y � Cx)| {z },
copy of the model correction due to sensor reading
where x is the estimated value of x. Defining the observation error as x = x� x
˙x = (A� LC)x.
Therefore, observation error ! 0 if eig(A� LC) in LHP.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 13
Dynamic Observer Theory, cont.
For sampled data systems we use a predictor-corrector structure:
Between Measurements (predictor):
˙
x = Ax+Bu,
At a Measurements (corrector):
x
+= x
�+ L(y(tn)� Cx
�),
where tn is the instant in time that the measurement is received and x
�is the
state estimate produced at time tn.
For nonlinear systems:
Between Measurements (predictor):
˙
x = f(x, u)
At a Measurements (corrector):
x
+= x
�+ L(y(tn)� h(x
�)).
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 14
Dynamic Observer Theory, cont.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 15
Kalman Filter
The Kalman filter follows this same process, but also attempts to estimate
the quality of the estimate at each time step by propagating an error covariance
matrix.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 16
Quadratic Forms
Recall that for a positive definite matrix P = P> > 0, the set
E(k) =�y 2 Rn
: y>P�1y = k2
is an ellipsoid whose axes are aligned with the eigenvectors of P , with stretching
along each axis given by kp�i where �i is an eigenvalue of P .
(From wikipedia)
The eigenaxes are given by x, y, z.
a, b, and c correspond to k
p�i.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 17
Multivariate Gaussian
The multivariate Gaussian distribution is given by
N (µ, P ) =
1
(2⇡)
k/2kPk1/2exp
✓�1
2
(x� µ)
>P
�1(x� µ)
◆,
where µ is the mean and P is the covariance matrix. The k � � ellipsoid is
defined as
E(k) =�x 2 Rk
: (x� µ)
>P
�1(x� µ) = k
2
where 1� � or k = 1 corresponds the the ellipsoid that represents one standard
deviation from the mean µ.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 18
Kalman Filter
Assume continuous linear system dynamics, with discrete mea-
surement update
x = Ax+Bu+ ⇠
y[n] = Cx[n] + ⌘[n],
where ⇠ ⇠ N (0, Q) is the process noise, ⌫ ⇠ N (0, R) is the mea-
surement noise.
The measurement covariance R is usually obtained from sensor
calibration. The process covariance Q represents all other uncer-
tainties in the system and is the tuning parameter for the Kalman
filter.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 19
Kalman Filter Derivation
The continuous-discrete Kalman filter has the form
˙
x = Ax+Bu (Prediction)
x
+= x
�+ L(y(tn)� Cx
�). (Correction)
Define the estimation error
x = x� x,
and the covariance of the estimation error
P (t) = E{x(t)x(t)>},
where P = P
> � 0.
Estimation Objective: Chose the estimation gain L s.t.
• E{x(t)} = 0 (unbiased),
• trace(P ) =
P�i is minimized.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 20
Kalman Filter Derivation
Between Measurements (prediction):
Di↵erentiate x
˙
x = x� ˙
x
= Ax+Bu+ ⇠ �Ax�Bu
= Ax+ ⇠.
Solve for x(t):
x(t) = e
Atx0 +
Z t
0e
A(t�⌧)⇠(⌧) d⌧.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 21
Kalman Filter Derivation
Between Measurements (prediction):
Compute the evolution of the error covariance P :
˙
P =
d
dt
E{xx>}
= E{ ˙xx>+ x
˙
x
>}= E
�Axx
>+ ⇠x
>+ xx
>A
>+ x⇠
>
= AP + PA
>+ E{⇠x>}+ E{x⇠>},
Where
E{x⇠>} = E{eAtx0⇠
>(t) +
Z t
0e
A(t�⌧)⇠(⌧)⇠
>(⌧) d⌧}
=
Z t
0e
A(t�⌧)Q�(t� ⌧) d⌧
=
1
2
Q. (only used half of area in delta function)
Therefore
˙
P = AP + PA
>+Q.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 22
Kalman Filter Derivation
At Measurements (correction):At a measurement, we have that
x
+= x� x
+
= x� x
� � L
�
Cx+ ⌘ � Cx
��
= x
� � LCx
� � L⌘.
We also have that
P
+= E{x+
x
+T }
= E
n
�
x
� � LCx
� � L⌘
� �
x
� � LCx
� � L⌘
�>o
= E
�
x
�x
�> � x
�x
�>C
>L
> � x
�⌘
>L
>
� LCx
�x
�>+ LCx
�x
�>C
>L
>+ LCx
�⌘
>L
>
�L⌘x
�>+ L⌘x
�>C
>L
>+ L⌘⌘
>L
>
= P
� � P
�C
>L
> � LCP
�+ LCP
�C
>L
>+ LRL
>.
Objective: Pick L to minimize tr(P
+).
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 23
Kalman Filter Derivation
At Measurements (correction):
Using
@
@Atr(BAD) = B>D> @
@Atr(ABA>
) = 2AB, if B = B>
a necessary condition is
@
@Ltr(P+
) = �P�C> � P�C>+ 2LCP�C>
+ 2LR = 0
=) 2L(R+ CP�C>) = 2P�C>
=) L = P�C>(R+ CP�C>
)
�1.
Substituting into prior equation for P+gives (after algebra)
P+= (I � LC)P�.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 24
Kalman Filter Derivation
Between Measurements (prediction):
˙
x = Ax+Bu
˙
P = AP + PA
>+Q,
At the i
thMeasurement (correction):
Li = P
�C
>i (Ri + CiP
�C
>i )
�1
x
+= x
�+ Li(yi � Cix
�),
P
+= (I � LiCi)P
�
where Li is called the Kalman gain for sensor i.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 25
Extended Kalman FilterBasic idea of Extended Kalman Filter:
• Propagate multivariate Gaussian distribution that captures probability
distribution associated with belief about state
• Mean of distribution is x — estimated state
• Covariance quantifies associated estimation error
• Kalman gain minimizes covariance of estimation error given uncertainty
in model and measurements
System model given by:
x = f(x, u) + ⇠
yi[n] = hi(x[n], u[n]) + ⌘i[n],
where
⇠ ⇠ N (0, Q)
⌘i ⇠ N (0, Ri)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 26
Extended Kalman Filter, cont.
Between Measurements (prediction):
˙
x = f(x, u)
A =
@f
@x
(x, u)
˙
P = AP + PA
>+Q,
At the i
thMeasurements (correction):
Ci =@hi
@x
(x
�)
Li = P
�C
>i (Ri + CiP
�C
>i )
�1
x
+= x
�+ Li(yi(tn)� hi(x
�)),
P
+= (I � LiCi)P
�
where Li is called the Kalman gain for sensor i.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 27
EKF Algorithm
Algorithm 1 Continuous-Discrete Extended Kalman Filter
1: Initialize: x = 0.
2: Pick an output sample rate T
out
which is much less than the sample rates
of the sensors.
3: At each sample time T
out
:
4: for i = 1 to N do {Prediction}5: x = x+
�T
out
N
�f(x, u)
6: A =
@f
@x
(x, u)
7: P = P +
�T
out
N
� �AP + PA
>+Q
�
8: end for
9: if Measurement has been received from sensor i then {Correction}10: C
i
=
@h
i
@x
(x, u[n])
11: L
i
= PC
>i
(R
i
+ C
i
PC
>i
)
�1
12: P = (I � L
i
C
i
)P
13: x = x+ L
i
(y
i
[n]� h(x, u[n])).
14: end if
If R is diagonal (uncorrelated sensors), then update one measurement at a time.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 28
Attitude Estimation Using EKF
Use the nonlinear propagation model
˙� = p+ q sin� tan ✓ + r cos� tan ✓ + ⇠�˙✓ = q cos�� r sin�+ ⇠✓
where
⇠� ⇠ N (0, Q�) and ⇠✓ ⇠ N (0, Q✓)
Use accelerometers as measured outputs:
yaccel =
0
@u+ qw � rv + g sin ✓
v + ru� pw � g cos ✓ sin�w + pv � qu� g cos ✓ cos�
1
A+ ⌘accel.
Problem: Do not have method for directly measuring u, v, w, u, v, and w.
What do we do?
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 29
Attitude Estimation Using EKF
Assume that u = v = w ⇡ 0
Recall that 0
@uvw
1
A ⇡ Va
0
@cos↵ cos�
sin�sin↵ cos�
1
A .
Assuming that ↵ ⇡ ✓ and � ⇡ 0 gives
0
@uvw
1
A ⇡ Va
0
@cos ✓0
sin ✓
1
A
Substituting into original output equation
yaccel =
0
@u+ qw � rv + g sin ✓
v + ru� pw � g cos ✓ sin�w + pv � qu� g cos ✓ cos�
1
A+ ⌘accel
gives
yaccel =
0
@qVa sin ✓ + g sin ✓
rVa cos ✓ � pVa sin ✓ � g cos ✓ sin��qVa cos ✓ � g cos ✓ cos�
1
A+ ⌘accel
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 30
Attitude Estimation Using EKF
Defining x = (�, ✓)
>, u = (p, q, r, Va)
>, ⇠ = (⇠�, ⇠✓)
>, and ⌘ = (⌘�, ⌘✓)
>, and
noting that there is noise on the gyros and pressure sensor, i.e., uactual = u+ ⇠u
where ⇠u = (⇠p, ⇠q, ⇠r, ⇠Va)>, gives
x = f(x, u) +G(x)⇠u + ⇠
y = h(x, u) + ⌘,
where
f(x, u) =
✓p+ q sin� tan ✓ + r cos� tan ✓
q cos�� r sin�
◆
h(x, u) =
0
@qVa sin ✓ + g sin ✓
rVa cos ✓ � pVa sin ✓ � g cos ✓ sin�
�qVa cos ✓ � g cos ✓ cos�
1
A
G(x) =
✓1 sin� tan ✓ cos� tan ✓ 0
0 cos� � sin� 0
◆.
Note that
E{(G⇠u + ⇠)(G⇠u + ⇠)
>} = GE{⇠u⇠>u }+ E{⇠⇠>} = GQuG>+Q
where we have assumed that E{⇠u⇠>u } = Qu.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 31
Attitude Estimation Using EKF
Implementation of Kalman filter requires Jacobians @f
@x
and @h
@x
.Accordingly,
@f
@x
=
✓q cos� tan ✓ � r sin� tan ✓ q sin��r cos�
cos
2✓
�q sin�� r cos� 0
◆
@h
@x
=
0
@0 qV
a
cos ✓ + g cos ✓�g cos� cos ✓ �rV
a
sin ✓ � pV
a
cos ✓ + g sin� sin ✓g sin� cos ✓ (qV
a
+ g cos�) sin ✓
1
A.
At this point, we have everything we need to implement the continuous-discreteEKF.
How well does it work?
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 32
Attitude Estimation Results
0 5 10 15 20 25 30 35 40−100
−50
0
50φ
(deg
)
t (s)
Roll estimation: Solid−true, Dashed−estimate
0 5 10 15 20 25 30 35 40−10
0
10
20
30
θ (d
eg)
t (s)
Pitch estimation: Solid−true, Dashed−estimate
Not perfect, but significantly better!
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 33
GPS Smoothing
• Want to fill in estimates between GPS measurements
• Want to estimate wind
Assuming level flight, evolution of position:
pn = Vg cos�
pe = Vg sin�
Evolution of the ground speed:
˙Vg =
d
dt
p(Va cos + wn)
2+ (Va sin + we)
2
=
(Va cos + wn)(˙Va cos � Va
˙ sin + wn) + (Va sin + we)(˙Va sin + Va
˙ cos + we)
Vg
Assuming that wind and airspeed are constant:
˙Vg =
(Va cos + wn)(�Va˙ sin ) + (Va sin + we)(Va
˙ cos )
Vg
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 34
GPS Smoothing
Evolution of �:
� =
g
Vgtan� cos(�� )
Assuming that wind is constant:
wn = 0
we = 0
From kinematics, evolution of :
˙ = qsin�
cos ✓+ r
cos�
cos ✓
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 35
GPS Smoothing
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 36
GPS Smoothing
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 37
GPS SmoothingDefine:
x = (pn, pe, Vg,�, wn, we, )>
u = (Va, q, r,�, ✓)>
For measurements, use GPS signals for north and east position (pn, pe), ground
speed (Vg), and course (�)
Notice that states are not independent – related by wind triangle
Use wind triangle relations to introduce two pseudo measurements.
Assume � = �a = 0:
Va cos + wn = Vg cos�
Va sin + we = Vg sin�
From these expressions, define the pseudo measurements
ywindtri,n = Va cos + wn � Vg cos�
ywindtri,e = Va sin + we � Vg sin�
where (pseudo) measurement values are equal to zero.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 38
GPS Smoothing
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 39
GPS Smoothing Results
0 10 20 30 40−5
0
5no
rth e
rror (
m)
time (s)0 10 20 30 40
−5
0
5
east
erro
r (m
)
time (s)
0 10 20 30 4010
15
20
V g (m/s
)
time (s)0 10 20 30 40
−50
0
50
� (d
eg)
time (s)
0 10 20 30 400
5
wn (m
/s)
time (s)0 10 20 30 40
−5
0
5
we (m
/s)
time (s)
0 10 20 30 40−100
0
100
� (d
et)
time (s)
trueestimate
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 40
Matlab Simulation
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 41
Suggestions for Tuning Your EKF
• Implement EKF in steps
– Attitude estimation
– GPS smoother
• Test and tune each component independently and thoroughly
– Attitude estimator first
– GPS smoother second
• As a first step, make sure your filter estimates track the states when sensors are
perfect (no sensor error). This will expose coding errors and show the limits of
performance of your filter.
• Keep the wind set to zero until you are confident that your filter is well tuned.
• We know R pretty well typically. Tune filter by changing Q.
• Tune filter by focusing on individual states. Give inputs to those states while keeping
other states “quiet”. Adjust Q value corresponding to state of interest. Tune by trial
and error (use your brain to make a good guess!).
• Don’t put extreme inputs into the system when tuning the filter (e.g., large steps). Your
filter will have its own dynamic limits – don’t make the problem impossibly difficult.
• Tune your controllers so that the response of the aircraft to typical inputs is smooth
and graceful. Abrupt and jerky state dynamics are difficult to estimate.
• Check your equations AGAIN!