Examples of Estimation Filters from Recent Aircraft Projects at MIT November 2004 Sanghyuk Park and Jonathan How Vehicles & Navigation Sensors OHS (Outboard Horizontal Stabilizer) Navigation Sensors (Piccolo from Cloudcap Tech) ? GPS Motorola M12 ? Inertial ? 3 Tokin CG-16D rate gyros ? 3 ADXL202 accelerometers Navigation Sensors ?Air Data ? GPS Receiver (Marconi, Allstar) ? Dynamic & absolute pressure sensor ? Inertial Sensors ? Air temperature sensor - Crossbow 3-axis Accelerometer, ? MHX 910/2400 radio modem Tokin Ceramic Gyro (MINI) or ? MPC555 CPU Crossbow IMU (OHS) ? Pitot Static Probe: measures ? Crista Inertial Measurement Unit airspeed ? 3 Analog Devices ADXL accelerometers ? Altitude Pressure Sensor ? 3 ADXRS MEMs rate sensors 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 ! Example : Tilt angle estimation using accelerometer and rate gyro ≈ ∫ rate)(angular dt - not good in long term due to integration outputaccel. ? ? ? + τ τ ? ? ? s 1 examplefor, s = est θ accelerometer rate gyro High Pass Filter ?? θ θ 1 g - not proper during fast motion ? ? ? τ = ? ? ? 1 s+ ? sin 1 - only good in long term Low Pass Filter ? ? ? ? ? ? ≈ θ Complementary Filter(CF) Examples ? CF1. Roll Angle Estimation ? CF2. Pitch Angle Estimation ? CF3. Altitude Estimation ? CF4. Altitude Rate Estimation CF1. Roll Angle Estimation ? High freq. : integrating roll rate (p) gyro output ? Low freq. : using aircraft kinematics - Assuming steady state turn dynamics, roll angle is related with turning rate, which is close to yaw rate (r) L sin φ = mV? L ≈ mg V φ ≈ r g ≈ ? r sinφ ≈φ Roll CF setup Rate Gyro Yaw Rate Gyro 1 s HPF LPF V g + + Roll angle estimate p r φ CF2. Pitch Angle Estimation ? High freq. : integrating pitch rate (q) gyro output ? Low freq. : using the sensitivity of accelerometers to gravity direction -“gravity aiding” In steady state A X = g sin θ ? x θ = tan 1 ? ? ? ? A ? ? A Z ? = g cosθ ? A z ? ? A X , A Z ? outputs ter accelerome ? Roll angle compensation is needed CF setup q meas  ≈ q meas cosφθ est θ φ est est + + s 1 HPF A A x ? ? ? x θ = tan 1 ? ? ? A cosφ est ? ? ssz φ est ? A z ? LPF CF3. Altitude Estimation ? Motivation : GPS receiver gives altitude output, but it has ~0.4 seconds of delay. In order of overcome this, pressure sensor was added. ? Low freq. : from GPS receiver ? High freq. : from pressure sensor CF setup & flight data h Sensor Pressure from LPF HPF + + h h KF GPS from est CF4. Altitude Rate Estimation ? Motivation : GPS receiver gives altitude rate, but it has ~0.4 seconds of delay. In order of overcome this, inertial sensor outputs were added. ? Low freq. : from GPS receiver ? High freq. : integrating acceleration estimate in altitude direction from inertial sensors CF setup a z Angular Transform a h s 1 HPF a est , est θ φ + +  h est y a x LPF  h KF GPS from A x 0 ? ? ? ?? ? a x ? ? ? ? = ? ? ? ? ? ? ? ? ? φ[ est θ[ ] est ? ? ] ? ? A A x z ? outputs ter accelerome , A y A z 0: note a y ]φ[ est , θ[ est ] matrices tion transforma angular : ? ? ? ? ? ? ? ? ? g a z Kalman Filter(KF) Examples ? KF1. Manipulation of GPS Outputs ? KF2. Removing Rate Gyro Bias Effect KF 1. Manipulation of GPS Outputs Background & Motivation ? Stand-alone GPS receiver gives position and velocity ? These are obtained by independent methods : ? position ? pseudo-ranges ? velocity ? Doppler effect and are certainly related (x  = v) ? Kalman filter can be used to combine them ! ? Motivation : Typical Accuracies Position ~ 30 m Velocity ~ 0.15 m/s Many GPS receivers provide high quality velocity information ? Use high quality velocity measurement to improve position estimate KF 1. Kalman Filter Setup x 2 ν += vv meas av dt d = 1 ω= j dt d Measurements Filter Dynamics 1 ν += xx meas ja dt d = vx dt d = North East Down est x meas v est v meas a est a v j ii ων , x : velocity : acceleration : jerk : position : white noises :a est ? noisy, but not biased ? combined with rate gyros in removing the gyro biases (KF2) KF 2. Removing Rate Gyro Bias Effect Background & Motivation ? In aircraft control, roll angle control is commonly used in inner-loop to create required lateral acceleration which is commanded from guidance outer-loop ? Biased roll angle estimate can cause steady-state error in cross-track 1 s HPF LPF V g Roll Rate Gyro Yaw Rate Gyro + + Roll angle estimate p r φ Drawback : biased estimate Complementary filter with roll & raw gyros (CF1) Single-Antenna GPS Based Aircraft Attitude Determination - Richard Kornfeld, Ph.D. (1999) Drawback : sampling rate limit (GPS), typical filter time constant ~ 0.5 sec. rVga s ?≈?≈ φ p≈φ  KF 2. Kalman Filter Setup s a V p ii ων , r φ : velocity : roll rate : yaw rate : bank angle : white noises 2 ν++= pmeas biaspp 2 ω=p dt d 3 ω= p bias dt d Measurement Equations Filter Dynamics meas p ( ) est p bias () est s a 1 νφ += ga s 3 νφ ++= rmeas bias V g r meas r 4 ω= r bias dt d 1 ωφ += p dt d est p ( ) est r bias est φ : acceleration in sideways direction from Rate Gyros from GPS Kalman Filter KF 2. Simulation Result ? Simulation for 10 degree bank angle hold ? Roll rate gyro bias=0.03 rad/s, yaw rate gyro bias = 0.02 rad/s were used in simulation References ? Applied Optimal Estimation Edited by Arthur Gelb, MIT Press, 1974 ? Fundamentals of Kalman Filtering – A Practical Approach Paul Zarchan & Howard Musoff, Progress in Astronautics and Aeronautics Vol. 190 ? Avionics and Control System Development for Mid-Air Rendezvous of Two Unmanned Aerial Vehicles Sanghyuk Park, Ph.D. Thesis, MIT, Feb. 2004 ? Fundamentals of High Accuracy Inertial Navigation Averil Chatfield, Progress in Astronautics and Aeronautics Vol. 174 ? Applied Mathematics in Integrated Navigation Systems R. Rogers, AIAA Education Series, 2000 ? The Impact of GPS Velocity Based Flight Control on Flight Instrumentation Architecture Richard Kornfeld, Ph.D. Thesis, MIT, Jun. 1999 ? Autonomous Aerobatic Maneuvering of Miniature Helicopters Valdislav Gavrilets, Ph.D. Thesis, MIT, May 2003