Lecture: Position Measurement in Inertial Systems Christian Wimmer Technical University of Munich Mechatronics - Foundations and Applications Position

Lecture: Position Measurement in Inertial SystemsChristian WimmerTechnical University of Munich

Mechatronics - Foundations and Applications

Position Measurement in Inertial Systems

JASS 2006, St.Petersburg

Christian Wimmer


Content

1. Motivation

2. Basic principles of position measurement

3. Sensor technology

4. Improvement: Kalman filtering


Motivation

Johnnie: A biped walking machine OrientationStabilizationNavigation


Motivation

Automotive Applications: Drive dynamics AnalysisAnalysis of test route topologyDriver assistance systems


Motivation

Aeronautics and Space Industry: Autopilot systemsHelicoptersAirplaneSpace Shuttle


MotivationMilitary Applications: ICBM, CMDrones (UAV)TorpedoesJets


Motivation

Maritime Systems: Helicopter PlatformsNaval NavigationSubmarines


Motivation

Industrial robotic Systems: MaintenanceProduction


Basic Principles

Measurement by inertia and integration:

AccelerationVelocityPosition

Newton‘s 2. Axiom:

F = m x a BASIC PRINCIPLE OF DYNAMICS

Measurement system with 3 sensitive axes

3 Accelerometers3 Gyroscope


Basic Principles

Gimballed Platform Technology: 3 accelerometers3 gyroscopescardanic Platform

ISOLATED FROM ROTATIONAL MOTIONTORQUE MOTORS TO MAINTAINE DIRECTIONROLL, PITCH AND YAW DEDUCED FROMRELATIVE GIMBAL POSITIONGEOMETRIC SYSTEM


Basic Principles

Strapdown Technology: Body fixed3 Accelerometers3 Gyroscopes


Basic Principles

Strapdown Technology: The measurement principle

SENSORS FASTENED DIRECTLY ON THE VEHICLEBODY FIXED COORDINATE SYSTEMANALYTIC SYSTEM


Basic Principles

Reference Frames: i-framee-framen-frameb-frame

Also normed: WGS 84

15.041 /eie h


Vehicle‘s acceleration in inertial axes (1.Newton):

Problem: All quantities are obtained in vehicle’s frame (local)Euler Derivatives!

Basic Principles

2

2( )i p i OP i i ie e e

d dv r f g A f g

dt dt

Interlude: relative kinematics

Differentiation:

2 2

2 22i p i OP ie e OP ie e OP e ie e OP e ie e ie e OPe

d d d dv r A r r r r

dt dt dt dt

trans corrot cent

Inertial system: i

Moving system: eP = CoM

O

P


Frame Mechanisation I: i-Frame

Vehicle‘s velocity (ground speed) and Coriolis Equation:

abbreviated:

Differentiation: Applying Coriolis Equation (earth‘s turn rate is constant):

subscipt: with respect to; superscript: denotes the axis set; slash: resolved in axis set

Basic Principles

( ) ( )ie

i e

d dr r r

dt dt

( )e

e

dr v

dt

2

2( ) ( )( )

e iei ii

d d dr v r

dt dt dt

2

2( )( )

e ie e ie ieii

d dr v v r

dt dt


Frame Mechanisation II: i-Frame

Newton’s 2nd axiom:

abbreviated:

Recombination: i-frame axes: Substitution:

subscipt: with respect to; superscript: denotes the axis set; slash: resolved in axis set

Basic Principles

2

2

( )

e ie e ie ie

i

dv f v r g

dt l ie ieg g r

2

2

( )

e ie e l

i

dv f v g

dt i i i i i

e ie e lv f v g

i b i i ie ib ie e lv A f v g


Basic Principles

Frame Mechanisation III: Implementation

BODYMOUNTEDGYROSCOPES

ATTITUDECOMPUTER

RESOLUTION OF SPECIFIC FORCEMEASUREMENTS

BODYMOUNTEDACCELEROMETERS NAVIGATION

COMPUTER

CORIOLISCORRECTION

GRAVITYCOMPUTER

INITIAL ESTIMATES OFVELOVITY AND POSITION

INITIAL ESTIMATES OFATTITUDE

POSITIONINFORMATION

