Mechatronics - Foundations and Applications
Position Measurement in Inertial Systems
JASS 2006, St.Petersburg
Christian Wimmer
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Content
1.
2.
3.
4.
Motivation
Basic principles of position measurement
Sensor technology
Improvement: Kalman filtering
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Johnnie: A biped walking machine
Orientation
Stabilization
Navigation
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Automotive Applications:
Drive dynamics Analysis
Analysis of test route topology
Driver assistance systems
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Aeronautics and Space Industry:
Autopilot systems
Helicopters
Airplane
Space Shuttle
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Military Applications:
ICBM, CM
Drones (UAV)
Torpedoes
Jets
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Maritime Systems:
Helicopter Platforms
Naval Navigation
Submarines
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Motivation
Industrial robotic Systems:
Maintenance
Production
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Measurement by inertia and integration:
Acceleration
Velocity
Position
Measurement system
with 3 sensitive axes
3 Accelerometers
3 Gyroscope
Newton‘s 2. Axiom:
F=mxa
BASIC PRINCIPLE OF DYNAMICS
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Gimballed Platform Technology:
3 accelerometers
3 gyroscopes
cardanic Platform
ISOLATED FROM ROTATIONAL MOTION
TORQUE MOTORS TO MAINTAINE DIRECTION
ROLL, PITCH AND YAW DEDUCED FROM
RELATIVE GIMBAL POSITION
GEOMETRIC SYSTEM
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Strapdown Technology:
Body fixed
3 Accelerometers
3 Gyroscopes
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Strapdown Technology:
The measurement principle
SENSORS FASTENED DIRECTLY ON THE VEHICLE
BODY FIXED COORDINATE SYSTEM
ANALYTIC SYSTEM
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
iee  15.041 / h
Reference Frames:
i-frame
e-frame
n-frame
b-frame
Also normed: WGS 84
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Interlude: relative kinematics
Vehicle‘s acceleration in inertial axes (1.Newton):
Moving system: e
P = CoM
d
d2
i vp 
i rOP  i f  i g  Aie ( e f  e g )
dt
dt 2
P
Problem: All quantities are obtained in vehicle’s frame (local)
Euler Derivatives!
O
Differentiation:
Inertial system: i
 d2

d
d2
d
v

r

A
r



r

2


r





r
i p
i OP
ie 
e ie
e OP
e ie
e ie
e OP 
e ie e OP
2 e OP
dt
dt 2
dt
dt


trans
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
rot
cor
cent
Basic Principles
Frame Mechanisation I:
i-Frame
Vehicle‘s velocity (ground speed) and Coriolis Equation:
d
d
r  r  ie  r
dt (i ) dt ( e )
Differentiation:
abbreviated:
d
r  ve
dt ( e )
Applying Coriolis Equation (earth‘s turn rate is constant):
d2
d
d
r  ve  ie  r 
2
dt (i ) dt ( i ) dt
(i )
d2
d
r

ve  ie  ve  ie  ie  r 
dt 2 (i ) dt (i )
subscipt: with respect to; superscript: denotes the axis set; slash: resolved in axis set
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Frame Mechanisation II:
i-Frame
Newton’s 2nd axiom:
d2
v
 f  ie  ve  ie  ie  r   g
2 e
dt
(i )
Recombination:
d2
ve  f  ie  ve  gl
dt 2 ( i )
abbreviated:
gl  g  ie  ie  r 
i-frame axes:
Substitution:
vei  f i  iei  vei  gli
vei  Aib f b  iei  vei  gli
subscipt: with respect to; superscript: denotes the axis set; slash: resolved in axis set
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Frame Mechanisation III:
Implementation
POSITION
INFORMATION
GRAVITY
COMPUTER
gi
BODY
MOUNTED
ACCELEROMETERS
f
b
RESOLUTION
OF
SPECIFIC FORCE
MEASUREMENTS
f
CORIOLIS
CORRECTION
i


NAVIGATION
COMPUTER
Aib
BODY
MOUNTED
GYROSCOPES
ibb
ATTITUDE
COMPUTER
INITIAL ESTIMATES OF
ATTITUDE
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
vei
POSSIBILITY FOR
KALMAN FILTER
INSTALLATION
POSITION AND
VELOVITY
ESTIMATES
INITIAL ESTIMATES OF
VELOVITY AND POSITION
Basic Principles
Strapdown Attitude Representation:
Direction cosine matrix
Quaternions
No singularities, perfect for internal
computations
Euler angles
singularities, good physical appreciation
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Basic Principles
Strapdown Attitude Representation: Direction Cosine Matrix
 c11 c12

Anb   c21 c22
c
 31 c32
c13 

c23 
c33 
For Instance:
  n b 
c13  cos  n1 ; n 3 


Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Axis projection:
Simple Derivative:
Anb  Anb bnb
With skew symmetric matrix:
 0

