Title: Lecture: Position Measurement in Inertial Systems
1Mechatronics - Foundations and ApplicationsPositi
on Measurement in Inertial Systems
- JASS 2006, St.Petersburg
- Christian Wimmer
2Content
- Motivation
- Basic principles of position measurement
- Sensor technology
- Improvement Kalman filtering
3Motivation
- Johnnie A biped walking machine
-
- Orientation
- Stabilization
- Navigation
4Motivation
- Automotive Applications
-
- Drive dynamics Analysis
- Analysis of test route topology
- Driver assistance systems
5Motivation
- Aeronautics and Space Industry
-
- Autopilot systems
- Helicopters
- Airplane
- Space Shuttle
6Motivation
- Military Applications
-
- ICBM, CM
- Drones (UAV)
- Torpedoes
- Jets
7Motivation
- Maritime Systems
-
- Helicopter Platforms
- Naval Navigation
- Submarines
8Motivation
- Industrial robotic Systems
-
- Maintenance
- Production
9Basic Principles
- Measurement by inertia and integration
- Acceleration
- Velocity
- Position
- Measurement system with 3 sensitive axes
- 3 Accelerometers
- 3 Gyroscope
Newtons 2. Axiom F m x a BASIC PRINCIPLE OF
DYNAMICS
10Basic 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
11Basic Principles
- Strapdown Technology
-
- Body fixed
- 3 Accelerometers
- 3 Gyroscopes
12Basic Principles
- Strapdown Technology
-
- The measurement principle
SENSORS FASTENED DIRECTLY ON THE VEHICLE BODY
FIXED COORDINATE SYSTEM ANALYTIC SYSTEM
13Basic Principles
- Reference Frames
-
- i-frame
- e-frame
- n-frame
- b-frame
- Also normed WGS 84
14Basic Principles
Interlude relative kinematics
Moving system e P CoM
Vehicles acceleration in inertial axes
(1.Newton) Problem All quantities are
obtained in vehicles frame (local) Euler
Derivatives!
P
O
Differentiation
Inertial system i
cent
trans
cor
rot
15Basic Principles
Frame Mechanisation I i-Frame Vehicles
velocity (ground speed) and Coriolis Equation
abbreviated Differentiation
Applying Coriolis Equation (earths turn
rate is constant) subscipt with respect to
superscript denotes the axis set slash
resolved in axis set
16Basic Principles
Frame Mechanisation II i-Frame Newtons
2nd axiom
abbreviated Recombination
i-frame axes
Substitution subscipt with respect
to superscript denotes the axis set slash
resolved in axis set
17Basic Principles
Frame Mechanisation III Implementation
POSITION INFORMATION
GRAVITY COMPUTER
CORIOLIS CORRECTION
RESOLUTION OF SPECIFIC FORCE MEASUREMENTS
BODY MOUNTED ACCELEROMETERS
NAVIGATION COMPUTER
POSITION AND VELOVITY ESTIMATES
POSSIBILITY FOR KALMAN FILTER INSTALLATION
BODY MOUNTED GYROSCOPES
ATTITUDE COMPUTER
INITIAL ESTIMATES OF ATTITUDE
INITIAL ESTIMATES OF VELOVITY AND POSITION
18Basic Principles
- Strapdown Attitude Representation
- Direction cosine matrix
- Quaternions
- Euler angles
No singularities, perfect for internal
computations
singularities, good physical appreciation
19Basic Principles
Strapdown Attitude Representation Direction
Cosine Matrix
Simple Derivative
Axis projection
For Instance
With skew symmetric matrix
20Basic Principles
Strapdown Attitude Representation Quaternions
Idea Transformation is single rotation about one
axis
Components of angle Vector, defined with respect
to reference frame
Magnitude of rotation
Operations analogous to 2 Parameter Complex number
21Basic 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
Gimbal angle pick-off!
Singularity
22Sensor Technology
- Accelerometers
- Physical principles
- Potentiometric
- LVDT (linear voltage differential transformer)
- Piezoelectric
Newtons 2nd axiom
gravitational part Compensation
23Sensor Technology
Accelerometers Potentiometric
-
24Sensor Technology
- Accelerometers
- LVDT (linear voltage differential transformer)
- Uses Induction
25Sensor Technology
Accelerometers Piezoelectric
26Sensor 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
27Sensor Technology
- Gyroscopes
- Vibratory Gyroscopes
- Optical Gyroscopes
Historical definition
28Sensor 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
29Sensor Technology
Gyroscopes Vibratory Gyroscopes
30Sensor Technology
- Gyroscopes Optical Gyroscopes
- Sagnac Effect
- Super Luminiszenz Diode
- Beam splitter
- Fiber optic cable coil
- Effective path length difference
INTERFERENCE DETECTOR
Beam splitter
LASER
MODULATOR
Beam splitter
31Kalman Filter
- The Kalman Filter A stochastic filter method
- Motivation
- Uncertainty of measurement
- System noise
- Bounding gyroscopes drift (e.g. analytic
systems) - Higher accuracy
32Kalman 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
33Kalman 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
34Kalman Filter
Basic Idea
35Kalman Filter
Combination of independent estimates stochastic
Basics (1-D) Mean value Variance Estimates
Mean of 2 Estimates (with weighting factors)
36Kalman Filter
Combination of independent estimates stochastic
Basics (1-D) Weighted mean Variance of
weighted mean Not correlated Variance of
weighted mean
Quantiles are independent!
37Kalman Filter
Combination of independent estimates stochastic
Basics (1-D) Weighting factors Substitution
Optimization (Differentiation) Optimum
weight
38Kalman Filter
Combination of independent estimates stochastic
Basics (1-D) Mean value Variance Multidimen
sional case Covariance matrix
39Kalman 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 n x n matrix, which can be modal
transformed, such that are only diagonal elements
with decoupled error contribution Symmetric and
quadratic
40Kalman Filter
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
Diagonal structure since white gaussian noise
41Kalman Filter
Combination of independent estimates
(n-D) Mean value measurement Mean
value Covariance with
42Kalman Filter
Combination of independent estimates
(n-D) Covariance Covariance Minimisation
of Variance matrixs Diagonal elements (Kalman
Gain)
For further information please also read P.S.
Maybeck Stochastic Models, Estimation and
Control Volume 1, Academic Press, New York San
Francisco London
43Kalman Filter
Combination of independent estimates
(n-D) Mean value Variance
44Kalman Filter
Interlude time continuous system to discrete
system Continuous solution Substitution
Conclusion Sampling time
45Kalman Filter
The Kalman Filter Iteration Principle
CALCULATION OFKALMAN GAIN (WEIGHTING OF
MEASUREMENT AND PREDICTION)
PREDICTION OF ERROR COVARIANCE BETWEEN TWO
ITERATIONS
PREDICTION OF STATES (SOLUTION) BETWEEN TWO
ITERATIONS
DETERMINATION OF NEW SOLUTION (ESTIMATION)
CORRECTION OF THE STOCHASTIC MODELLS TO NEW
QUALITY VALUE OF SOLUTION
PREDICTION
NEXT ITERATION
CORRECTION
INITIAL ESTIMATION OF STATES AND QUALITY OF STATE
46Kalman Filter
Linear Systems the Kalman Filter Discrete
State Model Sensor Model
47Kalman Filter
Linear Systems the Kalman Filter 1. Step
Prediction Prediction State Prediction
Covariance Observation Prediction
48Kalman Filter
Linear Systems the Kalman Filter 2. Step
Correction Corrected state estimate Corrected
State Covariance Innovation
Covariance Innovation
49Kalman Filter
The Kalman Filter Kalman Gain Kalman Gain
State Prediction Covariance Innovation Covariance
50Kalman Filter
The Kalman Filter System Model
-
Memory
For linear systems System matrices are
timeinvariant
51Kalman Filter
Non-Linear Systems the extended Kalman
Filter Nonlinear dynamics equation Nonlinear
observation equation Solution strategy
Linearize Problem around predicted state (Taylor
Series tuncation)
Error Deviation from Prediction state Necessary
for Kalman Gain and Covariance Calculation
52Kalman Filter
Non-Linear Systems the extended Kalman
Filter Prediction Correction
53Kalman 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)
Measurement Noise
Missile Motion
True Position
MISSILE
SURFACE SENSORS
Measurement Innovations
Estimated INS Error
_
KALMAN GAINS
INS Indicated Position
INS
MEASUREMENT MODEL
Estimated Range, Elevation and Bearing
System Noise
54Kalman Filter
Example Aiding the missile Nine State Kalman
Filter 3 attitude, 3 velocity, 3 position
errors Bounding Gyroscopes 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) Attentio
n All Matrices are vector derivatives matrices!
55Kalman 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) Radar Measurements
56Kalman Filter
Example Aiding the missile Estimates of the
radar measurements, z, obtained from the inertial
navigation system Innovation
(Measurement Equation)
57Kalman Filter
Example Aiding the missile H-Matrix
(Jacobian) Best Estimate of the errors
after update Covariance Prediction Initial
setup diagonal structure
58Kalman Filter
Example Aiding the missile Filter
update Estimates of error Covariance
update (R measurement noise, diagonal structure)
59Kalman Filter
Example Aiding the missile Velocity and
Position Correction Attitude
Correction (direction cosine matrix)
60thank you for your attention