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