bnb    z
  y

 z
0
x
y 

 x 
0 
Basic Principles
Strapdown Attitude Representation: Quaternions
Idea: Transformation is single rotation about one axis
cos  / 2

a 

  (  /  )sin  / 2 
b

p  x
 c   (  y /  )sin  / 2 

  
 d   (  z /  )sin  / 2 
p  a  ib  jc  kd
Components of angle Vector,
defined with respect to reference frame
x ,  y , z
Magnitude of rotation:
Operations analogous to 2 Parameter Complex number
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich

Basic Principles
Strapdown Attitude Representation: Euler Angles
Rotation about reference z axis through angle 
Rotation about new y axis through angle 
Rotation about new z axis through angle 
 cos  cos

Anb   cos  sin
  sin 

 cos  sin  sin  sin  cos
cos  sin  sin  sin  sin
sin cos 
T
Anb  Abn1  Abn
sin  sin  cos  sin  cos 

 sin  cos  cos  sin  sin 

cos  cos 

Singularity:   90
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Gimbal angle pick-off!
Sensor Technology
Accelerometers
Physical principles:
Potentiometric
LVDT (linear voltage differential transformer)
Piezoelectric
Newton’s 2nd axiom:
F  ma  mg
gravitational part: Compensation
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Accelerometers
Potentiometric
-
+
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Accelerometers
LVDT (linear voltage differential transformer)
Uses Induction
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Accelerometers
Piezoelectric
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Accelerometers
Servo principle (Force Feedback)
Intern closed loop feedback
Better linearity
Null seeking instead of displacement measurement
1 - seismic mass
2 - position sensing device
3 - servo mechanism
4 - damper
5 - case
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Gyroscopes
Vibratory Gyroscopes
Optical Gyroscopes
Historical definition:
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Gyroscopes: Vibratory Gyroscopes
Coriolis principle:
1. axis velocity caused by harmonic oscillation (piezoelectric)
2. axis rotation
3. axis acceleration measurement
Problems:
High noise
Temperature drifts
Translational acceleration
vibration
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Gyroscopes: Vibratory Gyroscopes
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Sensor Technology
Gyroscopes: Optical Gyroscopes
INTERFERENCE
DETECTOR
Sagnac Effect:
Super Luminiszenz Diode
Beam splitter
Fiber optic cable coil
Effective path length difference
Beam
splitter
LASER
 
8 A

c
MODULATOR
Beam
splitter

Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
The Kalman Filter – A stochastic filter method
Motivation:




Uncertainty of measurement
System noise
Bounding gyroscope’s drift (e.g. analytic systems)
Higher accuracy
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
The Kalman Filter – what is it?
Definition:
Optimal recursive data processing algorithm.
Optimal, can be any criteria that makes sense.
Combining information:



Knowledge of the system and measurement device dynamics
Statistical description of the systems noise, measurement errors and uncertainty in
the dynamic models
Any available information about the initial conditions of the variables of interest
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
The Kalman Filter – Modelization of noise
Deviation:
Bias: Offset in measurement provided by a sensor, caused by imperfections
Noise: disturbing value of large unspecific frequency range
Assumption in Modelization:
White Noise: Noise with constant amplitude (spectral density) on frequency domain (infinite energy);
zero mean
Gaussian (normally) distributed:
probability density function
1  x 
 
 
1
f ( x) 
e 2
 2
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
2
Kalman Filter
Basic Idea:
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Combination of independent estimates: stochastic Basics (1-D)
Mean value:
1 n
x  E ( x)   xi
n i 1
Variance:
1 n
 
 xi  x
n  1 i 1
Estimates:
x1 , x2
Mean of 2 Estimates
(with weighting factors):
2

x  w1 x1  w2 x2
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich

2
Kalman Filter
Combination of independent estimates: stochastic Basics (1-D)
Weighted mean:
Variance of weighted
mean:
Not correlated:
Variance of weighted
mean:

E x  w1E ( x1 )  w2 E ( x2 )


 2  E w12  x1  E ( x1 )   w22  x2  E ( x2 )   2w1w2  x1  E ( x1 )  x2  E ( x2 ) 
2
2
E  x1  E ( x1 )  x2  E ( x2 )   0



Quantiles are independent!

 2  w12 E  x1  E ( x1 )   w22 E  x2  E ( x2 )   w112  w2 22
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
2
2
Kalman Filter
Combination of independent estimates: stochastic Basics (1-D)
Weighting factors:
w1  w2  1
Substitution:
 2  (1  w)2  12  w2 22
Optimization
(Differentiation):
Optimum weight:
d 2
  2(1  w) 2  12  2 w2 22  0
dw
 12
w 2
 1   22
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Combination of independent estimates: stochastic Basics (1-D)
Mean value:
 22 x1   12 x2
