16.333: Lecture #15
Inertial Sensors
Complementary ?ltering
Simple Kalman ?ltering
1
Fall 2004 16.333 15–2
Fall 2004 16.333 15–3
Image removed due to copyright considerations.
Fall 2004 16.333 15–4
Image removed due to copyright considerations.
Fall 2004 16.333 15–5
Image removed due to copyright considerations.
Image removed due to copyright considerations.
Fall 2004 16.333 15–6
Fall 2004 16.333 15–7
Fibre Coil
Lens
Spatial
Filter
Laser
Source
Splitters
Sagnac
0
?
Phase Shift
?
Lens
?? ??
Fringe Pattern
Detector
Polariser
Beam
Optical Intensity
Phase Shift
Proportional to
Fall 2004 16.333 15–11
Fall 2004 16.333 15–8
Fall 2004 16.333 15–9
Examples of Estimation Filters
from Recent Aircraft Projects at MIT
November 2004
Sanghyuk Park and Jonathan How
Complementary Filter
(CF)
Often, there are cases where you have
two
different measurement sources
for
estimating
one
variable and the noise properties of the two measurements
are such that one source gives good information only in low frequency region while the other is good only in high frequency region. ?
You can use a complementary filter !
accelerometer
rate gyro
θ
?? ??
?? ??
≈
?
g
output
accel.
sin
1
θ
-
n
ot good in long term
due to integration
-
o
nly good in long ter
m
-
n
ot proper during fast m
o
tion
Low Pass FilterHigh Pass Filter
∫
≈
dt
rate)
(angular
θ
est
θ
Example
: Tilt angle estimation using accelerometer and rate gyro
? ??
? ??
+
=
1
1
s
τ
? ??
? ??
+
=
example
for
,
1
s
s
τ
τ
Complementary Filter(CF) Examples
?
CF1. Roll Angle Estimation
?
CF2. Pitch Angle Estimation
?
CF3. Altitude Estimation
?
CF4. Altitude Rate Estimation
CF1. Roll Angle Estimation
?
H
igh freq. : integrating roll rate (p) gyro output
?
L
ow freq. : using aircraft kinematics
-
Assuming steady state turn dynamics,
roll angle is related with turning
rate, which is close to yaw
r
ate (r)
φ
φ
φ
≈
≈
?
≈
?
=
sin
sin
r
mg
L
mV
L
r
g
V
≈
φ
CF setup
1 s
HPF
LPF
V
g
RollRateGyro YawRateGyro
++
Roll angleestimat
e
p r
φ
CF2. Pitch Angle Estimation
?
H
igh freq. : integrating pi
tch rate (q) gyro output
?
L
ow freq. : using the sensitivity of
accelerometers to gravity direction
-“
gravity aiding
”
In steady state
θ
θ
cos
sin
g
A
g
A
ZX
?
=
=
?? ??
?? ??
?
=
?
zx
AA
1
tan
θ
outputs
ter
accelerome
,
?
Z
X
A
A
?
R
oll angle compensation is needed
CF setup
est
meas
q
φ
θ
cos
≈
?? ??
?? ??
?
=
?
est
zx
ss
AA
φ
θ
cos
tan
1
meas
q
est
φ
x
A
z
A
est
φ
s
1
LPF
HPF
+ +
est
θ
CF3. Altitude Estimation
?
M
otivation : GPS receiver gives altitude ou
tput, but it has ~0.4 seconds of delay.
In order of overcome this,
pressure sensor was added.
?
L
ow freq. : from GPS receiver
?
H
igh freq. : from pressure sensor
CF setup & fl
ight dat
a
KF
GPS
from
h
LPF
HPF
+ +
est
h
Sensor
Pressure
from
h
CF4. Altitude Rate Estimation
?
M
otivation : GPS receiver gives altitude ra
te, but it has ~0.4 seconds of delay.
In order of overcome this, inerti
al sensor outputs were added.
?
L
ow freq. : from GPS receiver
?
H
igh freq. : integrating acceleratio
n estimate in altitude direction
from inertial sensors
CF setup
KF
GPS
from
h
z
a
y
a
x
a
Angular Transform
est
est
θ
φ
,
s
1
LPF
HPF
+ +
h
a
est
h
outputs
ter
accelerome
,
?
z
x
A
A
[]
[
]
? ?? ??
? ?? ??
?
?
? ?? ??
? ?? ??
=
? ?? ??
? ?? ??
g
AAA
aaa
est
est
zyx
zyx
00
:
note
θ
φ
[
]
[
]
matrices
tion
transforma
angular
:
,
est
est
θ
φ
Kalman Filter(KF) Examples
?
KF1. Manipulation of GPS Outputs
?
KF2. Removing Rate Gyro Bias Effect
KF 1. Manipulation of GPS Outputs
Backg
r
ound & Motivation
?
S
tand-alone GPS receiver gi
ves position and velocity
?
positi
on
?
pseudo-ranges
?
v
eloci
t
y
?
Doppler effect
?
T
hese are obtained by independent methods :
and are certainly relat
e
d
)
(
v
x
=
?
Kalman fil
t
er can be used to combine them !
?
M
otivation :
Position
~
30 m
Velocity
~ 0.15 m/s
Typi
cal
Accura
cie
s
Many GPS receivers provide high quality velocity information
?
Use
high quality velocity measure
m
ent to improve position estimate
KF 1. Kalman Filter Setup
2
ν
+
=
v
v
meas
a
v
dt
d
=
1
ω
=
j
dt
d
Measurements
Filter Dynamics
meas
x
1
ν
+
=
x
x
meas
meas
v
j
a
dt
d
=
v
x
dt
d
=
est
x
est
v
est
a
North
East
Down
a
v
j
i
i
ω
ν
,
x
: velocity
: acceleration
: jerk
: position
: white noises
:
est
a
?
noisy, but not biased
?
c
ombined with rate gyros in removing the gyro biases (KF2)
KF 2. Removing Rate Gyro Bias Effect
Backg
r
ound & Motivation
?
I
n aircraft control,
roll angle
control is commonly used in inner-loop to create required
lateral acceleration
which is commanded from guidance outer-loop
?
B
iased roll angle estimate can cause steady-state error in cross-track
1 s
HPF
LPF
V
g
RollRateGyro YawRateGyro
++
Roll angleestimat
e
p r
φ
Drawback : biased estimate
Complementary filter with roll & raw gyros (CF1)
Single-Ant
enna GPS B
a
sed
Aircraft Attitude Determination
-
Richard Kornfeld, Ph.D.
(1999)
Drawback : sampling rate limit (GPS),
typical filter tim
e
constant ~ 0.
5 sec.
r
V
g
a
s
?
≈
?
≈
φ
p
≈
φ
KF 2. Kalman Filter Setup
s
a
V
p
i
i
ω
ν
,
r
φ
: velocity
: acceleration in si
deways direction
: roll rate
: yaw rate
: bank angle
: white noises
2
ν
+
+
=
p
meas
bias
p
p
2
ω
=
p
dt
d
3
ω
=
p
bias
dt
d
Measurement Equations
Filter Dynamics
meas
p
(
)
est
p
bias
()
est
s
a
1
ν
φ
+
=
g
a
s
3
ν
φ
+
+
=
r
meas
bias
V
g
r
meas
r
4
ω
=
r
bias
dt
d
1
ω
φ
+
=
p
dt
d
est
p
(
)
est
r
bias
est
φ
from
Rate Gyros
from
GPS Kalman Filter
KF 2. Simulation Result
?
S
imula
tion for 10 degree bank angle hold
?
R
oll rate gyro bias=0.03 rad/
s, yaw rate gyro bias = 0.02 rad/s
w
ere used in simulation
References
?
Applied Optimal Estimation
Edited by Arthur Gelb
,
MIT Pres
s, 1974
?
Fundam
entals of K
a
lman Filtering –
A
Pr
actical Appr
oach
Paul Zarchan
&
Howard Musoff
,
Progress in Astronautics and Aeronautics Vol. 190
?
Avionics and Contro
l System Development for Mid-
Air Rendez
v
ous of T
w
o Unmanned Aerial Vehicles
Sanghyuk Park, Ph.D. Th
esis, MI
T, Feb. 2004
?
Fundamentals of High Accu
racy Inertia
l
Navigation
Averil
C
h
atfield, Progres
s in Astronautics and Aer
o
nautics Vol. 174
?
Applied Mathematics in Inte
grated N
a
vigation Systems
R. Rogers, AIAA Educatio
n Series, 2000
?
The Impact of GP
S V
e
locity B
a
sed Flight C
ontrol on Flight Instrumentation Architecture
Richard Kornfeld, P
h
.D. Thesis, MIT, Jun. 1999
?
Autonomous A
e
robatic Maneuvering of Miniature Helicopters
Valdislav
G
avrilets, Ph.D. Thesis, MIT, May 2003