bib

ibA

bf if

ig

POSITION ANDVELOVITY ESTIMATES

iev

POSSIBILITY FOR KALMAN FILTER INSTALLATION


Basic Principles

Strapdown Attitude Representation:

Direction cosine matrix

Quaternions

Euler angles

No singularities, perfect for internal computations

singularities, good physical appreciation


Basic Principles

Strapdown Attitude Representation: Direction Cosine Matrix

11 12 13

21 22 23

31 32 33

nb

c c c

A c c c

c c c

1 313 cos ;n b

c n n

For Instance:

Simple Derivative:Axis projection:

bnb nb nbA A

0

0

0

z ybnb z x

y x

With skew symmetric matrix:


Basic Principles

Strapdown Attitude Representation: Quaternions

Idea: Transformation is single rotation about one axis

cos / 2

( / )sin / 2

( / )sin / 2

( / )sin / 2

x

y

z

a

bp

c

d

, ,x y z

Components of angle Vector, defined with respect to reference frame

Magnitude of rotation:

Operations analogous to 2 Parameter Complex number

p a ib jc kd


Basic Principles

Strapdown Attitude Representation: Euler Angles

Rotation about reference z axis through angleRotation about new y axis through angleRotation about new z axis through angle

cos cos cos sin sin sin cos sin sin cos sin cos

cos sin cos sin sin sin sin sin cos cos sin sin

sin sin cos cos cosnbA

1 Tnb bn bnA A A 90 Singularity:

Gimbal angle pick-off!


Sensor Technology

Accelerometers

Physical principles:

PotentiometricLVDT (linear voltage differential transformer) Piezoelectric

F ma mg Newton’s 2nd axiom:

gravitational part: Compensation


Sensor Technology

Accelerometers

Potentiometric

+

-


Sensor Technology

Accelerometers

LVDT (linear voltage differential transformer)

Uses Induction


Sensor Technology

Accelerometers

Piezoelectric


Sensor Technology

Accelerometers

Servo principle (Force Feedback)

Intern closed loop feedbackBetter linearityNull seeking instead of displacement measurement

1 - seismic mass2 - position sensing device3 - servo mechanism4 - damper5 - case


Sensor Technology

Gyroscopes

Vibratory GyroscopesOptical Gyroscopes

Historical definition:


Sensor Technology

Gyroscopes: Vibratory Gyroscopes

Coriolis principle: 1. axis velocity caused by harmonic oscillation (piezoelectric)2. axis rotation3. axis acceleration measurement

Problems:High noiseTemperature driftsTranslational accelerationvibration


Sensor Technology

Gyroscopes: Vibratory Gyroscopes


Sensor Technology

Gyroscopes: Optical Gyroscopes

Sagnac Effect:

Super Luminiszenz DiodeBeam splitterFiber optic cable coilEffective path length difference

LASER

INTERFERENCEDETECTOR

MODULATOR

Beamsplitter

Beamsplitter

8 A

c


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


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


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 2

1

21( )

2

x

f x e


Kalman Filter

Basic Idea:


Kalman Filter

Combination of independent estimates: stochastic Basics (1-D)

Mean value:

Variance:

Estimates:

Mean of 2 Estimates(with weighting factors):

1

1( )

n

ii

x E x xn

2

2

1

1

1

n

ii

x xn

1 2,x x

1 1 2 2x w x w x


Kalman Filter


Weighted mean:

Variance of weightedmean:

Not correlated:

Variance of weighted mean:

1 1 2 2( ) ( )E x w E x w E x

2 22 2 21 1 1 2 2 2 1 2 1 1 2 2( ) ( ) 2 ( ) ( )E w x E x w x E x w w x E x x E x

1 1 2 2( ) ( ) 0E x E x x E x

2 22 2 2 2 21 1 1 2 2 2 1 1 2 2( ) ( )w E x E x w E x E x w w

Quantiles are independent!


Kalman Filter


Weighting factors:

Substitution:

Optimization (Differentiation):

Optimum weight:

1 2 1w w

2 2 2 2 21 2(1 )w w

2 2 2 2 21 22(1 ) 2 0

dw w

dw

21

2 21 2

w


Kalman Filter