x
 x1  w( x1  x2 )
 22   12
Variance:
 12 22
  2
  12 (1  w)
2
1   2
2
Multidimensional case:
Covariance matrix:

P  E ( x  x)( x  x)T
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich

Kalman Filter
Interlude: the covariance matrix
1-D: Variance – 2nd central moment
N-D: Covariance – diagonal elements are variances, off-diagonal elements encode the
correlations
Covariance of a vector:


cov( x)  P( x)  E x  x x  x
  P
T
xx
cov( x, y )  E ( x, y )  E ( x) E ( y )  Pxy
n x n matrix, which can be modal transformed, such that are only
diagonal elements with decoupled error contribution;
Symmetric and quadratic
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Interlude: the covariance matrix applied to equations
Equation structure:
z  Ax  By  c
x, y are gaussian distributed, c is constant:
Covariance of z:
Pzz  APxx AT  APxy BT  BPyx AT  BPyy BT
Linear difference equation:
x(ti 1 )  (ti 1 , ti ) x(ti )  B(ti )u (ti )  w(ti )
Covariance:
with:
P x(ti 1 )  (ti 1 , ti ) P x(ti ) (ti 1, ti )T  Qd

Qd  E w(ti ) w(ti )T
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich

Diagonal structure: since white gaussian noise
Kalman Filter
Combination of independent estimates: (n-D)
Mean value:
x  ( I  W ) x1  Wx2  x1  W ( x1  x2 )
measurement:
y2  Hx2
Mean value:
x  x1  KH ( x1  x2 )  x1  K ( Hx1  y2 )  (I  KH ) x1  Ky2
Covariance P  x   E (I  KH ) x  Ky
with:
W  KH
1
2
 ( I  KH ) E ( x1 )  KE ( x1 )  KE ( y2 )( I  KH ) x1  Ky2  ( I  KH ) E ( x1 )  KE ( y2 ) 
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
T

Kalman Filter
Combination of independent estimates: (n-D)




Covariance:
P  ( I  KH ) E  x1  E ( x1 ) x1  E ( x1 ) ( I  KH )T  KE  y2  E ( y2 ) y2  E ( y2 ) K T
Covariance:
P  ( I  KH ) P1 ( I  KH )T  KRK T
Minimisation of
Variance matrix‘s
Diagonal elements
(Kalman Gain):
K  P1 H T  HP1 H T  R 
T
1
For further information please also read:
P.S. Maybeck: ‘Stochastic Models, Estimation and Control Volume 1’,
Academic Press, New York San Francisco London
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
T
Kalman Filter
Combination of independent estimates: (n-D)
Mean value:
x  x1  K  Hx1  y2 
Variance:
P  P1  KHP1
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Interlude: time continuous system to discrete system
t
Continuous solution:
x(t )   e A(t  ) Bu ( )d  e A(t t0 ) x(t )
t0
Substitution:
u (t )  uk ; t     ; d  d
t
t  kT
kT
0
 (t  )d B  
( )d B  H (t  kT )
Conclusion:
x(t )  (t  kT ) x(kT )  H (t  kT )uk
Sampling time:
x((k  1)T )  (T ) x(kT )  H (T )uk
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
xk 1  xk  Huk
Kalman Filter
The Kalman Filter: Iteration Principle
PREDICTION OF ERROR
COVARIANCE BETWEEN
TWO ITERATIONS
CALCULATION OFKALMAN
GAIN (WEIGHTING OF
MEASUREMENT AND
PREDICTION)
PREDICTION OF STATES
(SOLUTION) BETWEEN
TWO ITERATIONS
DETERMINATION OF NEW
SOLUTION (ESTIMATION)
k 1  k
PREDICTION
NEXT
ITERATION
INITIAL ESTIMATION OF
STATES AND QUALITY OF
STATE
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
CORRECTION OF THE
STOCHASTIC MODELLS TO
NEW QUALITY VALUE OF
SOLUTION
CORRECTION
Kalman Filter
Linear Systems – the Kalman Filter:
Discrete State Model:
x(k  1)  x(k )  Bu (k )  w(k )
Sensor Model:
z (k )  Hx(k )  r (k )
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Linear Systems – the Kalman Filter: 1. Step Prediction
Prediction:
x(k  1| k )   x(k | k )  Bu (k )
State Prediction Covariance:
P(k  1| k )  P(k | k )T  Q
Observation Prediction:
z (k  1| k )  H x(k  1| k )
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Linear Systems – the Kalman Filter: 2. Step Correction
Corrected state estimate:
x(k  1| k  1)  x(k  1| k )  K (k  1)v(k  1)
Corrected State Covariance:
P(k  1| k  1)  P(k  1| k )  K (k  1)S (k  1) K T (k  1)
Innovation Covariance:
Innovation:
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
S (k  1)  H (k  1) P(k  1| k ) H T (k  1)  R(k  1)
v(k  1)  z (k  1)  z (k  1| k )
Kalman Filter
The Kalman Filter: Kalman Gain
Kalman Gain:
K (k  1) 
P(k  1| k ) H (k  1)
S (k  1)
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
T
State Prediction Covariance
Innovation Covariance
Kalman Filter
The Kalman Filter: System Model
u (k )
G
+
+
x(k | k )

