Mechatronics - Foundations and Applications Position Measurement in Inertial Systems

transcript

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( )i p i OP i i ie e e

d dv r f g A f g

Interlude: relative kinematics

Differentiation:

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

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

d dr r r

2( ) ( )( )

e iei ii

d d dr v r

dt dt dt

2( )( )

e ie e ie ieii

d dr v v r

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

e ie e ie ie

dv f v r g

dt l ie ieg g r

e ie e l

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

POSITION ANDVELOVITY ESTIMATES

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

A c c c

1 313 cos ;n b

For Instance:

Simple Derivative:Axis projection:

bnb nb nbA A

z ybnb z x

With skew symmetric matrix:

Basic Principles

Strapdown Attitude Representation: Quaternions

Idea: Transformation is single rotation about one axis

cos / 2

( / )sin / 2

, ,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

INTERFERENCEDETECTOR

MODULATOR

Beamsplitter

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

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

x E 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

2 21 2

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

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

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

Kalman Filter

Covariance:

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

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:

( )( )( ) ( ) ( )t

A t tA t

x t e Bu d e x t

( ) ; ;ku t u t d d

( ) ( ) ( )t t 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

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

State Prediction Covariance

Innovation Covariance

Kalman Filter

( )u k ( | )x k k

( 1| 1)x k k

The Kalman Filter: System Model

( 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

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

arctan

z R z v

Kalman Filter

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

Innovation: (Measurement Equation)

2 2arctan

arctan

z z z H x

Kalman Filter

H-Matrix (Jacobian):

Best Estimate of the errors after update:

Covariance Prediction:Initial setup: diagonal structure

22 2 2 2 2 2

2 2 2 2

0 0 0 0 0 0

0 0 0 0 0 0 0

x yxz yzH

RR x y R x y

x y x y

1/ 0k kx

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

H P H R

Kalman Filter

Velocity and Position Correction:

Attitude Correction:(direction cosine matrix)

thank you for your attention

Mechatronics - Foundations and Applications Position Measurement in Inertial Systems

Documents