Mean value:

Variance:

Multidimensional case:

Covariance matrix:

2 22 1 1 2

1 1 22 22 1

( )x x

x x w x x

2 22 21 2

12 21 2

(1 )w

( )( )TP E x x x x


Kalman Filter

Interlude: the covariance matrix

1-D: Variance – 2nd central momentN-D: Covariance – diagonal elements are variances, off-diagonal elements encode the correlations

Covariance of a vector:

n x n matrix, which can be modal transformed, such that are only diagonal elements with decoupled error contribution;Symmetric and quadratic

cov( ) ( )T

xxx P x E x x x x P

cov( , ) ( , ) ( ) ( ) xyx y E x y E x E y P


Interlude: the covariance matrix applied to equations

Equation structure:

x, y are gaussian distributed, c is constant:

Covariance of z:

Linear difference equation:

Covariance:

with:

Kalman Filter

z Ax By c

T T T Tzz xx xy yx yyP AP A AP B BP A BP B

1 1( ) ( , ) ( ) ( ) ( ) ( )i i i i i i ix t t t x t B t u t w t

1 1 1( ) ( , ) ( ) ( , )Ti i i i i i dP x t t t P x t t t Q

( ) ( )Td i iQ E w t w t Diagonal structure: since white gaussian noise


Kalman Filter

Combination of independent estimates: (n-D)

Mean value:

measurement:

Mean value:

Covariancewith:

1 2 1 1 2( ) ( )x I W x Wx x W x x

2 2y Hx

1 1 2 1 1 2 1 2( ) ( ) ( )x x KH x x x K Hx y I KH x Ky

1 2 1 1 2 1 2 1 2( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )T

P x E I KH x Ky I KH E x KE x KE y I KH x Ky I KH E x KE y

W KH


Kalman Filter


Covariance:

Covariance:

Minimisation of Variance matrix‘sDiagonal elements (Kalman Gain):

1

1 1T TK PH HPH R

1 1 1 1 2 2 2 2( ) ( ) ( ) ( ) ( ) ( )T TT TP I KH E x E x x E x I KH KE y E y y E y K

1( ) ( )T TP I KH P I KH KRK

For further information please also read:P.S. Maybeck: ‘Stochastic Models, Estimation and Control Volume 1’,Academic Press, New York San Francisco London


Kalman Filter


Mean value:

Variance:

1 1 2x x K Hx y

1 1P P KHP


Kalman Filter

Interlude: time continuous system to discrete system

Continuous solution:

Substitution:

Conclusion:

Sampling time:

0

0

( )( )( ) ( ) ( )t

A t tA t

t

x t e Bu d e x t

( ) ; ;ku t u t d d

0

( ) ( ) ( )t t kT

kT

t d B d B H t kT

1k k kx x Hu

( ) ( ) ( ) ( ) kx t t kT x kT H t kT u

(( 1) ) ( ) ( ) ( ) kx k T T x kT H T u


Kalman Filter

The Kalman Filter: Iteration Principle

INITIAL ESTIMATION OF STATES AND QUALITY OF STATE

PREDICTION OF STATES (SOLUTION) BETWEEN TWO ITERATIONS

PREDICTION OF ERROR COVARIANCE BETWEEN TWO ITERATIONS

CALCULATION OFKALMAN GAIN (WEIGHTING OF MEASUREMENT AND PREDICTION)

DETERMINATION OF NEW SOLUTION (ESTIMATION)

CORRECTION OF THE STOCHASTIC MODELLS TO NEW QUALITY VALUE OF SOLUTIONPREDICTION

CORRECTION

NEXT ITERATION

1k k


Kalman Filter

Linear Systems – the Kalman Filter:

Discrete State Model:

Sensor Model:

( 1) ( ) ( ) ( )x k x k Bu k w k

( ) ( ) ( )z k Hx k r k


Kalman Filter

Linear Systems – the Kalman Filter: 1. Step Prediction

Prediction:

State Prediction Covariance:

Observation Prediction:

( 1| ) ( | ) TP k k P k k Q

( 1| ) ( | ) ( )x k k x k k Bu k

( 1| ) ( 1| )z k k H x k k


Kalman Filter