x(k  1| k )
x(k  1| k )
H
z (k  1)
z (k  1| k )
-
x(k  1| k  1)
v ( k  1)
+
For linear systems: System matrices are timeinvariant
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
K (k  1)
+
+
Memory
x(k  1)  x(k )
Kalman Filter
Non-Linear Systems – the extended Kalman Filter:
x  f ( x, u )
Nonlinear dynamics equation:
Nonlinear observation equation:
Solution strategy: Linearize Problem around predicted state: (Taylor
Series tuncation)
x


f ( x, u ) ( x  x ) 
f ( x, u ) (u  u )
x

u
x ,u
x ,u
Error Deviation from Prediction state
Necessary for Kalman Gain and Covariance Calculation
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Non-Linear Systems – the extended Kalman Filter:
Prediction:
x(k  1| k )  f  k , x(k | k ) 
P(k  1| k )  f x (k ) P(k | k ) f xT (k )
Correction:
K (k  1)  P(k  1)hx (k  1) S 1 (k  1)
x(k  1| k  1)  x(k  1| k )  K (k  1)  z (k  1)  h k , x(k  1| k )  


P(k  1| k  1)  P(k  1| k )  K (k  1)S (k  1) K T (k  1)
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Example: Aiding the missile
MISSILE WITH ON-BOARD INERTIAL NAVIGATION SYSTEM (REPLACING THE PHYSICAL PROCESS
MODEL; 1 ESTIMATE) AND NAVIGATION AID (GROUND TRACKER MEASUREMENT; 2 ESTIMATE)
Missile
Motion
Measurement
Noise
True
Position
MISSILE
SURFACE SENSORS
Estimated INS
Error
INS
INS Indicated
Position
System
Noise
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
KALMAN GAINS
Measurement
Innovations

MEASUREMENT MODEL
Estimated Range,
Elevation and
Bearing
+
_
Kalman Filter
Example: Aiding the missile
Nine State Kalman Filter: 3 attitude, 3 velocity, 3 position errors
Bounding Gyroscope’s and accelerometers drifts by long term signal of surface sensor on
launch platform (complementary error characteristics)
Extended Kalman Filter:
around trajectory)
Attention: All Matrices are vector derivatives! Linearisation

Error Model:
 x  F x  Dw
Discrete Representation:
xk 1  k xk  wk
Attention: All Matrices are vector derivatives matrices!
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
(truncated Taylor series)
(System Equation)
Kalman Filter
Example: Aiding the missile
Measurement Equations with respect to radar, providing measurements in polar coordinates,
i.e. Range (R), elevation (  ) and bearing (  ).
Expressed in Cartesian coordinates (x,y,z):
R2  x2  y 2  z 2


  arctan 


z
x
2
y
2






Radar Measurements:
y
  arctan  
x
z   R, ,    z  v
T
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Example: Aiding the missile
Estimates of the radar measurements, z, obtained from the inertial navigation system:

2
2
2

x y z


R 

  

z
z       arctan 
2
2
  
 x y


  

 y 

arctan  

 x 




Innovation:






 


 




z  z  z  H x
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
(Measurement Equation)
Kalman Filter
Example: Aiding the missile
H-Matrix (Jacobian):

0 0 0 0 0 0


H  0 0 0 0 0 0


0 0 0 0 0 0


Best Estimate of the errors after update:
Covariance Prediction:
Initial setup: diagonal structure
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
x
R
y
R
xz
yz
R2 x2  y 2
R2 x2  y 2
y
x  y2
x
x  y2
2
2
xk 1/ k  0
Pk 1/ k   k Pk / k  k T  Q



2
2 
x y

R2



0


z
R
Kalman Filter
Example: Aiding the missile
Filter update:
Estimates of error:
xk 1/ k 1  K k 1zk 1
Covariance update:
(R measurement noise, diagonal structure)
Pk 1/ k 1   I  Kk 1Hk 1  Pk 1/ k
Pk 1/ k 1 H kT1
K k 1 
 H k 1Pk 1/ k H kT1  R 
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
Kalman Filter
Example: Aiding the missile
Velocity and Position Correction:
Attitude Correction:
(direction cosine matrix)
x  x  x
C   I  C
 0

   3
 
2

Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich
 3
0
1
2 

1 
0 
thank you for your attention
Lecture: Position Measurement in Inertial Systems
Christian Wimmer
Technical University of Munich