Linear Systems – the Kalman Filter: 2. Step Correction

Corrected state estimate:

Corrected State Covariance:

Innovation Covariance:

Innovation:

( 1| 1) ( 1| ) ( 1) ( 1)x k k x k k K k v k

( 1| 1) ( 1| ) ( 1) ( 1) ( 1)TP k k P k k K k S k K k

( 1) ( 1) ( 1| ) ( 1) ( 1)TS k H k P k k H k R k

( 1) ( 1) ( 1| )v k z k z k k


Kalman Filter

The Kalman Filter: Kalman Gain

Kalman Gain:( 1| ) ( 1)

( 1)( 1)

TP k k H kK k

S k

State Prediction Covariance

Innovation Covariance


Kalman Filter

( )u k ( | )x k k

( 1| 1)x k k

The Kalman Filter: System Model

G

( 1) ( )x k x k

( 1)z k

( 1| )x k k

( 1| )z k k

( 1| )x k k

( 1)v k ( 1)K k

H

Memory+

-

+ +

++

For linear systems: System matrices are timeinvariant


Kalman Filter

Non-Linear Systems – the extended Kalman Filter:

Nonlinear dynamics equation:Nonlinear observation equation:

Solution strategy: Linearize Problem around predicted state: (Taylor Series tuncation)

( , )x f x u

, ,

( , ) ( ) ( , ) ( )x u x u

x f x u x x f x u u ux u

Error Deviation from Prediction stateNecessary for Kalman Gain and Covariance Calculation


Kalman Filter

Non-Linear Systems – the extended Kalman Filter:

Prediction:

Correction:

( 1| ) , ( | )x k k f k x k k

( 1| ) ( ) ( | ) ( )Tx xP k k f k P k k f k

1( 1) ( 1) ( 1) ( 1)xK k P k h k S k

( 1| 1) ( 1| ) ( 1) ( 1) , ( 1| )x k k x k k K k z k h k x k k ( 1| 1) ( 1| ) ( 1) ( 1) ( 1)TP k k P k k K k S k K k


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 SURFACE SENSORS

KALMAN GAINS

INS MEASUREMENT MODEL

Missile Motion

MeasurementNoise

True Position

Measurement Innovations

Estimated INS Error

System Noise

INS Indicated Position

Estimated Range, Elevation and Bearing

+

_


Kalman Filter


Nine State Kalman Filter: 3 attitude, 3 velocity, 3 position errorsBounding Gyroscope’s and accelerometers drifts by long term signal of surface sensor on launch platform (complementary error characteristics)

Extended Kalman Filter: Attention: All Matrices are vector derivatives! Linearisation around trajectory)

Error Model: (truncated Taylor series)

Discrete Representation: (System Equation)

Attention: All Matrices are vector derivatives matrices!

x F x Dw

1k k k kx x w


Kalman Filter


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):

Radar Measurements:

2 2 2 2R x y z

2 2arctan

z

x y

arctan

y

x

, ,T

z R z v


Kalman Filter


Estimates of the radar measurements, z, obtained from the inertial navigation system:

Innovation: (Measurement Equation)

2 2 2

2 2arctan

arctan

x y z

Rz

zx y

y

x

z z z H x


Kalman Filter


H-Matrix (Jacobian):

Best Estimate of the errors after update:

Covariance Prediction:Initial setup: diagonal structure

2 2

22 2 2 2 2 2

2 2 2 2

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0 0

x y z

R R R

x yxz yzH

RR x y R x y

y x

x y x y

1/ 0k kx

1/ /T

k k k k k kP P Q


Kalman Filter


Filter update:

Estimates of error:

Covariance update:(R measurement noise, diagonal structure)

1/ 1 1 1k k k kx K z

1/ 1 1 1 1/k k k k k kP I K H P

1/ 1 11

1 1/ 1

Tk k k

k Tk k k k

P HK

H P H R


Kalman Filter


Velocity and Position Correction:

Attitude Correction:(direction cosine matrix)

x x x

C I C

3 2

3 1

2 1

0

0

0


thank you for your attention

Documents

Lecture: Position Measurement in Inertial Systems Christian Wimmer Technical University of Munich Mechatronics - Foundations and Applications Position