Development of a INS/GPS navigation loop for an...
131
Sven Rönnbäck Developement of a INS/GPS navigation loop for an UAV 2000:081 MASTER'S THESIS Civilingenjörsprogrammet Institutionen för Systemteknik Avdelningen för Robotik och automation 2000:081 • ISSN: 1402-1617 • ISRN: LTU-EX--00/081--SE
Transcript of Development of a INS/GPS navigation loop for an...
Sven Rönnbäck
Developement of a INS/GPS navigation loop for an UAV
2000:081
MASTER'S THESIS
Civilingenjörsprogrammet
Institutionen för SystemteknikAvdelningen för Robotik och automation
2000:081 • ISSN: 1402-1617 • ISRN: LTU-EX--00/081--SE
MASTER'S THESIS
Developement of a INS/GPSnavigation loop
Sven Rönnbäck
February 2000
Luleå University of Technology
Department of Computer Science and ElectricalEngineering
Abstract
This Master Thesis report presents an INS1/GPS2 navigation �lter written inC++ using a standard matrix library called MPP. The �lter have been testedon an airvehicle called Brumby. Here data have been logged from the InertialMeasurement Unit (IMU) and the Global Positioning System (GPS) receiver.This data have then been postprocessed and run through the navigation �lterto estimate the position, attitude and velocity of the vehicle during �ights.
The �lter are feed with position, velocity and attitude observations. The �l-ter loop estimates the attitude within 2o and with 95% con�dence. The velocity+� 1m=s 95%, and the position +� 2m with 95% con�dence.
Some of the work have been done on a redundant ( four axes) IMU3 calledthe Tetrad.
Keywords: Unmanned Aerial Vehicle, Inertial Measuring Unit, Inertial Navi-gation System, Information Filter,Navigation,Realtime, Distributed, C++,Autonomous
1Inertial Navigation System2Global Positioning System3Inertial Measuring Unit
0.1 Acknowledgements
I am very grateful to Professor Hugh Durrant-Whyte who gave me the oppor-turnity to do my thesis work at the Department of Mechanical and MechatronicEngineering at Sydney University.
Thanks to Professor Åke Wernersson who have inspired me to work in the �eldof Robotics.
Thanks to Ph.D. student Ben Grosholsky at the Department of AeronauticalEngineering for good co-operation and support.
Thanks to my supervisor Ph.D. Salah Sukkarei for supporting me in my work.
Thanks to all my friends I meet in Sydney that made my visit very pleasant.
Thanks to Saint Andrews College for an interesting stay.
And at last I want to thank the mighty God for the existens of the Universe.
1
Contents
Abstract 1
0.1 Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . 10.2 List Of Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70.3 Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1 Introduction 11
1.1 Scope of this thesis . . . . . . . . . . . . . . . . . . . . . . . . . . 121.1.1 Picture of the thesis work . . . . . . . . . . . . . . . . . . 12
1.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121.2.1 The ANSER project . . . . . . . . . . . . . . . . . . . . . 141.2.2 The Brumby UAV airframe . . . . . . . . . . . . . . . . . 15
1.3 The Future of UAVs . . . . . . . . . . . . . . . . . . . . . . . . . 161.4 Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.4.1 Body Frame . . . . . . . . . . . . . . . . . . . . . . . . . . 171.4.2 Earth Surface North-East-Down (NED) Frame . . . . . . 171.4.3 WGS-84 , World Geodetic System 1984 . . . . . . . . . . 181.4.4 ECI-Earth Centered Interial Frame . . . . . . . . . . . . . 191.4.5 ECEF - Earth Centered Earth Fixed Frame . . . . . . . . 19
2 Attitude Representation 20
2.1 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212.1.1 Roll rotation,� . . . . . . . . . . . . . . . . . . . . . . . . 222.1.2 Pitch,� . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 232.1.3 Yaw,Heading . . . . . . . . . . . . . . . . . . . . . . . . 242.1.4 True attack and attack angle . . . . . . . . . . . . . . . . 252.1.5 Sideslip . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.2 Body Rotation Rate . . . . . . . . . . . . . . . . . . . . . . . . . 262.3 Cbn, Direction Cosine Representation . . . . . . . . . . . . . . . . 27
2.3.1 The Cbn matrix and small angles . . . . . . . . . . . . . . 282.3.2 Conversion to Eulerangles from Cbn represention . . . . . 282.3.3 Calculation of Eulerangles with large pitch angle . . . . . 292.3.4 The changerate of Cbn matrix . . . . . . . . . . . . . . . . 292.3.5 Cbn matrix integration in discrete time . . . . . . . . . . . 302.3.6 Very accurate Cbn matrix integration in discrete time . . 302.3.7 Normalization of Cbn matrix . . . . . . . . . . . . . . . . 30
2.4 Eulerangle Representation . . . . . . . . . . . . . . . . . . . . . . 302.4.1 Calculation of [ _�; _�; _ ] using [p,q,r] . . . . . . . . . . . . . 312.4.2 Eulerangle integration in a discrete time system . . . . . . 32
2
2.5 Quaternion Angle Representation . . . . . . . . . . . . . . . . . . 332.5.1 Calculation of Eulerangles using Quaterion angles . . . . 342.5.2 Quaternionangle calculation using Euler representation . 342.5.3 Calculate Quaternion using the Cbn matrix representation 352.5.4 Quaternion Norm . . . . . . . . . . . . . . . . . . . . . . . 352.5.5 Quaternationangle change rate . . . . . . . . . . . . . . . 352.5.6 Quaternionangle update in discrete time . . . . . . . . . . 36
3 Inertial Sensors, IMU and INS 37
3.1 Newtonian/Inertial Sensors . . . . . . . . . . . . . . . . . . . . . 383.1.1 Sensor Termal Compensation . . . . . . . . . . . . . . . . 383.1.2 Newtonian Sensor Nonlinearity . . . . . . . . . . . . . . . 38
3.2 Multi sensor unit . . . . . . . . . . . . . . . . . . . . . . . . . . . 393.2.1 The Accelerometer . . . . . . . . . . . . . . . . . . . . . . 393.2.2 The Rate Gyros . . . . . . . . . . . . . . . . . . . . . . . 403.2.3 Sensor Bias . . . . . . . . . . . . . . . . . . . . . . . . . . 403.2.4 Redundant Sensor Unit . . . . . . . . . . . . . . . . . . . 41
3.3 IMU-Inertial Measuring Unit . . . . . . . . . . . . . . . . . . . . 423.3.1 The Tetrad . . . . . . . . . . . . . . . . . . . . . . . . . . 433.3.2 Tetrad Commands . . . . . . . . . . . . . . . . . . . . . . 443.3.3 Tetrad IMU Fault Detection . . . . . . . . . . . . . . . . 45
3.4 INS-Inertial Navigation System . . . . . . . . . . . . . . . . . . . 453.4.1 Leveling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.5 Navigation Equations . . . . . . . . . . . . . . . . . . . . . . . . 47
4 Kalman and Information Filters 49
4.1 Linear System in continous time . . . . . . . . . . . . . . . . . . 504.2 Linear System in discrete time . . . . . . . . . . . . . . . . . . . 50
4.2.1 Calculation of discrete time matrixes . . . . . . . . . . . . 504.2.2 Discrete Input Covariance Matrix . . . . . . . . . . . . . . 51
4.3 Nonlinear System . . . . . . . . . . . . . . . . . . . . . . . . . . . 514.3.1 Linearized Nonlinear System in continous time . . . . . . 534.3.2 Linearized Nonlinear System in discrete time . . . . . . . 53
4.4 The Complement Filter . . . . . . . . . . . . . . . . . . . . . . . 534.4.1 Complement �lter with feedback . . . . . . . . . . . . . . 544.4.2 Complement �lter without feedback . . . . . . . . . . . . 54
4.5 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . 544.6 The Information Filter . . . . . . . . . . . . . . . . . . . . . . . . 56
4.6.1 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . 574.6.2 Observation . . . . . . . . . . . . . . . . . . . . . . . . . . 574.6.3 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 574.6.4 The Innovation . . . . . . . . . . . . . . . . . . . . . . . . 584.6.5 Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
4.7 A Linear Error Model . . . . . . . . . . . . . . . . . . . . . . . . 594.8 Nonlinear Model using Eulerangles . . . . . . . . . . . . . . . . . 60
4.8.1 State transition matrix . . . . . . . . . . . . . . . . . . . . 614.8.2 Control matrix . . . . . . . . . . . . . . . . . . . . . . . . 614.8.3 Observation Models . . . . . . . . . . . . . . . . . . . . . 62
4.9 Nonlinear Model with Quaternions . . . . . . . . . . . . . . . . . 634.9.1 State transition matrix . . . . . . . . . . . . . . . . . . . . 63
3
4.9.2 Control matrix . . . . . . . . . . . . . . . . . . . . . . . . 644.9.3 Observation Models . . . . . . . . . . . . . . . . . . . . . 64
5 GPS-Global Positioning System 66
5.1 GPS-Global Positioning System . . . . . . . . . . . . . . . . . . . 675.2 GPS-Signal errors . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.2.1 Multipath . . . . . . . . . . . . . . . . . . . . . . . . . . . 705.2.2 Signal attenuation . . . . . . . . . . . . . . . . . . . . . . 705.2.3 Satellite motion . . . . . . . . . . . . . . . . . . . . . . . . 705.2.4 Ionosphere propagation delay . . . . . . . . . . . . . . . . 705.2.5 Satellite clock error . . . . . . . . . . . . . . . . . . . . . . 715.2.6 Troposhpere propagation delay . . . . . . . . . . . . . . . 715.2.7 GPS Receiver Clock Model . . . . . . . . . . . . . . . . . 71
5.3 WGS-84 transformations . . . . . . . . . . . . . . . . . . . . . . . 725.3.1 ECEF to geodetic transformation . . . . . . . . . . . . . . 735.3.2 Geodetic to ECEF transformation . . . . . . . . . . . . . 735.3.3 ECEF vector to NED transformation . . . . . . . . . . . . 73
5.4 Position Determination . . . . . . . . . . . . . . . . . . . . . . . 745.4.1 Dilution Of Precition . . . . . . . . . . . . . . . . . . . . . 76
5.5 Velocity determination . . . . . . . . . . . . . . . . . . . . . . . . 775.6 Attitude determination . . . . . . . . . . . . . . . . . . . . . . . . 785.7 GPS interface software . . . . . . . . . . . . . . . . . . . . . . . . 795.8 Coupled INS/GPS . . . . . . . . . . . . . . . . . . . . . . . . . . 80
5.8.1 Uncoupled INS/GPS system . . . . . . . . . . . . . . . . 805.8.2 Loosley coupled INS/GPS system . . . . . . . . . . . . . 805.8.3 Tightly coupled INS/GPS system . . . . . . . . . . . . . . 80
6 The Implementation 82
6.1 Hungarian Notation . . . . . . . . . . . . . . . . . . . . . . . . . 836.1.1 C++ example using hungarian notation . . . . . . . . . . 83
6.2 The Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 846.2.1 The �ow of the software . . . . . . . . . . . . . . . . . . . 84
6.3 Classes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 846.4 For The IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
6.4.1 The Sensor Class . . . . . . . . . . . . . . . . . . . . . . . 846.4.2 The Sensor Cluster Class . . . . . . . . . . . . . . . . . . 85
6.5 The Readdata Class . . . . . . . . . . . . . . . . . . . . . . . . . 856.6 The INS Class . . . . . . . . . . . . . . . . . . . . . . . . . . . . 856.7 The Angle Class . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
6.7.1 The Eulerangle Class . . . . . . . . . . . . . . . . . . . . . 866.7.2 The Cbn Class . . . . . . . . . . . . . . . . . . . . . . . . 866.7.3 The Quaternians Class . . . . . . . . . . . . . . . . . . . . 86
6.8 The Linear System Class . . . . . . . . . . . . . . . . . . . . . . . 866.8.1 The Kalman Filter Class . . . . . . . . . . . . . . . . . . . 866.8.2 The Information Filter Class . . . . . . . . . . . . . . . . 866.8.3 The Innovation Class . . . . . . . . . . . . . . . . . . . . . 86
6.9 The Vector Integration Class . . . . . . . . . . . . . . . . . . . . 876.9.1 The Trapetz Vector Integration Class . . . . . . . . . . . 876.9.2 The Simpson Vector Integration Class . . . . . . . . . . . 876.9.3 The Runge-Kutta Vector Integration Class . . . . . . . . 87
4
6.10 The Model Class . . . . . . . . . . . . . . . . . . . . . . . . . . . 876.10.1 The Quaternions Model Class . . . . . . . . . . . . . . . . 886.10.2 The Eulerangle Model Class . . . . . . . . . . . . . . . . . 886.10.3 The Linear 9 State Model Class . . . . . . . . . . . . . . . 88
6.11 Other Classes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 886.11.1 The Vector Statistics Class . . . . . . . . . . . . . . . . . 886.11.2 The Fast Trigonometric Class . . . . . . . . . . . . . . . . 886.11.3 The Earth Class . . . . . . . . . . . . . . . . . . . . . . . 886.11.4 The Serial Communication Class . . . . . . . . . . . . . . 886.11.5 The Firmware Class . . . . . . . . . . . . . . . . . . . . . 886.11.6 The Logging Class . . . . . . . . . . . . . . . . . . . . . . 88
6.12 The target computer . . . . . . . . . . . . . . . . . . . . . . . . . 89
7 Results 90
7.1 Test�ight . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 917.2 The Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 947.3 Tetrad and Mpac IMU solutions . . . . . . . . . . . . . . . . . . 94
7.3.1 IMU accelerations . . . . . . . . . . . . . . . . . . . . . . 947.4 Navigation �lter results . . . . . . . . . . . . . . . . . . . . . . . 957.5 IMU-Inertial Measurement Unit . . . . . . . . . . . . . . . . . . . 96
7.5.1 PSD of Inertial Sensors . . . . . . . . . . . . . . . . . . . 967.5.2 IMU Acceleration Solution . . . . . . . . . . . . . . . . . 987.5.3 Body frame velocity . . . . . . . . . . . . . . . . . . . . . 997.5.4 Body frame velocity angles, slideslip and angle of attack . 1007.5.5 IMU Rotationrate Solution . . . . . . . . . . . . . . . . . 1017.5.6 INS-Inertial Navigation System . . . . . . . . . . . . . . . 1017.5.7 INS acceleration . . . . . . . . . . . . . . . . . . . . . . . 1027.5.8 INS velocity . . . . . . . . . . . . . . . . . . . . . . . . . . 1037.5.9 INS omega, eulerangle velocities . . . . . . . . . . . . . . 1047.5.10 INS attitude . . . . . . . . . . . . . . . . . . . . . . . . . 1057.5.11 The Position Innovation . . . . . . . . . . . . . . . . . . . 1067.5.12 The Velocity Innovation . . . . . . . . . . . . . . . . . . . 1077.5.13 The Attitude Innovation . . . . . . . . . . . . . . . . . . . 108
7.6 The Flight Playback . . . . . . . . . . . . . . . . . . . . . . . . . 1097.7 The conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
7.7.1 Bias and nonlinearity states in �lter . . . . . . . . . . . . 1147.7.2 More e�cient algoritms . . . . . . . . . . . . . . . . . . . 1147.7.3 Vibration damping . . . . . . . . . . . . . . . . . . . . . . 1147.7.4 Tilt sensors. . . . . . . . . . . . . . . . . . . . . . . . . . . 1147.7.5 Placement of GPS antennas . . . . . . . . . . . . . . . . . 1157.7.6 The angle classes . . . . . . . . . . . . . . . . . . . . . . . 1157.7.7 The eulerangle model . . . . . . . . . . . . . . . . . . . . 1157.7.8 The quaternion model . . . . . . . . . . . . . . . . . . . . 1157.7.9 Use of GPS pseudoranges . . . . . . . . . . . . . . . . . . 116
7.8 Future Hardware Model . . . . . . . . . . . . . . . . . . . . . . . 116
5
8 Appendix 1
8.1 Data Speci�cations . . . . . . . . . . . . . . . . . . . . . . . . . . 28.1.1 Desk Computer Platform . . . . . . . . . . . . . . . . . . 28.1.2 Target System Computer Platform . . . . . . . . . . . . . 28.1.3 Motion Pack IMU . . . . . . . . . . . . . . . . . . . . . . 28.1.4 Data speci�cation of the Watson IMU . . . . . . . . . . . 38.1.5 Tetrad IMU . . . . . . . . . . . . . . . . . . . . . . . . . . 38.1.6 GPS Receiver . . . . . . . . . . . . . . . . . . . . . . . . . 3
8.2 Numerical Integrators . . . . . . . . . . . . . . . . . . . . . . . . 48.3 Gaussian Distrubutions . . . . . . . . . . . . . . . . . . . . . . . 48.4 Con�dence Ellipses . . . . . . . . . . . . . . . . . . . . . . . . . . 6
8.4.1 Two Dimensional Con�dence Ellipse . . . . . . . . . . . . 78.5 Rayleigh Distribution . . . . . . . . . . . . . . . . . . . . . . . . 88.6 Wienerprocess-Randomwalk . . . . . . . . . . . . . . . . . . . . . 8
6
Nomenclature
0.2 List Of Symbols
7
0.3 Symbols
Symbol Explanation_x Time derivative of position x. In this case velocity [m=s]�x Second time derivative of position x. In this case acceleration [m=s2]rF Linearization of system equation given by F.aN North acceleration in Navigation Frame. [m/s2]aE East acceleration in Navigation Frame. [m/s2]aD Down acceleration in Navigation Frame. [m/s2]xk Is the k'th element from a sequence, x = [x1; x2; :::; xn]
T
bi Represents biasc0 Velocity of light 299792458 [m/s]cij Components of matrix C. j; i = 1::3Cbn Rotation from Navigation Frame to Body FrameCnb Rotation from Body Frame to Navigation FrameDi Dopplershift to satellite iE = [e0; e1; e2; e3] Quaternion angle with elements.f System matrix in a continous time system, _x = f xge Earth gravity vector [m=s2]I ixi Eye matrix with size i x i.K Kalman gain matrixl Inline sight vector from position a to b. l = b�a
jb�aj
L1; L2 GPS frequencies L1 = 1575:42 [MHz], L2 = 1227:6 [MHz]0ixj Zero matrix with i rows and j columns.p Measured rotationsspeed of xBODY -axis [rad/s]q Measured rotationspeed of yBODY -axis [rad/s]r Measured rotationspeed of zBODY -axis [rad/s]P State Covariance MatrixQ Process Covariance MatrixR Measurement Covariance MatrixRe Equatorial radius of the Earth. 6379137 [m]Sb; Sf Classical Allan variance parameters.t Time [s]v Velocity [m/s]vD Vertical velocity in Navigation Frame[m/s]vE East velocity in Navigation Frame [m/s]vN North velocity in Navigation Frame [m/s]z Measurement vector
8
Greek Symbols
Symbol Explanation� Angular acceleration. � = _! = ��. [rad/s2]�(u� v) Discrete Delta Function �(0) = 1 �(n) = 0; n 6= 0�ion Ionosphere time delay [s]�trop Troposhere time delay [s]�u Receiver clock bias error [s]_�u Receiver clock drift [s/s]��i Error in pseudorange to satellite i [m]� _�i Error in pseudorangerate to satelite i [m/s]�t Time between samples [s]� Longitude [rad]� Wavelength [m]! Angular speed. Here ! = [ _�; _�; _ ][rad/s]_e The rotationrate of the Earth, 7.2921151467E-5 [rad/s]� Latitude [rad]� Roll with angle �. [rad]_� Roll rate, in Eulerangle representation [rad/s]� Discrete time state transition matrix�c Discrete time state transition matrix of GPS clock state� Mathematical constant pi�3.1415926535898 Yaw angle. North is zero. [rad]_ Yaw rotation rate . [rad/s]� Preudorange. Inline sight distance to the satellite. [m]_� Preudorange rate [m/s]� Standard deviation� Pitch angle. Elevation angle of vehicle. [rad]_� Pitch angle rate.[rad/s]
9
Abbreviations
Abbreviation ExplanationAAV Autonomous Aerial VehicleADC Analog to Digital ConverterASCII American Standard Code for Information InterchangeC/A Course/Acquisition codeDAC Digital to Analog ConverterDGPS Di�erential GPSDOP Dilution of precisionECEF Earth-Centered Earth-Fixed FrameECI Earth Centered Inerial FrameEIF Extended Information FilterEKF Extended Kalman FilterFFT Fast Fourier TransformGDOP Geometric DOPGPS Global Positioning SystemIF Information FilterIMU Inertial Measuring UnitINS Inertial Navigation SystemKF Kalman FilterNAVSTAR Navigation System with Timing and RangingOS Operating SystemPDOP Position DOPPPS Precise Positioning ServicePSD Power Spectrum DensityRMS Root Mean SquareRPM Rotations Per MinuteSA Selective AvailabilitySPS Standard Positioning ServiceTAS True Air SpeedTDOP Time DOPUAV Unmanned Aerial VehicleUTC Universal Coordinated TimeVDOP Vertical DOPWGS-84 World Geodetic System 1984ZOH Zero and Hold
10
Chapter 1
Introduction
11
1.1 Scope of this thesis
The main project was to implement a INS1/GPS2 navigation �lter that can beused on di�erent vehicles, just by changing the model of the vehicle the �lterwill be optimized for a speci�c purpose. The error �lter must accept all possibleexternal observations. The INS/GPS navigation �lter is written in C++ usinga matrix library called MPP3. This thesis presents a navigation �lter for anUAV4.The �lter is used to estimate the position, velocity and attittude of thevehicle. The navigation �lter uses high frequency information given by an IMU5
and uses low frequency external observations from GPS receivers to give a betterestimation of the states.
1.1.1 Picture of the thesis work
The dashed area represents the work done in the thesis. The core of the work isthe big block named �INS/GPS Navigation Filter�. Here you can see feedbacksignal loops to other blocks.
GPS Reveiver
GPS Reveiver
GPS Reveiver
GPS Reveiver
IMU
IMU
IMU
PC 104 INS/GPS
Navigation filterAttitude
PositionVelocityTime
PositionVelocity
AttitudeAcceleration
Feedback of states
INS
PositionVelocityAcceleration
Feedback of states
Watson
Motion Pak
Tetrad
Rot rateAcc
Rot rateacc
rot rateacc
bias feedbackrescaling
biases
In the �gure we can see that the switch is on to use the Terad IMU. Threedi�erent IMUs have been used. Therefore a IMU software have been derived.The IMUs works all di�erent so subclasses have been derived to handle what isunique with each one of the IMUs.
1.2 Background
The Department of Aeronautical Engineering at University of Sydney have along history of building low-cost operating Unmanned Aerial Vehicles (UAVs).TheUAV Research Group consists of about ten academics and research students.The Department of Aeronautical Engineering is now working together with the
1Inertial Navigation System2Global Positioning System3MPP-Matrix Plus Plus4Unmanned Aerial Vehicle5Inertial Measurement Unit
12
Deparment of Meachanical and Mechatronic Engineering to develop a new gen-eration of UAV's. The airframe used is called Brumby, see 1.2.2 An previousUAV named Ariel has earlier been designed, manufactured and �own as a de-mostration platfrom for di�erent aeronautical research activitis.
Brumby airframe with some of the researchers
Current UAV research activities at Sydney University
� Design studies on misson-speci�c UAVs.
� Micro UAVs.
� Design and fabrication of airframe components using advanced compositematerials.
� Wind-tunnel and �ight based experimental research in aerodynamics.
� Modelling of engine/propeller performance and aircraft stability charac-teristics.
� Aircraft model developement for simulation bases control system.
� Trajectory optimisation and autonomous guidance for self-piloted air-crafts.
� Sensor fusion strategies for state estimation using redundant sensors, in-cluding GPS.
13
� System identi�cation using neural networks for fault detection.
� Robustness analysis of control laws in the presence of uncertain dynamicsand wind gusts.
� Robust nonlinear high-performance tracking for autonomous aircrafts.
� Safe recovery of UAVs
� Flight control real-time software.
1.2.1 The ANSER project
It is a huge project to build an fully operating autonomous aircraft. Suchprojects must be broken down in small subprojects.
The ANSER sub projects
� Sensors
� Functional speci�cation
� Radar sensors
� IMU sensors
� GPS
� Vision
� Laser
� Theory
� Mission planing
� Autonomy
� Map building
� Control
� Mechanical
� Hardware
� Software
� Flight software
� Ground station software
� Simulator software
14
1.2.2 The Brumby UAV airframe
Brumby is an airframe designed at the deparment of Aeronautical Engineeringat the University of Sydney. Brumby looks like a delta wing. This makes it ahigh performance aircraft but di�cult to control. Especially landings can betricky because the airframe will �rst drop in altitude when the pitch angle in-creases.The aircraft can �y approximately 45 minutes on full tank with standardtwo stroke petrol/oil mixture. The aircraft is remote controlled by a pilot on theground. Brumby is collecting data during the �ights used later for postprocess-ing. There is an onboard computer used for logging IMU, Video and GPS datato onboard mounted harddisks. Flights with logging to RAM disks have beendone. The current operating system is Linux. Later it will be possible to switchover the control to a onboard computer that will �y Brumby autonomously andperform di�erent tasks. If the radio connection is broken, Brumby will be ableto land by itself.
The states of Brumby like position, velocity and attitude and other param-eters will be transmitted to a Ground Control Station for visualization. A newand bigger version of the Brumby airframe is under manufacturing. The newairframe will be able to take more payload.
The design of Brumby is well planned. It is very easy to unload and loaddi�erent payload. A special format for payload boxes exists and research groupsall over the world can design their own payloads to be �own in Brumby.
The airframe seen from three directions
Data of Brumby airframe
The values inside the brackets are the data for the new airframe, Brumby MkII.
15
Airframe Value
Wingspan 2.36 m (2.82 m)Wing Area 1.61 m2 ( 1.95 m2)Fuselage Length 1.97 mEmpty Weight ( �ying capable) 17 kg ( 19 kg)Dry / Operational Empty Weight (OEW) 28 kg (33 kg)Fuel Weight (2.4 liters) 1.9 kgUseful Payload (sensors, �ight control) 11 kg ( 14 kg)Max Take O� Weight 30 kg ( 35 kg)Engine Zenoah 74cc TwinEngine Power Approx 5.2 kW ( 7 hp)Max Speed 185 km/hLanding Speed 65 km/h
1.3 The Future of UAVs
An idea is to let a number of unmanned airvehicles take o� and perform taskstogether, like surveillance. They can create maps of the surrounding terrain and�nd targets. If one aircraft �nds a target, this information can be distributedto other UAV's in a near distance. The transmission can be done using laser orradio links. The UAV's can together be viewed as a �ying decentralized systemwith �ying nodes. If one UAV crashes only one node disappear from the �yingnetwork and almost all the information is kept within the other UAV's. TheUAV's can in the future be equipped with di�erent sensors, and all sorts ofpayload.
Interesting UAV applications
� Surveillance of important things and areas.
� Search for cars, lost people : : :
� Emergency transports. Example: organs between hospitals.
� Rescue operations.
� Flying Shepherd Dog.
� Military tasks.
The military is very interested in these sort of vehicles, UAV's can easily inthe future be used for di�erent combat purposes.
Airbases now need people to patrol the fences, this can be done with UAV's.With good algorithms they can operate within big cities to prevent tra�c jamsand report tra�c accidents and help ambulances through the tra�c.
They can be used to track highspeed cars and criminals. Unmanned UAVcan be helpful by monitoring the water status of lakes, protecting the rainforrestsfrom illegal tree cutting, etc
With magnetic and gravity sensors as payload it is possible to indicate orespots from the air. Airpollution can be tested near chimenys and oil spots canbe found on the ocean.
16
1.4 Frames
A frame is the same as a coordinate system. When navigation is done we needatleast two frames. One for the body/intertial representation ( vehicle) and onefor the navigation frame representation (map).
List of di�erent frames
� Body frame
� Earth Surface NED Frame
� WGS-84 , World Geodetic System 1984
� ECI - Earth Centered Intertial Frame
� ECEF-Earth Centered Earth Fixed Frame.
1.4.1 Body Frame
This is the basic frame for the inertial sensors. The axis of this frame is in thiscase the same as the vehicle frame, and you can say that this is a reference framecarried by the vehicle. The x-axis is pointing forward, y-axis is pointing to theright and the z-axis completes a right-hand ortogonal system by pointing downand are ortogonal to both x and y-axis. The origin is at the center of gravity.The velocity of the vehicle in body frame is expressed using [u; v; w].
Body Frame
−4−2
02
46
0
5−6
−4
−2
0
2
x
y
x
Body frame
y
z
z
1.4.2 Earth Surface North-East-Down (NED) Frame
The NED-Frame is for navigation. We have three vectors that form a right handortogonal system. The N vector is pointing North, E vector is pointing Eastand D is pointing Down along the local gravity vector. The N and E vectorsspan a plane that tangent the surface of the Earth. Observe that vector D ismapped from the origin of this plane. This frame works �ne when the operating
17
distances are limited near the origin.
Earth NED frame
X
Y
Z
E
N
x
y
z
D
[x,y,z]-ECEF coordinates
λφ
=longitude
=latitude
[N,E,D]-NED frame axes
[X,Y,Z]- ECEF axes
Greenwich meridian
Equator
1.4.3 WGS-84 , World Geodetic System 1984
The geodetic frame reminds very much about spherical mapping and uses twoangles and the height. It is good for navigation over longer distances. The sealevel of the Earth is mapped if the height is put equal to zero.
Longitude,�: At the Greenwich meridian the longitude is equal zero (� = 0)and completes 360o in the east direction. The longitude says how far tothe east or west we are from the Greenwich meridian.
Latitude,�: At the equator � = 0 and reaches � = 90o at the North Pole and� = �90o at the South Pole. The latitude says how far we are from theequator.
height, h: Height in meter above the Sealevel.
WGS-84 ellipsoid four de�ning parameters
These WGS-84 parameters de�nes the shape of the earth.
Parameter Notation Magnitude
Semimajor axis a 6378137 mNormalized Second Degree Zonal Harmonics C2;0 �484:16685E�6Coe�cient of the Gravitational PotentialAngular Velocity of the Earth _e 7292115:1467E� 10 rad/sEarth's Gravity Constant � 3986005E8 m3=s2
From this parameters the geometric �attering of the earth and the semiminoraxis can be derived.
18
1.4.4 ECI-Earth Centered Interial Frame
This frame is for inertial reference. The inertial frame is �xed with some of theaxes pointing at distant stars. This frame does not follow the rotation of theearth, �xed directions of the axes in space.
Origin: The mass center of the Earth.
X-axis: The X-axis points to a distant star called The Vernal Equinox, thisvector lies in the equatorial plane.
Y-axis: This axis spans the equatorial plane with the X-axis. The Y-axis formsa right handed ortogonal system together with the X and Z axes.
Z-Axis: Points from the origin out through the north pole. This axis is parallelto the rotation vector of the Earth.
1.4.5 ECEF - Earth Centered Earth Fixed Frame
This one is �xed with respect to the Earth and follows the rotation of the Earth.The ECEF frame is used in the GPS system called WGS-84. GPS observationsare represented in this frame. This frame can also be called the Earth cartesianframe because of the cartesian coordianates [x; y; z]ECEF . For more informationabout this frame read in [22] and [6].
Origin: Earth's center of mass.
X-axis: This axis pointing through the Greenwich meridian (longitude=0) andthe equator. Intersection of the WGS-84 reference meridian plane andthe plane of the mean astronomic equator, the reference meridian beingparallel to the zero meridian.
Y-axis: Completes the right-handed ECEF ortogonal system.
Z-axis: This axis is pointing from origin out through the North Pole. Parallel tothe direction of the COVENTIONAL INTERNATIONAL ORIGIN (CIO)for polar motion. Same as the ZECI axis.
19
Chapter 2
Attitude Representation
20
2.1 Rotations
This chapter will explain the three di�erent rotations of an vehicle. Theserotations are roll, pitch and yaw. This chapter will also explain the di�erentangle representations used. The euler angle, Cbn matrix and the quaternionangle representation.
We can be an observer in the navigation frame ( standing on the ground)and observe the airvehicle. What is the acceleration and the rotation rate ofthe object that are under observation ?. This is the information needed to dosome navigation.
In the airframe we have a mounted strapdown1 The IMU measures the ac-celeration [ax; ay; az] and rotation rates [p; q; r]. We need to transform thesevectors down to the navigation frame. The transformation matrix used is theCbn matrix. bn indicates rotation of a vector from body frame to navigationframe. If we use this matrix in body frame we will change the coordinate systemfrom body frame to navigation frame. The rotation is done with three angles,called the eulerangles [�; �; ]. In aeronautics (reference [10]) these angles arecalled roll, pitch and yaw. The rotations are done with respect to the bodyframe.
Roll Pitch and Yaw
−5 0 5
−5
0
5
roll
−5 0 5−5
0
5Pitch
−5 0 5−5
0
5yaw
1This means that the inertial sensors are rigid mounted in the vehicle frame and there
exists no moving parts. The calibration is done with pure mathematic operations
21
2.1.1 Roll rotation,�
This rotation represents wingtips up/down. This is identical with rotation aboutthe center-line of the airframe. A positive angle will make a clockwise rotation.When � = 0 the wings are in horizontal position.
3D visualization of 30o roll
The aircraft tips the wings to 30o.
−4−2
02
46
810
1214
16
−2
0
2
−2
−1
0
1
2
Roll 30 degrees
EASTφ
φ
DOWN
Rotation with angle � about x-axis.
R(�) =
24 1 0 0
0 cos� sin�0 � sin� cos�
35 (2.1)
22
2.1.2 Pitch,�
In aeronautics this is airframe nose up/down. When the vehicle is in horizontalposition the pitch angle is zero, � = 0.
3D visualization of 30o pitch
The airframe to the left in the �gure below changes the pitch to 30o.
−4−2
02
46
810
1214
1618
−2
0
2
−2
−1
0
1
Pitch 30 degrees
NORTH
DOWN
θ
θ
A more mathematical de�nition is the rotation with angle � about y-axis.
R(�) =
24 cos � 0 � sin �
0 1 0sin � 0 cos �
35 (2.2)
23
2.1.3 Yaw,Heading
Yaw rotation of the airframe, nose left/right. The yaw angle represents therotation of the airframe about the gravity vector. The heading is the directionof travel in navigation frame. The heading and yaw angle are almost the sameand di�er if the vehicle has a sideslip component. This sideslip can be causedby sidewinds or by the dynamics of the airframe. The airframe starts poitingnorth with the yaw angle equal zero, = 0. Rotation is done clockwise aroundthe gravity vector.
3D visualization of 30o yaw
Yaw rotation of an airvehicle, the airvehicle changes the heading with 30o.
−4−2
02
46
810
1214
16
−2
0
2
−0.50
0.51
Yaw 30 degrees
ψ
ψ
NORTH
EAST
Another de�nition of the yaw angle is rotation with angle about z-axis.
R( ) =
24 cos sin 0� sin cos 0
0 0 1
35 (2.3)
24
2.1.4 True attack and attack angle
If the direction of the �ight doesn't follow the pitch angle we have a smalldi�erence between the true air speed and the pitch angle. The sideslip angle iscalled �. We can calculate the sideslip with equation (2.4).
� = � � arctan
�wp
u2 + v2
�(2.4)
True attack
α
u
wTAS
TAS=True Air Speed
2.1.5 Sideslip
If the heading of the aircraft in navigationframe di�er from the yaw angle, wehave a sideslip. This sideslip is often caused by winds or by the dynamics of theairframe. The � angle is called the sideslip angle and are the di�erence betweenthe heading and the yaw angle.
� = � heading = � arctan�vu
�(2.5)
Sideslip
u
v
=sideslip angleβTAS=True Air Speed
β
25
2.2 Body Rotation Rate
The rotation rate measured by the IMU in bodyframe is [p; q; r]T . By taking thecrossproduct with the eigenvectors in the bodyframe gives us the rotationratein matrix representation. If we have a vector [1; 1; 1]T the resulting crossprod-uct with [p; q; r]T will be [q � r; r � p; p � q]T . The vector [1; 1; 1]T is a linearcombination of the base vectors [1; 0; 0]T ,[0; 1; 0]T and [0; 0; 1]T these can beput as a I3x3 matrix and the crossproduct with the rotationrate will result ina skewsymetric matrix, equation (2.6). In the �gure below we have the vec-tor [1; 1; 1]T ,the rotation rates [p; q; r]T about X,Y and Z-axis and the resultingchangerate vector.
Frame vectors crossproducted with [p; q; r]T
p
q
r
a(q-r)
X
Y
Z
r=[1,1,1]
q-r
r-p
p-q
From the �gure we see that we have a resulting velocity vector. This can be putinto a skewsymmetric matrix.
PQR(p; q; r) = I3x3 �24 pqr
35 =
24 0 �r q
r 0 �p�q p 0
35 (2.6)
An interesting thing to do is to write the changes as a di�erential equation24 _x
_y_z
35 =
24 0 0 0
0 0 �p0 p 0
3524 xyz
35 (2.7)
The solution to this equation is24 x(t)y(t)z(t)
35 = exp
0@24 0 0 0
0 0 �p0 p 0
35 t1A+ constant (2.8)
26
If we take a short time �t, this gives us the angle � = �tp.
exp
0@24 0 0 0
0 0 ��0 � 0
351A = I3x3 +
1Xk=1
1
k!
24 0 0 0
0 0 ��0 � 0
35k
=
24 1 0 0
0 cos� � sin�0 sin� cos�
35
(2.9)
Which inded is a very interesting result. The same procedure can be repeatedfor both the q and r rotationrates and we end up with the rotation matrixes forall three directions.
2.3 Cbn, Direction Cosine Representation
In this representation the angle are kept within the Cbn matrix. The rotation-rates are used to update the matrix. If we want the attitude the angles can becalculated from the elements of the Cbn matrix.
Block schematics of Cbn matrix representation
Cbn to Quaternion p
q
r
[p,q,r]
to
Cbn.
Cbn
Cbn to qulerangles
[e0,e1,e2,e3]
[x,y,z] Rotation of vector [x1,y1,z1]
ψ
φ
θ
Cbn
dot Cbn
Cbn
Selector[e0,e1,e2,e3]
φ,θ,ψ
The roll, pitch and yaw rotations can be put in a sequence, the result willbe a rotationmatrix that rotates a vector from navigationframe to bodyframewith the angles [�; �; ]. This rotation matrix is called the Cbn matrix and isde�ned in equation (2.10).
Cbn = R(�; �; ) = R�R�R (2.10)
27
Cbn =
24 c11 c12 c13c21 c22 c23c31 c32 c33
35 (2.11)
The Cbn matrix is a 3x3 ortogonal matrix with a total of nine elements. It have
the property that detCbn = 1 which means that the volume is preserved andno rescaling is done. The rows and columns of the Cbn matrix are ortogonal toeach other.
CbnCTbn = I3x3 (2.12)
This matrix is possible to invert for all angles. If we write out all terms usingtrigonometric functions we get (2.13).
Cbn(�; �; ) =h
cos � cos � cos � sin + sin� sin � cos sin� sin + cos� sin � cos cos � sin cos� cos + cos� sin � sin � sin� cos + cos� sin � sin � sin � sin� cos � cos � cos �
i(2.13)
2.3.1 The Cbn matrix and small angles
When small angles are used we can use �rst order Taylor expansion of thetrigonometric functions in the Cbn matrix, cos� � 1 and sin� � �. We thenget a skewsymetric rotationmatrix, equation (2.14).
Cbn(�; �; ) = I3x3 + I3x3 �24 �
�
35 =
24 1 �� �
� 1 � �� 1
35 (2.14)
2.3.2 Conversion to Eulerangles from Cbn represention
If the Cbn matrix is given, the eulerangles can easily be calculated with equa-tions (2.15),(2.16) and (2.17). These equations is derived from (2.13). The cijvariables are found in the Cbn matrix.
� = arctan
�c32c33
�= arctan
�sin� cos �
cos� cos �
�(2.15)
� = arcsin (�c31) = arcsin (sin �) (2.16)
= arctan
�c21c11
�= arctan
�cos � sin
cos � cos
�(2.17)
28
2.3.3 Calculation of Eulerangles with large pitch angle
If we have a Cbn matrix with a pitch angle such that cos � � 0 we can notuse equations (2.15),(2.16) and (2.17) because the equations are not numericalstable. There exits other equations for this case. In [37] we �nd the equations(2.18) to (2.21). The cij variables are elements in the Cbn matrix. i=row,j=column.
c23 � c12 = (sin � + 1) sin ( � �) (2.18)
c13 + c22 = (sin � + 1) cos ( � �) (2.19)
c23 + c12 = (sin � � 1) sin ( + �) (2.20)
c13 � c22 = (sin � � 1) cos ( + �) (2.21)
Equations (2.20), (2.21), (2.18) and (2.19) have three unknown variables, thisequation system can be solved. If we divide equation (2.18) with (2.19) we get(2.23), (2.18) and (2.19) gives (2.22).
= arctan
�c23 + c12c13 � c22
�(2.22)
� � = arctan
�c23 � c12c13 + c22
�(2.23)
By combining (2.22) and (2.23) we get:
� = arctan
�c23 � c12c13 + c22
�� arctan
�c23 � c12c13 + c22
�(2.24)
The � angle is calculated with the previous equation (2.16).
� = arcsin (�c31) = arcsin (sin �) (2.25)
2.3.4 The changerate of Cbn matrix
We know how to calculate the rotationrate matrix, lets calculate the changerateof the Cbn matrix . The Cbn matrix have bodyframe as reference frame becausethe rotation is always done relative to the airframe. The [p; q; r] vector representsthe rotationrate about each axis. It is shown in 2.2 that the crossproduct canbe written as a skewsymmetric matrix. The Cbn matrix is multiplied with (2.6)and this gives us an expression to calculate the changerate of the Cbn matrix.
_Cbn = Cbn
24 0 �r q
r 0 �p�q p 0
35 (2.26)
We get the Cbn matrix by integration
Cbn =
Z_Cbndt =
ZCbn
24 0 �r q
r 0 �p�q p 0
35dt (2.27)
29
2.3.5 Cbn matrix integration in discrete time
Equation (2.27) is a continous time integrator that we need to discretisize forsoftware implementation. The change the Cbn matrix will do under a shorttime �t = tk+1� tk is _Cbn�T . This change must be added to the actual Cbn(k)matrix. A simple ZOH integrator is
Cbn(k + 1) = Cbn(k) + _Cbn(k)�T = Cbn(k)
24 1 �r q
r 1 �p�q p 1
35�T (2.28)
2.3.6 Very accurate Cbn matrix integration in discrete time
To make the update of the Cbn matrix more accurate we can use the Taylorexpansion presented in [37]. The idea is to expand the rototationrate matrix,eA using � =
pp2 + q2 + r2 which lead to equation (2.29)
eA = I3x3 +sin�
�A+
1� cos�
�2A2 (2.29)
A =
24 0 �r q
r 0 �p�q p 0
35�T (2.30)
A2 =
24 �(q2 + r2) pq pr
pq �(p2 + r2) qrpr qr �(q2 + r2)
35�T 2 (2.31)
The Cbn matrix update will be
Cbn(k + 1) = Cbn(k)
�I3x3 +
sin�
�A+
1� cos�
�2A2
�(2.32)
2.3.7 Normalization of Cbn matrix
If the Cbn matrix is updated ( rotated) alot this introduces rescaling of the rowsvectors. The row vector will after a while not be ortogonal to each other. Thiscan be caused by the �oating point aritmetic in a modern computer. Lets saywe have a ~Cbn matrix who ~Cbn ~C
Tbn <> I3x3.
The matrix can be normalized using (2.33).
Cbn = ~Cbn � 0:5�~Cbn ~C
Tbn
�~Cbn (2.33)
For more information read [37]
2.4 Eulerangle Representation
In eulerangle representation the attitude are kept in a vector [�; � ]. The rota-tionrates [p; q; r] must update the [�; �; ] vector. This is done with an integra-tor.
30
Block schematics of Eulerangle representation
pqr to eulerangle rate
p
q
r
φ,θ,ψ. . .
eulerangle rateintegration
Calculationof Cbn matrix
Calculatio of quaternion angles
φ,θ,ψ
e0,e1,e2,e3
Cbn matrix
Cbn
*vector rotated v
e0,e1,e2,e3
φ,θ,ψ
2.4.1 Calculation of [ _�; _�; _ ] using [p,q,r]
The elements in the Cbn matrix can be calculated if we know the eulerangles. Byknowing this we can calculate the changerate of the eulerangles with the rota-tionrates as arguments. We can transform rotationrates [p; q; r]BODY FRAME tonavigationframe and then we will get the anglerates of the eulerangles [ _�; _�; _ ].Because the rotation using eulerangles are done in sequence R(�)R(�)R( ) wemust have this in mind when we want to calculate [ _�; _�; _ ]. We put this into aequation system and get (2.34).2
4 pqr
35 =
24 _�
00
35+R(�)
24 0
_�0
35+R(�)R(�)
24 0
0_
35 (2.34)
This equation can be written as
24 pqr
35 =
24 1 � sin � 0
0 cos � cos � sin�� sin� 0 cos � cos�
3524 _�
_�_
35 (2.35)
31
24 pqr
35 = C
pqrj _� _� _
24 _�
_�_
35 (2.36)
To solve [ _�; _�; _ ]T we take the inverse of Cpqrj _� _� _ = C�1_� _� _ jpqr. The C _� _� _ jpqr
transformation matrix will be :
C _� _� _ jpqr =
24 1 sin� tan � cos� tan �
0 cos� � sin�0 sin� sec � cos� sec �
35 (2.37)
So the euleranglerates can be calculated using equation (2.38).
24 _�
_�_
35 = C _� _� _ jpqr
24 pqr
35 (2.38)
24 _�
_�_
35 =
24 1 sin� tan � cos� tan �
0 cos� � sin�0 sin� sec � cos� sec �
3524 pqr
35 (2.39)
There is a problem with the C _� _� _ jpqr matrix. We have a singularity in thematrix, element c33, when cos � = 0 leads to sec � = 1= cos� = undefined.The singularity in a strapdown system is equivalent of gimbal lock. This occurswhen the aircraft is �ying with the nose straight up or straight down. Becausethe euleranglerate has singularities it is bad angle representation in high per-formance aircrafts like �ghter aeroplanes. This three parameter euler algoritmare normally used in system with bounded pitch angle, between + � 30o. Wehave other models to bypass this, both the Cbn matrix and the Quaternionanglerepresentation supports this.
Equation (2.39) is not de�ned when sec � becomes singular, and this canbe bypassed by these equations found in [23]. The equations have not beenimplemented in the software at all, but is an interesting thing to investigate.
_� = p+ _ sin �_� = q cos�� r sin�_ = ( _�� p) sin � + (q sin�+ r cos�) cos �
(2.40)
2.4.2 Eulerangle integration in a discrete time system
In a navigation system we need to keep track of the eulerangles. The rate gyrosin the IMU2 measures the rotationrates of the airframe which are transformedto euleranglerates. To update eulerangles we run the euleranglerates trough anZOH integrator. 2
4 ��
35k+1
�24 �
�
35k
+
24 _�
_�_
35�T (2.41)
2Inertial Measuring Unit
32
This model is very fast but it's weakness is that the error is of order O(h2), withan IMU samplerate of 300Hz and a 2� makes this a very bad model. Howeverlet's not cry, there exists more fancy integrators. In the software several di�erentdiscrete integration methods were tested, the best one implemented is a fourorder Rungekutta integrator with an error of O(h5). Simpson, and Trapetzintegration methods have been tested and implemented and are good methodsif highspeed integration is needed.
2.5 Quaternion Angle Representation
The idea with this method is to use four variables for rotation in R3 instead ofthree. It can also be shown that this system with quaternions is valid for allattitudes. The [p; q; r] rotationrates are used to update the quaternion angle.
Block schematics of quaternion angle representation
φ, θ, ψ
. . . .[e0,e1,e2,e3]. . . .
[e0,e1,e2,e3]. . . .
[p,q,r] top
r
q to
Eulerangle rate
φ, θ, ψ. ..
to Cbn change rate
Cbn.
[e0,e1,e2,e3]. . . .
[e0,e1,e2,e3]
CbnCalculation
of Cbn
[e0,e1,e2,e3]
Cbn
[e0,e1,e2,e3]
From this stage quaternion angles are viewed as:
E =�e0 e1 e2 e3
�(2.42)
Let u be the vector before a single R3 rotation about vector q and v the re-sulting vector. Then � is de�ned as the angle between the two vectors. So� = arccos
�u�vjujjvj
�.
33
µ/2
µ/2 q
u
vx
y
z
α
βγ
u
vq
The q axis makes the angles cos�1 �, cos�1 � and cos�1 with the inertialaxes x,y and z, see �gure above. The equations to calculate the the E vectorelements are:
e0 = cos(�=2)e1 = � sin(�=2)e2 = � sin(�=2)e3 = sin(�=2)
(2.43)
It can be shown that the quaternionangle vector elements can be used tocalculate the Cbn matrix, the equation (2.44) is found in [37].
Cbn =
24 (e20 + e21 � e22 � e23) 2(e1e2 � e0e3) 2(e1e3 + e0e2)
2(e1e2 + e0e3) (e20 � e21 + e22 � e23) 2(e2e3 � e0e1)2(e1e3 � e0e2) 2(e2e3 + e0e1) (e20 � e21 � e22 + e23)
35(2.44)
2.5.1 Calculation of Eulerangles using Quaterion angles
The Eulerangles can be calculated from equation (2.44) using the known Cbnrelation, see equations (2.13) .
� = arctan�c32c33
�= arctan
�2(e0e1+e2e3)e20�e
21�e
22+e
23
�� = arcsin (�c31) = arcsin (2(e0e2 � e3e1))
= arctan�c21c11
�= arctan
�2(e0e3+e1e2)e20+e
21�e
22�e
23
� (2.45)
2.5.2 Quaternionangle calculation using Euler representa-tion
If we do not have the Cbn matrix we can use the eulerangles [�; �; ] to calculatethe quaternion vector elements using equations (2.46),(2.47), (2.48) and (2.49).
e0 = cos�
2cos
�
2cos
2+ sin
�
2sin
�
2sin
2(2.46)
e1 = sin�
2cos
�
2cos
2� cos
�
2sin
�
2sin
2(2.47)
34
e2 = cos�
2sin
�
2cos
2+ sin
�
2cos
�
2sin
2(2.48)
e3 = cos�
2cos
�
2sin
2� sin
�
2sin
�
2cos
2(2.49)
After that the vector elements have been calculated the quaterionangle vectorneed to be normalized, e20 + e21 + e22 + e23 + e24 = 1 should always be true.
2.5.3 Calculate Quaternion using the Cbn matrix represen-tation
Using equation (2.44) we can calculate the quaternionangle vector elements.
e0 =12
p1 + c11 + c22 + c33 =
12
p4e20
e1 =14e0
(c32 � c23) =14e0
(2(e2e3 + e0e1)� 2(e2e3 � e0e1))
e2 =14e0
(c13 � c31) =14e0
(2(e1e3 + e0e2)� 2(e1e3 � e0e2))
e3 =14e0
(c21 � c12) =14e0
(2(e1e2 + e0e3)� 2(e1e2 � e0e3))
(2.50)
2.5.4 Quaternion Norm
The L2 norm of the quaternion angle vector should always be one and is usedto correct for accumulated computational errors.
1 = jEj =qe20 + e21 + e22 + e23 (2.51)
If a quaternionangle vector ~E doesn't have the right L2 norm it can be nor-malized using equation (2.52). If we look at equation (2.45) we see that theeulerangles are calculated using all elements of the quaternion angle vector.This can introduce some unwanted e�ects. If one of the quaternion elementshave a small error this will a�ect all the other elements when the normalizationis done. And say if we are using eulerangles and have an error in pitch, this errorwill contribute to all quaternion angle elements. When the vector is normalizedthe error is spread to both roll and yaw.
Quaternionangle vector normalization
E =~E
j ~Ej =~Ep
~e20 + ~e21 + ~e22 + ~e23(2.52)
2.5.5 Quaternationangle change rate
The components denoted [p; q; r] are the rotationrates in body frame. Therelation between quaternion angle rate and [p; q; r] can be found in [37] and is:2
664_e0_e1_e2_e3
3775 =
1
2
2664�e1 �e2 �e3e0 �e3 e2e3 e0 �e1e2 e1 e0
377524 pqr
35 (2.53)
35
The equation is continous in time and is a system of di�erential equations.This should be implemented in a computer system and we prefer to keep thequaternion angle in vector form. So we do the following rewriting of equation(2.53).2
664_e0_e1_e2_e3
3775 =
1
2
2664
0 �p �q �rp 0 r �qq �r 0 pr q �p 0
37752664e0e1e2e3
3775 = _E = A(p; q; r)E (2.54)
We know the angle at all time if we integrate (2.54).
E(t) =
Z_E(p(t); q(t); r(t))dt (2.55)
2.5.6 Quaternionangle update in discrete time
The previos section lead us to equation (2.55). This equation must however bediscrete to �t in a computer system, hence
En+1 = En +
Z tn+1
tn
_Endt = En +
Z tn+1
tn
A(p; q; r)Endt; n 2 f0::Ng (2.56)
With �t = tn+1� tn, and assuming that p,q and r are almost constant betweentn+1 and tn. Equation (2.56) can be implemented using a ZOH integrator.
En+1 = En + _En�t = (I +A�t)En (2.57)
Equation (2.57) is identical to :2664e0e1e2e3
3775n+1
=1
2
2664
1 �p�t �q�t �r�tp�t 1 r�t �q�tq�t �r�t 1 p�tr�t q�t �p�t 1
37752664e0e1e2e3
3775n
(2.58)
This is a �rst order integrator. If a more accurate integration is needed we canuse the same method as in section 2.3.6. A model of this can be derived forquaternions, read more about this in [37].
36
Chapter 3
Inertial Sensors, IMU and
INS
37
3.1 Newtonian/Inertial Sensors
They can measure newtonian/inertial values like forces, and are called inertialsensors. Di�erent sensors exists to especially measure acceleration and rotation-rates. It is prefered to have very accurate and sensitvte sensors when we wantto measure acceleration. The error in position will grow with a square factor intime with respect to the error in acceleration, shown in equation (3.1).
�p =
Z Z�a dtdt = O(t2) (3.1)
3.1.1 Sensor Termal Compensation
A sensor, especially a gyro, is strongly a�ected by the surrounding temperature.An IMU that is not temperature stabilized or temperature compensated is al-most useless. Even if the temperature is monitored and we try to keep it at adesired temperature will there be �uctation. In order to get a high performingIMU/INS system we must have a mathematical model of how the temperaturea�ects the measurements from the inertial sensors. Equation 3.2 represents agood model.
Variable Explanation
t Time [s]� Self-Heat time constant. [s]T Actual temperature [K]Tref Reference temperature. [K]B0, B1, B2 Calibration constants. [1;K�1;K�2]
S(t; T ) = B0 exp
��t�
�+B1(T � Tref ) +B2(T � Tref )
2 (3.2)
When a sensor is switched on it takes a while before the temperature reachesequlibrium or working point. The time it takes for the sensor to reach thisworking point is unique for every sensor and the enviroment it is placed in. Thevariable �; Tref ; B0; B1andB2 must be calculated from tests.
3.1.2 Newtonian Sensor Nonlinearity
All sensors are unique. So therefore they will give di�erent outputs even ifthey are of the same type and placed in the same enviroment. A lot of sensorsare delivered with calibration data and di�erent terms to put into calibrationequations. This is normally the case for very expensive and accurate sensors.If a sensor is bought and no calibration paper is submitted with the sensor itis normally a low cost sensor that only have standard parameters given. Theoutput from a newtonian sensor is often an analog signal. This signal containsa bias 'b' we need to know or calculate. A sensor work in a special range, an
38
ideal sensor is linear in this region. Some sensors are almost linear and no e�ortare done in calculating the nonlinear terms. A very common and simple modelto improve a sensor is to use equation (3.3).
m = SF�1 n � v + b (3.3)
This is rearranged and we get :
n � v = sf(1 + sfe)(m� b) (3.4)
If we have a strong nonlinearity then sfe can be made to a function, with themeasured value m as argument, sfe(m):
Variable Explanation
m Measured valueb Measured sensor biassfe Scalefactor errorsf ScalefactorSF�1 1/(sf(1+sfe))n Sensor normalvector, jnj = 1. Direction of sensitivityv Real newtonian vector. We want to measure this one
3.2 Multi sensor unit
We have a sensor unit with lot of sensors of the same type. The sensors axes arepointing in di�erent directions. To measure the vector v in R3 we need atleastthree sensors mounted such that the sensoraxes spans R3, optimal is to placethem ortogonal to each other.
The total scalefactor of sensor k is written as sk, this includes both there-scalefactor and the scalefactor error.
sk = sfk(1 + sfek) (3.5)
We have a unit with three sensors with normalvectors (n1; n2 and n3) that arelinear independent.2
4 n1n2n3
35v =
24 s1 0 0
0 s2 00 0 s3
3524 m1 � b1m2 � b2m3 � b3
35 (3.6)
bfv =
24 n1n2n3
35�1 24 s1 0 0
0 s2 00 0 s3
3524 m1 � b1m2 � b2m3 � b3
35 (3.7)
3.2.1 The Accelerometer
This sensor can be modelled as a weight hanging in spring. A tension sensor isconnected to the spring. The spring must be very strong to prevent oscillations.The force is linear to the acceleration acting on the mass, equation (3.8). The
39
accelerometer have a direction of sensitivity, this direction unit vector is calledn.
F = m(n � a) (3.8)
When the force is acting on the �spring� this will create a tension that can bemeasured. The analog signal is converted to digital signals by using an Analogto Digital Converter (ADC). Before this is done the signal must pass a lowpassanalog �lter. The cutfrequency should be at maximum the Nyquist frequency,half the sample rate.
Analogfilter A/D Converter
Accelerometer
Direction of sensitivity
lowpass filter
3.2.2 The Rate Gyros
The standard old gyro is a rotating disc with a very high rotating speed. Thegyro will try keep the direction of the rotating vector. If the direction will changethere will be a momentum applied to the mount axis. With some sensors atthe mounting points it is possible to measure the force and or the stress of thematerial. With this measurement you calculate the change rate of the rotatingvector. With this it is possible to keep track of angles.
The angluar rate gyros of today use vibrating piezoelectric ceramic trans-ducers.
Analogfilter A/D Converter
Rate gyro
Dir
ectio
n of
sen
sitiv
ity
Lowpass filter
3.2.3 Sensor Bias
Some of the sensors will drift in time, the bias or even the scalefactor. This ismainly caused by temperature changes. But in a gyro the bias will change overtime caused by the rotation of the earth.
40
Gyro Bias Drift Example
Lets have a bias br in the rotation rate r measured by the IMU, we assumethe bias to be small. The unit of br is [rad=s]. We get the increse in angle byintegration.
�� =
Zbrr dt = brr t (3.9)
This small angle will cause an misalignment of the IMU. The accelerationvector is projected wrong. We get an acceleration in the north direction,an = sin(�)ge � brget. This is integrated twice to get the error in position.
�p =
Z Zbrget dtdt = brge(t
3=6 + C1t+ C2) = O(t3) (3.10)
We have an error in position that increases cubic in time when an bias errorexists on the gyros.
To get a high performance IMU the biases of the gyros should be added tothe navigation �lter. This can be modelled with this simple two state model,where �bGYRO 2 N(0; �bGYRO )
�_b1_b2
�=
�0 10 0
� �b1b2
�+
�01
��bGYRO (3.11)
The b1 parameter is the bias and b2 is the bias change rate. The gyro bias is:
bGYRO =�1 0
� � b1b2
�(3.12)
3.2.4 Redundant Sensor Unit
If the we have more sensors than unknown dimensions we have a redundantsensor system. The sensor normalvectors are placed such that they togetherspans R3. But they are not linear independent, and there will not exist ainverse to matrix [n1;n2; : : : ;nk]
T, k>3. We have the following sensor system26664n1n2...nk
37775v =
26664s1 0 : : : 00 s2 : : : 0...
.... . .
...0 0 : : : sk
3777526664m1 � b1m2 � b2
...mk � bk
37775 (3.13)
Equation (3.13) can be solved in least square norm. Assuming all sensor havethe same measurement noise the best solution of v in L2 norm is :
~v =
0BBBB@
26664n1n2...nk
37775T 26664n1n2...nk
37775
1CCCCA
�1 26664n1n2...nk
37775T 26664s1 0 : : : 00 s2 : : : 0...
.... . .
...0 0 : : : sk
3777526664m1 � b1m2 � b2
...mk � bk
37775
(3.14)
41
If we have di�erent noise characteristics on the sensors this can be put in themeasurement noise matrix R. The optimal solution ~v will in this case become.
~v =
0BBBB@
26664n1n2...nk
37775T
R�1
26664n1n2...nk
37775
1CCCCA
�1 26664n1n2...nk
37775T
R�1
26664s1 0 : : : 00 s2 : : : 0...
.... . .
...0 0 : : : sk
3777526664m1 � b1m2 � b2
...mk � bk
37775
(3.15)
3.3 IMU-Inertial Measuring Unit
In an autonomous vehicle it is prefered to know its position at a speci�c time.This can be achieved with external sensors. They are usually low rate sensorsthat need a lot of dataprocessing. A rather simple way to give information ata high frequency, 100-500Hz is to use inertial sensors mounted on or inside thevehicle frame. Normally these sensors are placed in one intertial measuring unit(IMU). The IMU provide readouts about acceleration and rotationrates usingaccelerometers and rate gyros as sensors.
A strapdown1 mounted IMUs were used in this thesis and the transformationbetween diferent frame is done mathematically. The cbn matrix is very usefulto to this, look equation 2.13.
In the picture below we see a typical mounting of an IMU. In this case theIMU is placed at a distance of r from the mass center. When the vehicle isunder rotation this will cause an acceleration that is measured by the IMU. Ahigh performance navigation system must compensate for this acceleration.
IMU mountpoint in airframe
Mass Center
X
Y
IMU
r
1The relation between the sensor axes and the body axes remains the same. The sensors
are rigid mounted inside the IMU unit.
42
3.3.1 The Tetrad
The tetrad have a set-up of newtonian sensors, four rate gyros and four ac-celerometers. These are mounted on each surface of a tetrad such that accelerometeriand gyroi have parallel normalvectors except of opposite directions.
The tetrad
= Rate gyro sensor
= Accelerometer sensor
n1
n3
n4
n2
n1,n2,n3,n4=Sensor axes
The sensors values are measured with an 16-bit ADC. The temperature sen-sors are measured with an 8-bit ADC. The desired working temperature ap-proximate 40oC is reached with heating elements, low ohmic resistors. Thecurrent through the resistors are controlled using a DAC ( U=RI, U=Voltage,R=Resistance, I=Current).The ADC's and DAC's are controlled with a PIC mi-crocontroller. The communication to the hardware and mircocontroller is donethrough a FlexIO system developed Ph.D. student Keith Willis at the Depart-ment och Mechanical and Mechatronic Engineering at Sydney University. TheFlexIO board have both CAN and serial bus interface. The software I wroteuses the serial interface to read and control the Tetrad.
43
The Tetrad IMU hardware
Temperature Sensor
Temperature Sensor
Temperature Sensor
Temperature Sensor
Temperature Sensor
Temperature Sensor
Temperature Sensor
Temperature Sensor
1
2
3
4
5
6
7
8
8-B
it A
DC
16-B
it A
DC
Gyro
Gyro
Gyro
Gyro
Accelerometer
Accelerometer
Accelerometer
Accelerometer4
3
2
1
4
3
2
1
Flex IO MotherboardHEATER RS232 Port
TETRAD INERTIAL MEASUREMENT UNIT WITH FLEX-IO
Flex IO AnalogDaughter Board
3.3.2 Tetrad Commands
The Tetrad IMU can be controlled with serveral commands sent through theserial channel. The software inside the Tetrad IMU is called the Firmware. Themost useful commands are listed in the table below.
COMMAND ASCII DESCRIPTION
RESET T Reset the TetradVERSION V Read Tetrad Firmware versionSTATUS U Read Firmware status �agTIME T Return Timestamp. Units 10usAUTOZERO Z Average Channel Values. Calculates the biases.TEMPERATURE P Read Temperatures.SET S Set temperature and sample frequencies.READ R Read speci�c channel.READ_ALL A Return ADC channel values with time stamps.START C Continous sampling of ADC channels.STOP F Halts continous output
44
3.3.3 Tetrad IMU Fault Detection
3.4 INS-Inertial Navigation System
The information provided by the IMU such as the body accelerations and rota-tionrates [p; q; r] are processed in this unit to provide other systems informationabout the vehicle position, velocity and attitude. The acceleration in bodyframeis transformed to navigation frame and the gravity vector is subtracted. Theresulting acceleration vector is integrated with respect to time and we get thevelocity of the vehicle. Another way is to stay in the bodyframe and integratethe body accelearation after the gravity have been cancelled.2
4 uvw
35 =
Z 24 _u_v_w
35 dt (3.16)
The velocity vector is then integrated and we can read the position of the vechi-cle. If the the acceleration transformed down to navigationframe and integratedwe get the position [x; y; z]T vector.2
4 xyz
35 =
ZCbn(�; �; )
24 uvw
35 dt (3.17)
The position vector is given in navigation frame coordinates.
Cbn
pqr to
omega
Acc
pqr
x
y
z
-
INS
IMU
Accelertaion
Velocity
Position
Anglevelocity ( omega)
Eulerangle
NAVIGATIONFRAME
Gravity
3.4.1 Leveling
Alignment of the IMU is done by leveling. In a strapdown inertial system this isa mathematical operation. The idea is to calculate the biases of the sensors. Thescalefactors are assumed to remain the same. The IMU is placed in horizontalposition and during the leveling the IMU should remain stationary. If the IMUhave tilt sensors the leveling equation can be coorected with the angles.
Accelerometers: We know the normalvectors of the accelerometers. The localgravity vector is projected onto the sensor axes. We can calculate theteoretical readout from the sensor. The sensors are sampled over a time
45
period. The delta bias is the di�erece between the teoretical value andthe calculated mean. The samples m are calculated using (3.4).
�baccj = SF�1accj
naccj � ge �
1
N
NXi=0
maccj [i]
!(3.18)
Rate Gyros: When the IMU is placed stationary we know that the attituderemains the same. Therefor the meanvalue of the sampled rotationratesshould be zero.
�bgyroj = SF�1gyroj
0� 1
N
NXi=0
mgyroj [i]
!(3.19)
The known sensor biases are updated with the biases errors calculated in equa-tions (3.18) and (3.19).
46
3.5 Navigation Equations
We have [ _u; _v; _w] as the true vehicle acceleration in the body frame. This accel-eration is not the acceleration measured by the IMU. The IMU accelerometersensors will measure the gravity vector, the vehicle acceleration the centripetalacceleration and some noise. The radius of curvature is r. The acceleration dueto angularvelocity is
an = v2=r =!r!r
r= !v (3.20)
In vector form this will be
an = ! � v = ! � (! � r) (3.21)
Centripetal acceleration
r
a
v
n
ω
The IMU will also measure the gravity acceleration and can not know the di�er-ence between this acceleration and true vehicle acceleration. To solve the vehicleacceleration we must substract the known gravity components from the mea-sured accelerations. This gives us equation (3.24). [ax; ay; az] are the measuredacceleration in bodyframe and [gx; gy; gz] are the gravity component expressedin body frame.The gravity vector in the bodyframe is not dependent of the yawor heading of the airvehicle.2
4 gxgygz
35 = Cbn[0; 0; ge]
T =
24 �ge sin �ge cos � sin�ge cos � cos�
35 (3.22)
We get the � and � angles from the INS and can then calculate the gravitycomponents in body frame.
We have another acceleration term if the IMU is not placed in the center ofgravity. Suppose the IMU is placed at coordinate [rx; ry; rz ]BODY relative to thecenter of gravity, then there will be an acceleration when the vehicle turns. Wecall this acceleration aIMU and the magnitude is dependent of the rotationrates
47
and the placement of the IMU. This extra acceleration can be calculated withequation (3.23) 2
4 aIMUx
aIMUy
aIMUz
35 =
24 pqr
35�
2424 pqr
35�
24 rxryrz
3535 (3.23)
We use equation (2.6) and (3.22) and get:
_u� vr + wq + gx + aIMUx = ax_v + ur � wp+ gy + aIMUy = ay_w � uq + vp+ gz + aIMUz = az
(3.24)
We can solve the true vehicle acceleration from equation (3.24) . Have assumedthat the IMU is placed at the center of gravity hence aIMU = 0.
:
24 _u
_v_w
35 =
24 uvw
35�
24 pqr
35+
24 axayaz
35�
24 gxgygz
35 (3.25)
24 _u
_v_w
35 =
24 0 �w v
w 0 �u�v u 0
3524 pqr
35+
24 axayaz
35�
24 gxgygz
35 (3.26)
Other acceleration terms
If the navigation system is ment to work over longer distances we need to takein calculation the Corriolis acceleration caused by the rotation of the Earth.The Corriolis acceleration will be nonzero if the vehicle is travelling such thatthe latitude is changing.
48
Chapter 4
Kalman and Information
Filters
49
This chapter will explain the nonlinear �lter and the kalman �lter used in thenavigation �lter. There di�erent models are presented, one almost linear, oneusing eulerangle representation and one using quaternion angle representation.The complement �lter are also presented here.
4.1 Linear System in continous time
In control theory and �lter theory we often use continous time models. We canset up two very common equations for continous time systems, equations (4.1)and (4.2).
_x(t) = Ax(t) +Bu(t) (4.1)
y(t) = Cx(t) +Du(t) (4.2)
4.2 Linear System in discrete time
Equations (4.1) and (4.2) can not be use in a computer controlled enviroment.It is prefered to convert/transform the system matrixes into disctete time. Wewant in some way convert equations (4.1) and (4.2) to (4.3) and (4.4)
x(k + 1) = �x(k) + �u(k) (4.3)
y(k) = Cx(k) +Du(k) (4.4)
4.2.1 Calculation of discrete time matrixes
Let h be the discrete time steps, h = tk+1� tk. The time discrete system matrixand control matrix can be calculated with equations (4.5) and (4.6).
� = eAh (4.5)
� =
Z h
0
eAsdsB (4.6)
If the samplingtime h is small then � and � is calculated with the formulasfound in [33]
Calculation of �
=
Z h
o
eAsds = Ih+Ah2
2!+A2h3
3!+ : : :+
An�1hn
n!+ : : : (4.7)
� = I +A (4.8)
50
Calculation of �
� = B (4.9)
Matrixes C and D remains the same.
4.2.2 Discrete Input Covariance Matrix
We have the ideal control signal ~u(t) with noise �u(t).
u(t) = ~u(t) + �u(t) (4.10)
The covariance matrix of this noise is Q. When the continoustime system isdiscretisized the Q matrix must be recalculated. There are di�erent methodsto calculate the Qk matrix. In [33] and [5] we �nd two equations, (4.11) and(4.12).
Qk = BQBTh (4.11)
Qk =1
2
�ABQBTAT +BQBT
�h (4.12)
Triple Integrator Example
The Triple Integrator is described by the system
_x(t) = f x(t) + g u(t) (4.13)
_x(t) =
24 0 1 0
0 0 10 0 0
35x(t) +
24 0
01
35u(t) (4.14)
Hence the discrete model for the system is
x(k + 1) = �x(k) + �u(k) (4.15)
X(k + 1) =
24 1 h h2=2
0 1 h0 0 1
35X(k) +
24 h3=6h2=2h
35U(k) (4.16)
4.3 Nonlinear System
In a nonlinear system the state transitionmatrix is built of nonlinear functionssuch as trigonometric functions, higher order polynomials and square root terms.The nonlinear system is mainly built up by equation (4.17). The vehicle or thesystem is described by the f model function.
_x(t) = f(x(t); u(t); t) (4.17)
zn(t) = hn(x(t); t) + �zn(t) (4.18)
51
We may want to use di�erent sensor models. The sensor models are describedby the functions hn. If we only have one sensor we have only one sensor model,h0. Usually we have a lot of di�erent types of sensors in the system and therefordi�erent sensor models. In this thesis I used sensor models for velocity, positionand attitude updates.
u(t) = ~u(t) + �u(t) (4.19)
The expectated meanvalue for the noiseterms is equal zero.
E[�u(t)] = E[�zn(t)] = 0 (4.20)
The control noise signal covariance matrix is Q.
E[�u(i)�Tu(j)] = �ijQ(i) (4.21)
The observation noise signal covariance matrix is R.
E[�zn(i)�Tzn(j)
] = �ijR(i) (4.22)
And there is no correlation between the observation noise and the control noise.
E[�u(t)�Tzn(t)
] = 0 (4.23)
In equation (4.17) we have a nonlinear function f . We need to have a linearsys-tem like _x = Fx. This can not be done by linearisation of equation (4.17) atthe working point. We derives the
Linearisation of the state transition function f
F = rf = @f
@x=
266664
@f1@x1
@f1@x2
: : : @f1@xn
@f2@x1
@f2@x2
: : : @f2@xn
......
. . ....
@fn@x1
@fn@x2
: : : @fn@xn
377775 (4.24)
Linearisation statetransition function with respect to the control sig-
nal
G =@f
@u=
266664
@f1@u1
@f1@u2
: : : @f1@un
@f2@u1
@f2@u2
: : : @f2@un
......
. . ....
@fn@u1
@fn@u2
: : : @fn@un
377775 (4.25)
Linearisation of the observation function
H =@h
@x=
26664
@h1@x1
@h1@x2
: : : @h1@xn
@h2@x1
@h2@x2
: : : @h2@xn
......
. . ....
@hn@x1
@hn@x2
: : : @hn@xn
37775 (4.26)
52
4.3.1 Linearized Nonlinear System in continous time
The nonlinear system equation (4.17) and (4.60) have been linearized taking the�rst order derivatives with respect to the states x and the control vector u. Thelinearisation is done at the workingpoint x(t) and u(t) The result is equation(4.27) and (4.28)
� _x(t) =@f
@x�x(t) +
@f
@u�u(t) = F�x(t) +G�u(t) (4.27)
�z =@h
@x�x+ � = H�x (4.28)
4.3.2 Linearized Nonlinear System in discrete time
We calculate the discrete transition matrix and control matrix using equations(4.8) and (4.9).
�x(k + 1) = ��x(k) + ��u(k) (4.29)
�z(k) =H�x(k) (4.30)
4.4 The Complement Filter
In the previous chapters we have discussed the di�ent frames, bodyframe andnavigationframe, that are used during the navigation. We are interested in theposition, velocity and attitude of the airvehicle. A complement �lter have theproperty to estimate the error by cancelling the 'real' values. Suppose we wantto calculate the error in position. One position is given by the INS and one ismeasured with a GPS receiver. Both positions contains errors.
pins = preal + �ins (4.31)
pgps = preal + �gps (4.32)
The error �ins and �gps modelled as Gaussian noise with zero as meanvalue,p 2 N(0; �p). To estimate the position error we subtract equations (4.31) and(4.32) and end up with equation (4.33).
�p = pins � pgps = preal + �ins � preal + �gps = �ins � �gps: (4.33)
The standard deviation of �p is �2�p = �2ins + �2gps if �ins and �gps are uncor-rolated.
There exists two di�erent types of complement �lters.
� Complement �lter with feedback.
� Complement �lter without feedback
53
4.4.1 Complement �lter with feedback
The INS keeps track of the states of the aircraft. The error �lter matrixes arecalculated using the states in the INS. The �lter is linearized at the workingpoint given by the INS states. The error �lter uses Kalman�lter theory toestimate the error. We have external observations of the states. After each stepthe error is estimated and fed back to the INS. The error state is cleared. Thenew �lter matrixes is calculated. And a prediction of the error states is done.
+
-1
INS(P,V,A)
GPS(P,V)
KALMAN FILTER
Error estimates
Correctedoutput
IMU
innovations
4.4.2 Complement �lter without feedback
Here there is no feedback to the INS. Instead the error is cancelled at the out-put. The problem with this is that the INS states diverges if no error feedbackis given. The acceleration error will increase with O(t2) and after a while theposition will diverge. Therefor I haven't implemented this version.
(P,V,A)
(P,V) Error estimates
Correctedinertialoutput
+
+
-1
INS
GPSKALMAN FILTER
Inertialerrors
4.5 The Kalman Filter
This famous �lter technique was invented my Kalman and is very useful espe-cially in navigation and enviroments with gaussian noise. The �lter containsfour basic blocks in the �owchart. Imagine that you are sleeping in your roomand wake up in the middle of the night and the electricity is gone. The whole
54
house is dark and the only way for you to go to the kitchen is to use the in-formation you have about the enviroment , the initial position you have and assensors you only can use your feets and hands.
You almost know your current position and know that if you walk in a desireddirection you will be able to reach a wall or maybe a carpet that you can feelwith your feets.
As you reach something know by you like a carpet, a door, a starcase youhave an observation. You can directly use this information to estimate whereyou are and then predict what will happen if you walk in a special direction.This is repeated and in the end you will reach your goal.
In the story above we can isolate four di�erent states.
� Initialisation
� Prediction
� Observation
� Estimation
Flowchart of Kalman�lter
INITIALISEK=0
PREDICTION
OBSERVATION
ESTIMATION
K=K+1
We work in discrete time and after theestimation step we increase k by one and go back to the prediction step. Thedi�erent equation involved in these steps are (4.34) to (4.39).
Initialisation
We set x0 to a desired value and P0 to nonzeros in the diagonal and makes thematrix symetic along the diagonal, it is important that this matrix is possibleto invert.
55
Prediction
x̂k+1 = �kxk (4.34)
P̂k+1 = �kPk�Tk +Qk (4.35)
Observation
The observation matrix zk is measured by a sensor. The di�erence between thepredicted sensor value Hkx̂k+1 and the observation is called the innovation. Aperfect innovation vector is true gaussian distributed with zero mean.
inno = zk �Hkx̂k+1 (4.36)
Estimation
After the innovation have been calculated and all the sensor information havebeen added we can estimate the states of the sensor. The kalman gain is calcu-lated for every time step. The equation below is valid only for one observation.If more observation are used, another equation must be used. Read in [8].
Kk = P̂k+1HTk+1
�HkP̂k+1H
Tk +Qk
��1(4.37)
The kalman gain Kk have been calculated the predicted states are fused withthe observations. And we get a new optimal estimation of the states.
xk+1 = x̂k+1 +Kk (zk �Hkx̂k+1) (4.38)
The covariance matrix of the new states states is calculated using equation(4.39).
Pk+1 = (I +KkHk) P̂k+1 (I +KkHk)T (4.39)
4.6 The Information Filter
The Kalman�lter used in this thesis is a kalman�lter in information form. Herethe statevector and the uncertaiity matrix is not the same as in a traditionalkalman�lter. We here use the information state vector y with the correspondinginformation matrix Y, and there exists relations to convert between information-form and standard Kalman form. The relations are equation (4.40) and (4.41).
Y (k) = P (k)�1 (4.40)
The and the P matrix can not be singular, because we need to be able to calcu-late the inverse. The problem is very big if this matrix become bad conditionedand, the matrix is symetric and this is used in the numerical calculation to makethe matrix calculations more stable.
y(k) = P (k)�1(k)x(k) = Y (k)x(k) (4.41)
56
4.6.1 Prediction
At time k we have the statevector y and matrix Y, we can with this informationpredict what the states will be at time k+1 when the system have an inputwith noisematrix Q. The predicted values of y and Y can be calculated withequation(4.42) and (4.44)
Y (k + 1jk) = [F (k)Y �1(kjk)F T (k) +Q(k)]�1 (4.42)
L(k) = Y (k + 1jk)�1F (k)Y (kjk) = P (k + 1jk)F (k)Y (kjk) (4.43)
y(k + 1jk) = L(k)y(k) (4.44)
4.6.2 Observation
After the prediction step we fuse the �lter state with external observations, anobservation can for example be GPS position. The observation is the z vectorand with the uncertainty matrix R. With this the z vector and R matrix wecalculate the information observation vecor i and the I matrix using (4.45) and(4.46).
i(k) = HT (k)R�1(k)z(k) (4.45)
I(k) = HT (k)R�1(k)H(k) (4.46)
If more then one observation exists the information form have a very nice prop-erty that makes it possible to just all the observations, and this will make itvery easy to decentralize the �lter calculations.
i(k)SUM =
N=#OBSXn=0
= HT (k)R�1n (k)zn(k) =
N=#OBSXn=0
in(k) (4.47)
I(k)SUM =
N=#OBSXn=0
HT (k)R�1n (k)H(k) =
N=#OBSXn=0
In(k) (4.48)
4.6.3 Estimation
With the predicted values and the observations we can estimate the best in-formationstate vector and the information matrix. When we are working witha information�lter whis is done very easily with only the plus operator. Withonly one observation we use equations (4.49) and (4.50). Again it can be notedthat this makes a this �lter a very good candidate for decentralized systemswith multiple observations, equations (4.51) and (4.52).
y(k + 1jk + 1) = y(k + 1jk) + i(k) (4.49)
Y (k + 1jk + 1) = Y (k + 1jk) + I(k) (4.50)
57
y(k) = y(k + 1jk + 1) = y(k + 1jk) +N=#OBSX
n=0
in(k) (4.51)
Y (k) = Y (k + 1jk + 1) = Y (k + 1jk) +N=#OBSX
n=0
In(k) (4.52)
4.6.4 The Innovation
The innovation is the di�erence between the observation and the predicted value.The innovation can be tested with the �-square distribution. If the innovation isoutside the limits the observation are thrown away. Let �zk be the innovationwith Sk as the innovation covariance matrix.
�-square distribution test
�zTk S�1k �zk � & (4.53)
The value & is set to reject observation beyond 95% threshold. This is anexcellent to cancel multipath e�ects in the GPS solution. The �lter will remainin predicted state, or the next estimate will be the current prediction. If theobservations are rejected the navigation �lter runs only on the IMU and thatcan be a risk. It is here very important with a �lter that have been tunedcorrectly.
4.6.5 Fusion
There exists di�erent sensors that can be mounted on an UAV like GPS, airvelocity probes, altitude sensors (pressure), ccd cameras, radar and horizondetector. Every sensor can described by a sensor model. The observation matrixHi are calculated using that model. We can therefor have lot of di�erent sensorswith separate observation matrixes. The sensor observations are added to the�lter states when present.
58
4.7 A Linear Error Model
A very common linear error model is a model found in [37].
24 � _P
� _V
� _
35 =
24 0 I3x3 0
0 0 An�0 0 0
3524 �P�V�
35 (4.54)
Lets explain it with a �gure, we have the accelerations [aN ; aE ; aD] in navigationframe (NED-frame). The IMU is misaligned with small angles [�; �; ]. We wantto know the resulting acceleartion on the frame axes caused by the misalignment.
N
D
θ
ψ
ψ
aN
aD
aE
aN
N
E
From the �gure we calculate the resulting acceleration aN and we see that themisalignment angles and � will project the accelerations aD and aE to thex-axis (North). From this we put up the equations
_vN = aD sin � � aE sin (4.55)
If and � are small we can approximate the equation with
_vN = aD� � aE (4.56)
We get � _V = An � �. Were An is a skewsymetric matrix.
An� = [aN ; aE ; aD]T � I3x3 =
24 0 aD �aE�aD 0 aNaE �aN 0
35 (4.57)
The equation (4.54) is written out with all its terms. This model works �ne if thetrigonometric functions can be approximated with �rst order Taylor expansion.We can use this model in the leveling process. The misalignment angles can befound if the vehicle is stationary and if the readouts from the INS say that thevehicle is moving. The model is also a good error model for trucks and cars.
59
We can notice that the misalignment on a stationary vehicle will project theacceleration caused by the gravity forward if we have an small elevation angleand to the left if we have an bank angle. More about this can be read in [37]and [1]. The model was tested on the airvehicle but didn't work so �ne.
The model
266666666666664
� _N
� _E
� _D� _vN� _vE� _vD� _�
� _�
� _
377777777777775
=
26666666666664
0 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 0 aD �aE0 0 0 0 0 0 �aD 0 aN0 0 0 0 0 0 aE �aN 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0
37777777777775
26666666666664
�N
�E
�D
�vN�vE�vD��
��
�
37777777777775
+
26666666666664
000
�aN�aE�aD��
��
�
37777777777775
(4.58)
The model can be expanded to hold the Coriolis accelerations caused by therotation of the Earth. In this thesis these acceleration have been neglected.
4.8 Nonlinear Model using Eulerangles
At this stage we are ready to present a nonlinear model that describes an airve-hicle. We have a nonlinear system, equations (4.59) and (4.60).
_x(t) = f(x(t);u(t); t) (4.59)
y(t) = h(x(t);u(t); t) (4.60)
We take the equations (3.17),(3.25) and (2.39) and put these equatations into amatrix and gets (4.61) This gives a state transition matrix with the size 9x9.
f(x;u) =
26666664
24
cos � cos � cos � sin + sin� sin � cos sin� sin + cos � sin � cos cos � sin cos � cos + cos � sin � sin � sin� cos + cos � sin � sin � sin � sin� cos � cos� cos �
3524u
v
w
35
24
u
v
w
35 �
24p
q
r
35 +
24
� sin �cos � sin�cos � cos�
35 ge +
24axayaz
35
24
1 sin� tan � cos� tan �0 cos � � sin�0 sin� sec � cos � sec �
3524p
q
r
35
37777775
(4.61)
The state vector of this nonlinear model have the elements position,velocity inbodyframe and attitude in eulerangles.
x =�X Y Z u v w �
�T(4.62)
The timederivative of x is :
_x =@x
@t=�
_X _Y _Z _u _v _w _� _� _ �T
(4.63)
60
4.8.1 State transition matrix
The nonlinear state transition function is linearized. The matrix contains thepartial derivatives of f with respect to x.
@f
@x=
�@f
@X;@f
@Y;@f
@Z;@f
@u;@f
@v;@f
@w;@f
@�;@f
@�;@f
@
�(4.64)
�@f
@X;@f
@Y;@f
@Z
�= 09x3 (4.65)
�@f
@u;@f
@v;@f
@w
�=
26664
24
cos � cos � cos� sin + sin� sin � cos sin� sin + cos � sin � cos cos � sin cos � cos + cos � sin � sin � sin� cos + cos � sin � sin � sin � sin� cos � cos� cos �
35
I3x3 �
24p
q
r
35 =
24
0 r �q
�r 0 p
q �p 0
35
03x3
37775
(4.66)
@f
@�=
2666664
24
0 sin� sin + cos� sin � cos cos� sin � sin� sin � cos 0 � sin� cos � sin� sin � sin � cos� cos � sin� sin � sin 0 cos� cos � � sin� cos �
3524u
v
w
35
24
0cos � cos�
� cos � sin�
35 ge
24
0 cos � tan � sin� tan �0 � sin� � cos�0 cos� sec � � sin� sec �
3524p
q
r
35
3777775 (4.67)
@f
@�=
2666664
24� sin � cos sin� cos � cos cos� cos � cos � sin � sin cos� cos � sin cos� cos � sin
� cos � � sin� sin � � cos � sin �
3524u
v
w
35
�
24
cos �sin � sin�sin � cos �
35 ge
24
0 sin�(1 + tan2 �) cos�(1 + tan2 �)0 0 00 sin� sec � tan � cos � sec � tan �
3524p
q
r
35
3777775 (4.68)
@f
@ =
" 24� cos � sin� � cos cos �� sin sin � sin� sin cos � � cos sin � sin�cos � cos � � cos sin� + cos sin � cos� sin sin� + cos sin � cos�
0 0 0
3524u
v
w
35
06x1
#
(4.69)
4.8.2 Control matrix
@f
@u=
�@f
@ax;@f
@ay;@f
@az;@f
@p;@f
@q;@f
@r
�(4.70)
�@f
@ax;@f
@ay;@f
@az
�=
24 03x3
I3x3
03x3
35 (4.71)
61
�@f
@p;@f
@q;@f
@r
�=
266666666664
03x324 u
v
w
35� I
3x3 =
24 0 �w v
w 0 �u
�v u 0
35
24 1 sin� tan � cos� tan �
0 cos � � sin�0 sin� sec � cos � sec �
35
377777777775
(4.72)
G =@f
@u=
266664
0 0
I3x3
24 uvw
35� I3x3
0 C _� _� _ jpqr
377775 (4.73)
G =
26666666666664
0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 01 0 0 0 �w v
0 1 0 w 0 �u
0 0 1 �v u 00 0 0 1 sin� tan � cos� tan �0 0 0 0 cos � � sin�0 0 0 0 � sin� sec � � cos � sec �
37777777777775
(4.74)
The process noise is the noise of the input to the �lter. The inputs are the accelerationsand rotationrates in bodyframe of the vehicle.
�u(t) =��ax �ay �az �p �q �r
�T(4.75)
4.8.3 Observation Models
h(x) =
26666664
24X
Y
Z
35
24
cos � cos � cos� sin + sin� sin � cos sin� sin + cos � sin � cos cos � sin cos� cos + cos � sin � sin � sin� cos + cos� sin � sin � sin � sin� cos � cos � cos �
3524u
v
w
35
24
�
�
35
37777775
(4.76)
Position observation model
The position observation equation is:
�zXYZ =HXYZ�x+ �XYZ (4.77)
With the observation matrix HXYZ, derived from (4.76).
HXYZ =�I3x3 06x3
�(4.78)
62
Velocity obsevation model
Velocity observation equation and the corresponding observation matrix H _X _Y _Z.
�z _X _Y _Z = H _X _Y _Z�x+ � _X _Y _Z (4.79)
H _X _Y _Z =�03x3 Cbn 06x3
�(4.80)
Attitude observation model
�z�� = H�� �x+ ��� (4.81)
H�� =�06x3 I3x3
�(4.82)
4.9 Nonlinear Model with Quaternions
Another model tested is this model that uses quaternions. The quaternions havethe advantage that all we use polynomials of order two in the continous timetransision state matrix, equation (4.83). This matrix is built of three equationsnamely (3.17),(3.25) and (??).
f(x;u) =
2666666664
264
(e20 + e21 � e22 � e
23) 2(e1e2 � e0e3) 2(e1e3 + e0e2)
2(e1e2 + e0e3) (e20 � e21 + e22 � e
23) 2(e2e3 � e0e1)
2(e1e3 � e0e2) 2(e2e3 + e0e1) (e20 � e21 � e
22 + e23)
37524u
v
w
35
24u
v
w
35�
24p
q
r
35 +
24
2(e1e3 � e0e2)2(e2e3 + e0e1)
(e20 � e21 � e
22 + e23)
35 ge +
24axayaz
35
12
2664�e1 �e2 �e3e0 �e3 e2e3 e0 �e1
�e2 e1 e0
3775
24p
q
r
35
3777777775(4.83)
4.9.1 State transition matrix
@f
@x=
�@f
@X;@f
@Y;@f
@Z;@f
@u;@f
@v;@f
@w;@f
@e0;@f
@e1;@f
@e2;@f
@e3
�(4.84)
�@f
@X;@f
@Y;@f
@Z
�= 09x3 (4.85)
�@f
@u;@f
@v;@f
@w
�=
266664
264
(e20 + e21 � e22 � e
23) 2(e1e2 � e0e3) 2(e1e3 + e0e2)
2(e1e2 + e0e3) (e20 � e21 + e22 � e
23) 2(e2e3 � e0e1)
2(e1e3 � e0e2) 2(e2e3 + e0e1) (e20 � e21 � e
22 + e23)
375
I3x3 �
24p
q
r
35 =
24
0 r �q
�r 0 p
q �p 0
35
04x3
377775(4.86)
63
@f@e0
=
2666666666666666664
2e0
24
1 �1 11 1 �1�1 1 1
3524u
v
w
35
2e0
24�111
35 ge
12
2664
0 0 01 0 00 1 00 0 1
3775
24p
q
r
35
3777777777777777775
@f@e1
=
266666666666666666664
2e1
24
1 1 11 �1 �11 1 �1
3524
u
v
w
35
2e1
24
11
�1
35 ge
12
2664�1 0 00 0 00 0 �10 1 0
3775
24p
q
r
35
377777777777777777775
@f@e2
=
2666666666666666664
2e2
24�1 1 11 1 1�1 1 �1
3524u
v
w
35
2e2
24�11�1
35 ge
12
2664
0 �1 00 0 10 0 0�1 0 0
3775
24p
q
r
35
3777777777777777775
@f@e3
=
2666666666666666664
2e3
24�1 �1 11 �1 11 1 1
3524
u
v
w
35
2e3
24
111
35 ge
12
2664
0 0 �10 �1 01 0 00 0 0
3775
24p
q
r
35
3777777777777777775
(4.87)
4.9.2 Control matrix
@f
@u=
�@f
@ax;@f
@ay;@f
@az;@f
@p;@f
@q;@f
@r
�(4.88)
�@f
@ax;@f
@ay;@f
@az
�=
�I3x3
07x3
�(4.89)
�@f
@p;@f
@q;@f
@r
�=
26666666666664
06x324 u
v
w
35� I
3x3 =
24 0 �w v
w 0 �u
�v u 0
35
12
2664�e1 �e2 �e3e0 �e3 e2e3 e0 �e1�e2 e1 e0
3775
37777777777775
(4.90)
4.9.3 Observation Models
Position observation model
�zXYZ =HXYZ�x+ �XYZ (4.91)
HXYZ =�I3x3 07x3
�(4.92)
Velocity observation model
The velocity in the statevector x is represented in bodyaxis. The velocities(u,v,w) need to be transformed to navigation frame and is done with equation(4.93).
H _X _Y _Z =�03x3 Cbn 07x3
�(4.93)
64
Attitude observation model
Because we in use quaternions and the observations are in eulerangle represen-tation we need to do a convertion. The formula (2.45) is written in matrix fromand we get equation (4.94).
h�� (x) =
2664
arctan�2(e0e1+e2e3)e20�e
21�e
22+e
23
�arcsin (2(e0e2 � e3e1))
arctan�2(e0e3+e1e2)e20+e
21�e
22�e
23
�3775 (4.94)
This model must be linearized, this gives us (4.95)
H�� =hO3x6 @h�� (x)
@e0
@h�� (x)@e1
@h�� (x)@e2
@h�� (x)@e3
i(4.95)
@h�� (x)
@e0=
2664
2e1(e20�e
21�e
22+e
23)�4e0(e0e1+e2e3)
(e20�e21�e
22+e
23)
2+4(e0e1+e2e3)22e2p
1+4(e0e2�e3e1)2
2e3(e20+e
21�e
22�e
23)�4e0(e0e3+e1e2)
(e20+e21�e
22�e
23)
2+4(e0e3+e1e2)2
3775 (4.96)
@h�� (x)
@e1=
2664
2e0(e20�e
21�e
22+e
23)+4e1(e0e1+e2e3)
(e20�e21�e
22+e
23)
2+4(e0e1+e2e3)2�2e3p
1+4(e0e2�e3e1)2
2e2(e20+e
21�e
22�e
23)�4e1(e0e3+e1e2)
(e20+e21�e
22�e
23)
2+4(e0e3+e1e2)2
3775 (4.97)
@h�� (x)
@e2=
2664
2e3(e20�e
21�e
22+e
23)+4e2(e0e1+e2e3)
(e20�e21�e
22+e
23)
2+4(e0e1+e2e3)22e0p
1+4(e0e2�e3e1)2
2e1(e20+e
21�e
22�e
23)+4e2(e0e3+e1e2)
(e20+e21�e
22�e
23)
2+4(e0e3+e1e2)2
3775 (4.98)
@h(x)�� @e3
=
2664
2e2(e20�e
21�e
22+e
23)�4e3(e0e1+e2e3)
(e20�e21�e
22+e
23)
2+4(e0e1+e2e3)2�2e1p
1+4(e0e2�e3e1)2
2e0(e20+e
21�e
22�e
23)+4e3(e0e3+e1e2)
(e20+e21�e
22�e
23)
2+4(e0e3+e1e2)2
3775 (4.99)
65
Chapter 5
GPS-Global Positioning
System
66
5.1 GPS-Global Positioning System
The information about the GPS system is read in [6],[22], [17] and [20]. The GPSsystem is a worldwide navigation system based on radio transmitters placedin 26 satellites. The GPS system was developed by Department of Defence(DOD), USA. The satellites are placed in circular orbits around the Earth withan altitude of 20 200 km. There are six di�erent orbital planes. Each planemake an inclination of 55o relative to Earth's equator. The satellites are placedso that a receiver can track atleast four satellites with a 5o elevation mask abovethe horizon.
GPS working principle by Professor Bradford Parkinsson
The fundamental navigation technique for the GPS is to use one-way rangingfrom GPS satellites that are also broadcasting their estimated positions. Pseu-doranges are measured to four satellites simultaneously in view by matching(correlating) the incoming signals with a user-generated replica signal and mea-suring the received phase against the user's (relative crude) crystal clock. Withfour satellites and appropriate geometry, four unknowns can be determined;typically they are latitude, longitude, altitude and a correction to the user'sclock.
Orbits of GPS satellites
Pseudorange
The pseudorange is measured by correlating the incoming signal with a identicalsignal re-produced by the clock in the GPS receiver. With the pseudorange toatleast four satellites it is possible to calculate the receivers position in threedimension. One element in the solution vector gives an estimate of the receiverclock o�set.
67
Ideal case, 3 intersecting Point of solution.
spheres
If we had a perfect clock in both the satellite and the receiver and if the signalwasn't a�ected by noise then there would be really easy to calculate the receiversposition.
Because of bad crystals in the receivers, propagationdelays, gravity changesand lot more we have a system with four unknown variables. Three for positionand one for time o�set of the GPS receiver clock.
����������������������������������������
����������������������������������������
Area created by nonideal case. Additional sateliteneeded for solution
Area of solution 2D case
GPS-SPS and PPS
There are two di�erent GPS users, those with Y-code access which give PPS-Precise Positioning Service and those without access and those have SPS-StandradPositioning Service. The GPS signals are transmitted on two carrier frequencies,1575.42 MHz and 1227.60Mhz GHz. The marjor frequency is L1 at 1575.42Mhz.The signals are modulated by two di�erent pseudorandom noise codes. One witha frequency of 1.023Mhz called the C/A-code and one at 10.23Mhz called theY-code. The Y-code have a wavelength of 30m and is restricted for militarypurposes and can be used to generate very accurate position and velocity so-
68
lutions. The coarse/acquisition code (C/A-code) have a wavelenght of 300m isless accurate but free to use.
The L2 signal contains the Y-code modulation and are used to compensatethe transmitted signals for Ionoshpere delays. This can be done with two fre-quencies because the delay is frequency dependent. But Y-code is restricted onlyfor authorized users. Additional to this 50bit/s satellite messages are transmit-ted on L1 and is repeated every 30min. The message blocks are 1500bit long andcontains di�erent informations that are unique for every satellite. Informationabout satellite position, estimated trajectoria, ionosphere delay and status arereceived in these messages.
5.2 GPS-Signal errors
The GPS signal errors are of di�erent types, the main errors is
� Multipath
� Ionosphere delays
� Troposphere delays
� Signal attenuation
� Ephemeris error
� Satellite clock error
� Receiver clock error
GPS errors
Actual orbitSateliteclock
Ionosphere delay
Troposhere delay
Mulitipath
Teoretical orbit
The attenuation tree
GPS reciver clock
Good signal
69
5.2.1 Multipath
This occurs when the satellite signal bounces against like a wall and then re-ceived by the GPS receiver. The distance the signal have traveled is not theshortest geometric distance. Use a antenna with cut-o� angle and placement ofantenna such that it minimizes the problem. An reference station can also beused to cancel these errors. With an �lter it is possible to reject pseudorangesthat are beyond the �lter mask.
Fermats Principle
Fermat's Principle says that:If a wave travel between two points A and B the path will always give the min-imum time.
(ts � tr)P = min1
c0
ZP
n(s)ds; P 2 fAll possible pathsg (5.1)
5.2.2 Signal attenuation
Here the GPS signal is damped by di�erent materials, like forests........
5.2.3 Satellite motion
The perfect satellite orbits should be ellipses. The satellites are a�ected by theEarth gravity vector and this vector does not have the same magnitude all overthe planet. The satellites are also under in�uence of forces from other planets,the moon and the sunwinds. These forces can not be neglected. The paths of thesatellites are predicted and estimated by the ground stations. The harmonicscoe�cients for the angle inclination and orbit radius are update in each satellitecontinously.
5.2.4 Ionosphere propagation delay
When the GPS signal propagates through the Ionosphere the signal will collidewith free ions. The delay is dependent of the density of ions and the elevationof the signal (distance of travel). The number of ions is dependent on severalfactors like time, magnetic latitude and the sunspot cycle.
A method to calculate Ionosphere propagation delay was presented by Saas-tamoinen in 1973. This model does a serial expansion and says that the refrac-tion index n is frequency dependent.
n(s) = 1 +c1f2
+c2f4
+ : : : (5.2)
We integrate (5.2) over the signal path and get the error in pseudorange causedby the Ionosphere, see equation (5.3). The parameters b1 and b2 are transmittedfrom the satellite and this is a mode that is restricted only to the military. It isonly those with Y-code access that get PPS.
��I(f) =
ZP
(n(s)� 1)ds � b1f2
+b2f4
(5.3)
70
With this dual-frequency techinque gives a 1-2 ranging accuracy for well cali-brated receivers, more information can be found in [6] page 479.
5.2.5 Satellite clock error
The GPS uses atomic clock, cesium and rubidium oscillators. The drift in oneday of these clocks result in 10�8 s or about 3.5m error in range.
5.2.6 Troposhpere propagation delay
The Troposhpere closer to the Earth surface. The delay here is sensitive to useraltitude and signal elevation angle and can be modelled with a function thattakes temperature, water vapour and temperature as arguments.
5.2.7 GPS Receiver Clock Model
The GPS satellites have onboard atomic clocks but the normal receivers do nothave atomic clocks. Instead they have di�erent sorts of oscillators usually basedon di�erennt sort of crystals. This will cause the clocks to drift in time. Thiscan be modelled with a model introduced by Allan and found in [20]. Is is adouble integrator of whitenoise. Allan have in this modell two di�erent whitenoise sources with spectral densities Sb and Sf . Sb; Sf are called the ClassicalAllan variance parameters.
S
1 1
S
clockbiaserror
White noise
White noise+
Wc1
Wc2
clock drifterror
wc =
�wc1wc2
�= N
��00
�;
�Sb 00 Sf
��(5.4)
Typical values for the Classical Allan Variance parameters.
Variable Valueh0 2 � 10�19h�2 2 � 10�20Sf 2h0Sg 8�2h�2
The model estimates the frequency drift error and the frequency bias errorof the receiver clock. We have a model where the frequency rate are integratedfrom jerks, ( White noise) with a spectral density of Sf . The frequency o�set( bias) are the sum of integrated white noise and the frequency rate. It can bewritten in a continious time system like
_xc(t) =
�0 10 0
�xc(t) + wc(t) (5.5)
71
yc(t) = [10]xc(t) (5.6)
We make this to a discrete time system with a discrete time step of of �t.
xc(k + 1) = �cxc(k) + wc(k) (5.7)
xc = [�bc;�fc]T (5.8)
�c =
�1 �t0 1
�(5.9)
The process noise covariance matrix is
Qc =
"Sb�t+ Sf
�t3
3 Sf�t2
2
Sf�t2
2 Sf�t
#(5.10)
GPS-clock drift expressed in pseudorange
This �gure was calculated with a Matlab script using data from a �ight donein August 1999. The GPS position solution prepared by Ph.D. student BenGrosholsky.
0 20 40 60 80 100 12015
20
25
30
35
40
45
50receiver clock offset
t (s)
delta
tim
e (m
)
0 20 40 60 80 100 12014
15
16
17
18
19
20receiver clock drift rate
t (s)
delta
tim
e ra
te (
m/s
)
5.3 WGS-84 transformations
It is important to be able to do coordinate transformation from and to the nav-igation frame from the ECEF frame. The variables used in this transformationis listed below.
72
Variable
� Longitude� LatitudeRN Local Earth radiush Height above sea levela Earth semi major axis, Earth radius at equator.a=6378137.0b Earth semi minor axis, radius at poles.b=a(1-f)f Earth �attering.f=0.003352810664747
5.3.1 ECEF to geodetic transformation
Equations to transform ECEF coordinates [x; y; z] to geodetic coordinates [�; �; h].
p =px2 + y2 (5.11)
� = tan�1�z a
p b
�(5.12)
_e2 =a2 � b2
b2=
2f � f2
(1� f)2(5.13)
e2 = f(2� f) (5.14)
� = tan�1�z + _e2b sin(�)3
p� e2a cos(�)3
�(5.15)
RN =ap
1� e2 sin2 �(5.16)
h =p
cos ��RN (5.17)
� = tan�1�yx
�(5.18)
(5.19)
5.3.2 Geodetic to ECEF transformation
The local Earth radius is dependent of latitude because the �attering of theEarth. The Earth looks almost like an ellipse and the radius can be calculatedusing (5.20).
Local Earth radius
RN (�) =a2p
a2 cos2 �+ b2 sin2 �(5.20)
The cartesian coordinates becomes24 xyz
35ECEF
=
24 (RN + h) cos� cos�
(RN + h) cos� sin�
(a2
b2RN + h) sin�
35 (5.21)
5.3.3 ECEF vector to NED transformation
The calculated satellite positions and velocities need to be transformed to thenavigationframe. To do this we need the approximate longitude and latitude.
73
The transformation is done using equation (5.22), � is longitude and � is lati-tude.
C(�; �) =
24 � sin� cos� � sin� sin � cos�
� sin� cos� 0� cos� cos� � cos� sin� � sin�
35 (5.22)
Example
ECEF velocity to NED velocity frame24 _x
_y_z
35NED
= C(�; �)
24 _x
_y_z
35ECEF
(5.23)
5.4 Position Determination
The position of the receiver is solved by knowing the distance to at minimumfour satellites. The distance between receiver and satellites is just to measurethetime if takes for a radiosignal to be transmitted from a satellite to the receiver.Here it is important that the clock in the receiver and the satellite are syn-cronized. The pseudorange are the geometric distance between the receiver andthe transmitter. This geometric distance are calculated in ECEF frame, WGS-84 standard. Transformation from ECEF to NED frame must be done later, inorder to use the results in the navigation �lter.
If the position solution is determined with the same setup of satellites thesolution will be rather smooth. But if di�erent setup of satellites are used thiscan cause abrupt jumps in the solutions. The accuracy is highly dependent ofthe satellite positions.
GPS reveicer that is tracking four satellite signals.
��������������������������������������������������������������������������������
��������������������������������������������������������������������������������
time tu
EARTH
5 degreeshorizon
horizon
5 degrees
1
2 3
4
time t1
time t2 time t3
time t4
(x,y,z)
(X1,Y1,Z1)
(X2,Y2,Z2) (X3,Y3,Z3)
(X4,Y4,Z4)
74
Variable
�i Pseudorange to satellite i.c0 Speed of light(Xi; Yi; Zi) ECEF position of satellite i at transmission(xr; yr; zr) ECEF receiver position at time of receptionb Receiver clock bias�pi Error in pseudorange.li Direction unit vector between receiver and satellite.l̂i Predicted direction unit vector between receiver and satellite.�̂i(xk) Predicted pseudorange.
From the receiver we get the pseudorange to satellite i. The clock in the receiverare not perfectly syncronized with the satellite clocks so we add a time bias.The distance the wave transmitts over a time b is c0b.
�i =p(Xi � xr)2 + (Yi � yr)2 + (Zi + zr)2 + c0�b+ ��i (5.24)
Linearization of pseudorange �i to satellite i gives us equation (5.25) gives usthe unit direction vector to the satellite.
li =
�@�i@x
;@�i@y
;@�i@z
�(5.25)
A very nice property of li is that jlij = 1: The partial derivatives are listedbelow, equation (5.26) to (5.28)
@�i@xr
= � Xi � xrp(Xi � xr)2 + (Yi � yr)2 + (Zi + zr)2
(5.26)
@�i@yr
= � Yi � yrp(Xi � xr)2 + (Yi � yr)2 + (Zi + zr)2
(5.27)
@�i@zr
= � Zi � zrp(Xi � xr)2 + (Yi � yr)2 + (Zi + zr)2
(5.28)
We have predicted pseudoranges to the satellites �̂i(xk) and xr is the predictedposition of the receiver at time k. The di�erence between the predicted pseu-dorange and the measured pseudorange can be put down into formulas.
2664
�1�2...�n
3775�
2664�̂1�̂2...�̂n
3775 =
26664
@�1@xr
@�1@yr
@�1@zr
1@�2@xr
@�2@yr
@�2@zr
1
......
......
@�n@xr
@�n@yr
@�n@zr
1
37775264
�xr�yr�zrc0�b
375+
2664
���1���2...
���n
3775 (5.29)
Equation (5.29) is written in a shorter form with matrixes.
�� = G�x+��p (5.30)
75
�� =
26664
��1��2...
��n
37775 G =
26664l̂1 1
l̂2 1...
...l̂n 1
37775
�x =
2664
�xr�yr�zrc0�b
3775 ��� =
26664
���1���2...
���n
37775
(5.31)
Least square solution of position
The position of the receiver can be solved if four valid satellite signals are beingtracked by the GPS receiver. The �x vector have four unknown and we needatleast four pseudoranges to solve the system. This is none with the normalequation (5.32)
�x̂ = (GTG)�1GT�� (5.32)
If the signals have di�erent noise, we use (5.33). Where R is the measurementnoise of the signal.
�x̂ = (GTR�1G)�1GTR�1�� (5.33)
5.4.1 Dilution Of Precition
It is clear that the satellite geometry will a�ect the calculated position. Thecovariance of the solution is :
E[�x̂�x̂T] = (GTG)�1GTRG(GTG)�T (5.34)
If the pseudoranges have the same noise and if the noise of the pseudorangesare uncorrolate E[��i��j ] = 0; i <> j we can replace R with a scalar �R =E[��i��i. With this the resulting position covariance will be
E[�x̂�x̂T] = (GTG)�1GT�2RG(GTG)�T = �2R(G
TG)�1 (5.35)
If we list all elements of the matrix (5.35) we get:
�2R(GTG)�1 =
2664
E[�x�x] E[�x�y] E[�x�z] E[�xc0�b]E[�y�x] E[�y2] E[�y�z] E[�yc0�b]E[�z�x] E[�z�y] E[�z2] E[�zc0�b]E[c0�b�x] E[c0�b�y] E[c0�b�z] E[c0�b
2]
3775(5.36)
The (GTG)�1 matrix is called the dilution matrix. If we want the dilution inanother frame than ECEF we need to do a frame change. The frame change isdone with a matrix C. Is can be appropirate to use the dilution in navigationframe. In this case the C matrix will be the equation (5.22). Then the newdilute matrix will be.
A = C (GTG)�1CT (5.37)
76
Geometric Dilute Of Position Solution
GDOP =q�2x + �2y + �2z + �2b =
ptrace(A) (5.38)
Position Dilute Of Position Solution
PDOPq�2x + �2y + �2z =
pA11 +A22 +A33 (5.39)
Time Dilute Of Position Solution
TDOP = �b =pA44 (5.40)
Horizontal Dilute Of Position Solution
HDOP =q�2x + �2y =
pA11 +A22 (5.41)
Vertical Dilute Of Position Solution
V DOP = �z =pA33 (5.42)
5.5 Velocity determination
The measured frequency by the receiver will be dependent of the vehicle velocity.The doppler will a�ect the frequency in the way that if the distance betweenthe satellite and the receiver is decreasing the locked frequency will be higherthan the transmitted frequency. We can use this frequency shift to estimate thevelocity of the receiver. The velocity of the satellite can be calculated with veryhigh precision. The frequency shift follows equation (5.43)
fr = ft
�1 +
_�
c0
�= ft
�1 +
vr cos
c0
�(5.43)
Doppler e�ect
Velocity vectorψ
GPS signal
As we see the dopplershift of the frequency is highly dependent of the angle
77
between the velocity vector and the inline sight vector to the satellite. Themodel is inded more complex than this. We must take in calculation that thesatellite also have a velocity and a insted of vr cos we use (vr � vi) � li, wherevi is the satellite velocity and li is the inline sight vector. Equation (5.43) canbe rearranged and we solve _�
_� = c0
�1� fr
ft
�(5.44)
Pseudoranges and pseudorange rates
uρρ
ρPCA
Receiver
s
u
Vu
ρρ ρ= ρ
ρ
s +
Vs
Vs
If the GPS satellite signal is tracked we can use integration of the dopplershifted frequency to keep track of the actual position, see equation (5.45). Ifthe position is well known and the satellite signals are locked this is a very goodway of position estimation.
�� =
Z t+�t
t
_�dt =
Z t+�t
t
(vr � vi) � lidt = c0ft
Z t+�t
t
(ft � fr)dt (5.45)
2664
_�1_�2..._�n
3775�
26664
_̂�1_̂�2...
_̂�n
37775 =
26664
@�1@xr
@�1@yr
@�1@zr
1@�2@xr
@�2@yr
@�2@zr
1
......
......
@�n@xr
@�n@yr
@�n@zr
1
37775264
� _xr� _yr� _zrc0�b
375+
2664
���1���2...
���n
3775 (5.46)
� _�i = _̂�i � _�i =�li 1
� � �v�f
�+�� _�i (5.47)
5.6 Attitude determination
The GPS receivers can be used to give periodic position an attitude updatesto the INS, Inertial Navigation System. With three receivers and antennas
78
mounted on the frame of the vehicle it is possible to estimate the attitude.This is presented in [36]. By measuring the phaseshift di�erence between thereceived signals it is possible to calculate the attitude with a resolution downto 0.5 degrees.
Four GPS antennas placed on the airframe
A
A
A
A
X
INS &GPS
A=GPS ANTENNA
Y
5.7 GPS interface software
The receiver software consists of three modules, the GPS binary, Data I/Oand the User Interface. The software can control multiple receivers and outputnavigation data. With a ground receiver the onboard software can compensatethe outputs using Di�erential GPS technique.
79
5.8 Coupled INS/GPS
5.8.1 Uncoupled INS/GPS system
Here the information given by the GPS is estimated position and speed of thereceiver. This if feed into the integration processor for system update. Fromthe INS we also get acceleration, velocity and position of the system. Theinformation from the GPS and INS is fused together for the optimal estimateusing a Kalman Filter.Antenna
GPSReceiver
GPS
INS
processor
PositionVelocityTime
PositionVelocityAtt. AngleAcc
Angular speed
PositionSpeed
Time
Integration
Kalman Filter
Uncoupled GPS/INS
pseudorange
deltarange
Att. angle
IMU
5.8.2 Loosley coupled INS/GPS system
Here we have a feedback loop to the GPS receiver. The external navigation�lter have an estimation of position, velocity and attitude. This information isfed to the receiver to aid the internal �lter of the GPS. Acceleration is fed tothe GPS kalman�lter that estimates the position, velocity and time.Antenna
GPSReceiver
GPS
INS
processor
PositionVelocityTime
PositionVelocityAtt. AngleAcc
Angular speed
PositionSpeed
Time
Integration
Kalman Filter
pseudorange
deltarange
PositionSpeedAttackanglefor code and carrier aiding
acceleration
Loosely coupled GPS/INS
Att. angle
IMU
Navigation filter
5.8.3 Tightly coupled INS/GPS system
In this system solution we use pseudorange and pseudorate information fromthe satellites to aid the navigation �lter. This system will be more integratedthan the other solutions and the information from less than four satellites willcontribute to the accuracy of the position and velocity. The information sentfrom the receiver is pseudorange and pseudorange rate to di�erent satellites.
80
Position, Velocity and attitude are fed back to the GPS receiver from the navi-gation �lter strictly to be used for code and carrier aiding.Antenna
GPSReceiver Integration
processor PositionSpeed
Time
INS
PositionVelocityAtt. angle
Tightly coupled GPS/INS
Att. angle
81
Chapter 6
The Implementation
82
6.1 Hungarian Notation
I have used something called hungarian notation. This way of writing the soft-ware is very powerful when you need a simple and e�ective way of identify thetypes of the variables, classes and the type returned by functions.
Hungarian notation used in the software
Variabletype Hungarian Notation Example
array a char caName[10]boolean b bool bStatuschar c char cTmpclass C class Ctmp alt. class Ttmpdouble r double rTmpint i int iTmplong l long lTmp�oat sr �oat srTmpobejct o class Ctmp oTmppointer p char *pcNameshort s short sTmpstruct S struct Stmpunsigned u unsigned int uiTmpvoid v void vTmp( int i)
The letters i,j,k are used for indexing in nested loops. If more index variablesare needed they should be de�ned as the rules above. Variables of the sametype begin with the letter decribing the type and then a number. Like if youneed two varibles of type double, r1 and r2. Constants are de�ned with capitalletters like the math constant � should be represented by MATH_PI.
6.1.1 C++ example using hungarian notation
class Ccomplex
{
double rImag, rReal;
public:
Ccomplex(double rAbs, double rArg);
}
Ccomplex::Ccomplex(double rIm, double rRe)
{
rImag = rAbs*rSin(rArg);
rReal = rAbs*rCos(rArg);
}
void main( void)
{
Ccomplex *poNumber= new Ccomplex( 0.1,0.1);
delete poNumber;
}
83
6.2 The Software
6.2.1 The �ow of the software
The main program of the navigation loop is a Information Filter written genericin C++. The �lter estimates the errors in position, velocity and attitude. Theprediction step uses the states given by the INS to calculate the system matrixes.The �lter matrixes are calculated inside an object that represents the vehicle,the observation models are updated if needed.
The software �ow
� Read measurements from the IMU hardware.
� Convert to true accelearations and rotationrates.
� Transform body acceleration and rorationrate vectors to navigation frame.
� Velocity and position integration. Update attitude.
� Exists external GPS observations ? If yes:
� Add observations to error �lter.
� Estimate position, velocity and attitude.
� Feedback errors to the INS.
� Clear errors in error �lter.
� Calculate the new state transition matrix, calls the vehicle model.
� Predict 0.1s ahead, external observations with 10Hz rate.
6.3 Classes
6.4 For The IMU
6.4.1 The Sensor Class
� Set reference temperature
� Set sensor temperature
� Set bias
� Set scale factors
� Set standard deviation
� Set normal vector
� Get normal vector
� Get scale factor
� Rescale
84
6.4.2 The Sensor Cluster Class
This class contains several instances of the Sensor Class. The number of sensorsin each class can be choosed arbitary. An IMU normally have a cluster ofaccelerometers and one gyro-cluster. This class contains the important functionsto setup the sensors in a sensor frame.
6.5 The Readdata Class
� Read Watson IMU data �le
� Read Tetrad IMU
� Read Tetrad IMU data �le
� Read Motion-Pack IMU data �le
� Read GPS
6.6 The INS Class
Class that have the implemeted functions to keep track of a vehicle, navigation.The input to this class is from the IMU and error feedback from the error �lter.The �lter contains a lot.
� Set
� Set acceleration
� Set velocity
� Set position
� Set attitude
� Get
� Get acceleration
� Get velocity
� Get position
� Get attitude
� Error feedback
6.7 The Angle Class
This Class contains a lot of virtual functions that are used for dataconversionbetween di�erent angle representations. The angle class are mainly used in thecode but the subclasses Eulerangle, Cbn and Quaternion classes are those whoare created as objects in the program. All three classes have been implementedand gives almost the same result, but the Eulerangle class is the most accu-rate because it uses a fourth order Runge-Kutta integrator to keep track of
85
the angles. The picture below gives an idea how the implementation is done.
EULER Cbn QUATERNIONS
ANGLE CLASS
6.7.1 The Eulerangle Class
There are several methods to keep track of the angle, one of them is simplyto transform the body turn rates (pqr) to navigation frame which gives usomega, see equation (2.39), this is integrated with discrete integrators and theRunge-Kutta fourh order integrator is used as default. However integrators withTrapetz and Simpsons Rule can also be used with an additional switch if loveraccuracy is prefered.
6.7.2 The Cbn Class
This class keeps the rotationrates in bodyframe and uses equations (2.32) or(??) to update the Cbn matrix.
6.7.3 The Quaternians Class
In this class equations (2.58) or (??) are used to update the Quaternion angles.
6.8 The Linear System Class
The basic class for the �lters is the linear equations presented in section 4.2.This class implements all equations needed to use a linear equation system.
6.8.1 The Kalman Filter Class
This class is derived from the linear system class and implements the KalmanFilter in section 4.5.
6.8.2 The Information Filter Class
Derived from the linear system class. The class implements the information�lter presented in section 4.6
6.8.3 The Innovation Class
Functions implemented to operate on the innovations. Functions to calculateeigenvalue, eigenvectors, standard deviatons. �-square distribution tests.
86
6.9 The Vector Integration Class
6.9.1 The Trapetz Vector Integration Class
Implementation of the trapetz integration rule.
6.9.2 The Simpson Vector Integration Class
Implementates the Simpson rule of numerical integration.
6.9.3 The Runge-Kutta Vector Integration Class
Fourth order numerical integration with an error of order O(h5).
6.10 The Model Class
The idea with this class is to make it very simple to change the vehicle modelwithout doing a lot of changes in the existing sourcecode. By putting isolatingthe model within a class we just have to derive a new subclass of the Cmodelclass to test a new model in the software. The importand functions are declaredas virutal and must be de�ned in the new class, because they are very modeldependent. Three di�erent models were implemented and tested in this thesis.
� Linear Class
� Eulerangle Class
� Quaternionangle Class
The header �le of the Cmodel class looks like this.
class Cmodel
{
public:
DoubleMatrix oF;
DoubleMatrix oH[16];
DoubleMatrix oG;
public:
Cmodel::Cmodel(double rSampleTime);
Cmodel::~Cmodel();
virtual void vCalcF( void);
virtual void vCalcG( void);
virtual void vCalcHn( int n=0);
virtual void vUpdate( DoubleVec oX, DoubleVec oU);
virtual bool bNeedUpdate( void);
};
� Calculate F matrix
� Calculate G matrix
� Calculate H matrixes
87
6.10.1 The Quaternions Model Class
This class is a subclass of Cmodel, and here the Quaternion model in section4.9.
6.10.2 The Eulerangle Model Class
This class is a subclass of Cmodel, and here the Quaternion model in section4.8.
6.10.3 The Linear 9 State Model Class
This class is a subclass of Cmodel, and here the Quaternion model in section??.
6.11 Other Classes
6.11.1 The Vector Statistics Class
This class contains simple statistical functions to operate on the DoubleVectorclass. Functions that calculates the mean, standard deviation and variance isimplemented.
6.11.2 The Fast Trigonometric Class
6.11.3 The Earth Class
6.11.4 The Serial Communication Class
Initially I worked with the Tetrad IMU. The communication with the IMU isdone through a serial interface at 115200 Baud and I developed this class to beable to have di�erent instances for di�erent serial communication ports. Withthis class you create instances for the di�erent serial communication ports andchooses the desired baudrate, parity, port number and bit length.
6.11.5 The Firmware Class
To commuicate with the Tetrad IMU you need a very special communicationprotocol. This most basic commands for this protocol is found in table ??. Withthe Tetrad you can communicate over a CAN-bus interface and a serial interface.At the moment if a instance of this class is created the class communicates overa with a serial port instance that is created.
6.11.6 The Logging Class
A class for logging have been made, this class
The MatlabTM Logging Class
This class is derived from The Logging Class, and is contains functions to printvectors and matrixes to logging �les.
88
6.12 The target computer
The hardware in the airframe so far is a single computer, see section 8.1.2.To this computer all the sensors and receivers are connected, total four GPSreceivers and at maximum two IMU's. At the moment the solutions is postpro-cessed from data�les that have been logged during the test�ight.
89
Chapter 7
Results
90
7.1 Test�ight
The test�ight was taken place on a farm north of Goulburn in NSW. The farmis owned by the University of Sydney, the farm area is very large and will beperfect for test�ights as the project proceed.
Brumby takeo�
There was a �ight which the inertial system and the GPS in November 1999 andthe �ight was named the CCD FLIGHT. During this �ight measurements wascollected and stored and in some ways processed by the onboard computer sys-tem based on the PC104 standard. Under previous �ights there have been someproblems to store data on a physical harddisc. In this �ight the information wasstored in RAM discs. The �ight was successful and data from the �ight could bedownloaded through a TCP/IP connection to a external computer and storedon harddisc. The GPS/INS data was later processed and I wrote a matlab scriptto show the trajectoria �own during the test�ight. The aircraft position andvelocity were calculated using the data from the GPS receiver. The �gures showthe aircrafts position in navigation frame, using NED frame ( North, East andDown).
91
Flight path plotted in three dimensions
−800−600
−400−200
0200
400
−600
−400
−200
0
200
400−50
0
50
100
150
200
250
East
Flightpath in 3 dimensions
North
Alti
tude
3D �ights zoomed at turns
The 3D visualization of the �ight contains both the GPS observations and theesitmated position of the UAV. In these �gures zoomed from the one above wecan clearly see di�erences in the curves.
92
Flight path plotted in two dimensions
−500 0
−400
−200
0
200
East
Nor
th
INS POSITION (m)
−400 −200 0 200
−400
−200
0
200
North
Dow
n
INS
−500 0
−400
−200
0
200
East
Ddo
wn
INS
−1000 −500 0 500−600
−400
−200
0
200
400
East
Nor
th
GPS OBSERVATION
93
7.2 The Results
The logged data from the test�ight were processed by the navigation �ltersoftware. This software creates several outputs that are later processed byMatlabTM scripts to create the �gures presented in the following sections. Theimplemeted �lter estimates the position of the UAV and a special MatlabTM
script was written to simulate the �ight using this data.
7.3 Tetrad and Mpac IMU solutions
In one �ight two IMU's where mounted in the airframe. The Motion-Pack IMUis the standard IMU in the airvehicle. The Tetrad is a IMU under developementand the researchers needed some real data. The Tetrad IMU was mountedin the airframe together with the Motion-Pack IMU. The idea with this wasto calculate how accurate the Tetrad are in a real application compared witha reference IMU. The logged data are used to calculate the acceleration androtationrate in body frame.
The �gures shows a ten secound slice of the calculated acceleration androtation rates.
7.3.1 IMU accelerations
As we see here the Tetrad IMU is very sensitive. The measurements have aless of noise in the signal compared to the Motion-Pac IMU. The Tetrad aremounted on vibration dampers and is more heavy than the Motion-Pac IMU,this is a mechanical low-pass �lter. The Motion-Pac is lighter and follows theoscillation of the airframe and it can also be that the airframe resonates at thepoint where this IMU was placed.
But overall, we can clearly see that the signals follows the same average tra-jectoria.
0 1 2 3 4 5 6 7 8 9 10−2
0
2
4
time
Bod
y X
Tetrad IMU Acc (m/s2)
0 1 2 3 4 5 6 7 8 9 10−4
−2
0
2
time
Bod
y Y
0 1 2 3 4 5 6 7 8 9 10−20
−15
−10
−5
0
time
Bod
y Z
0 1 2 3 4 5 6 7 8 9 10−2
0
2
4
time
Bod
y X
Mpac IMU Acc (m/s2)
0 1 2 3 4 5 6 7 8 9 10−6
−4
−2
0
2
time (s)
Bod
y Y
0 1 2 3 4 5 6 7 8 9 10−30
−20
−10
0
10
time (s)
Bod
y Z
94
Rotationrates compared from two IMUs
The Tetrad-IMU was mounted on vibrationdampers. The angle oscillations willbe damped by the dampers and that the Tetrad-IMU infact is very heavy. TheMotion-Pac IMU is lighter and follows the motion more and is therefor it canbe more sensitive. The noise may be caused by the ADC card and do not needto be mechanical, electrical e�ects like disturbance from the electrical ignitionof the engine.
0 1 2 3 4 5 6 7 8 9 10−1
−0.5
0
0.5
1
time
Bod
y X
Tetrad IMU pqr (rad/s)
0 1 2 3 4 5 6 7 8 9 10−0.2
−0.1
0
0.1
0.2
time
Bod
y Y
0 1 2 3 4 5 6 7 8 9 10−0.2
0
0.2
0.4
0.6
time
Bod
y Z
0 1 2 3 4 5 6 7 8 9 10−1
−0.5
0
0.5
1
time
Bod
y X
Mpac IMU pqr (rad/s)
0 1 2 3 4 5 6 7 8 9 10−0.4
−0.2
0
0.2
0.4
time (s)
Bod
y Y
0 1 2 3 4 5 6 7 8 9 10−0.4
−0.2
0
0.2
0.4
time (s)
Bod
y Z
7.4 Navigation �lter results
The error�lter written in this thesis is in informationform and uses a nonlin-ear state space model. This model is linearized after every estimation stepof the Kalman�lter. The external observations feed to the �lter is GPS posi-tion/velocity and attitude of the aircraft. One way to measure the error betweenthe �lter and the observation is the innovation. Here I present the the innova-tions of the observations. The solid line in the plots are the 2�xn boundaryscalculated from state matrix P. The outer dashed line represents the 2� bound-ary calculated from the innovation covariance matrix.
The following standard deviations were used
SIGNAL/OBSERVATION STANDARD DEVIATIONIMU PQR 3:2=
p40:0 Degrees/s
IMU ACCELERATION 0.4 m=s2
GPS POSITION 3.0 mGPS SPEED 2.0 m/sGPS ATTITUDE 2.0 Degrees
Standard deviations used for the low pass �ltered data
SIGNAL/OBSERVATION STANDARD DEVIATIONIMU PQR 1:2=
p40:0 Degrees/s
IMU ACCELERATION 0.1 m=s2
GPS POSITION 3.0 mGPS SPEED 2.5 m/sGPS ATTITUDE 2.5 Degrees
95
7.5 IMU-Inertial Measurement Unit
7.5.1 PSD of Inertial Sensors
An interesting aspect of the sensors is to know how they are a�ected by sys-tematic error such as vibrations and misalignment. The IMU with the inertialsensors is rigid mounted inside the airframe and even if the IMU is very wellisolated from vibrations with vibration dampers, the sensors will measure lot ofthe engine vibrations and the vibrations caused by the eigenfrequencies of theairframe. My idea here is to use FFT1 with a time axis and calculate the PSD�surface� for each sensor. In this �ight the MotionPac-IMU, datasheet 8.1.3 wasused. The PSD results can be seen in the section 7.5.1. The �gures reveals atrace of the engine rpm clearly in atleast three �gures: gyro 2,acceleromter 1and accelerometer 3. With some imagination we can see that the engine rotationper minute is � 60*50 to 60*130 rpm. Between zero and � 150 seconds we cansee heavy signals in the low frequency band, 50Hz and below. This is repetedat 600 seconds during landing and this is probably both the engine at low rpmand the aircraft taxing on the runway. The runway at the farm in Goulburn farfrom perfect. It is not prepared in anyway and is only a �at grass�eld and cannot be compared with a standard runway. The airframe is rater light and thewheels of the aircraft is small so even small di�erences in the grass�eld will bemeasured by the IMU.
From this I think that the engine pistons are placed in vertial direction,because pitch angle have heavy signals and and the same for the vertical ac-celerometer.
PSD of the rate gyros
� Gyro 1 - x-axis
� Gyro 2 - y-axis
� Gyro 3 - z-axis
The PSD of gyro 2 we can see the oscillations, strong signals in the low fre-quency, when the aircraft is taxing on the ground. Flips with the wings becauseof a rough grass�eld.
0
200
400
600
800
1000
0
50
100
150
200
−15
−10
−5
0
5
Time(s)
Gyro 1
Freq (Hz)
log(
psd)
0
200
400
600
800
1000
0
50
100
150
200
−15
−10
−5
0
5
Time(s)
Gyro 2
Freq (Hz)
log(
psd)
1Fast Fourier Transform
96
0
200
400
600
800
1000
0
50
100
150
200
−15
−10
−5
0
5
Time(s)
Gyro 3
Freq (Hz)
log(
psd)
PSD of the accelerometers
� Accelerometer 1 - x-axis
� Accelerometer 2 - y-axis
� Accelerometer 3 - z-axis
0
200
400
600
800
1000
0
50
100
150
200
−15
−10
−5
0
5
10
Time(s)
Accelerometer 1
Freq (Hz)
log(
psd)
0
200
400
600
800
1000
0
50
100
150
200
−10
−5
0
5
10
Time(s)
Accelerometer 2
Freq (Hz)
log(
psd)
0
200
400
600
800
1000
0
50
100
150
200
−20
−10
0
10
20
Time(s)
Accelerometer 3
Freq (Hz)
log(
psd)
97
7.5.2 IMU Acceleration Solution
The largest acceleration is in the z direction. This is airframe up/down. Atapproximate 170 s we see accelerations in x from acceleration during takeo�.The z-axis acceleration is more hidden in noise.
100 150 200 250 300 350 400 450−10
0
10
20
time (s)
Bod
y X
IMU ACCELERAION (m/s2)
100 150 200 250 300 350 400 450−40
−20
0
20
40
time (s)
Bod
y Y
100 150 200 250 300 350 400 450−40
−20
0
20
time (s)
Bod
y Z
98
7.5.3 Body frame velocity
The velocity of the airvehicle can be given in bodyframe. The forward velocity[u; 0; 0] have the highest magnitude. I thought that the sideway velocity [0; v; 0]should be zero, but as the aircraft does sharp turns this will a�ect the aircraft toslip sideways. The aircraft has a up/down velocity [0; 0; w] because the aircraftis �ying with an angle of attack. If the aircraft is �ying to slow the aircraft willstall and the down velocity will increase rapidly.
The maximum speed of the airframe is 185 km/h. From the �gure we cansee that the velocity u reaches a magnitude of about 45 � 3:6 � 160 km/h
Body frame velocity
100 150 200 250 300 350 400 450−20
0
20
40
60
Time(s)
u (m
/s)
Body frame velocites during flight
100 150 200 250 300 350 400 450−10
0
10
20
Time(s)
v (m
/s)
100 150 200 250 300 350 400 450−5
0
5
10
15
Time(s)
w (
m/s
)
99
7.5.4 Body frame velocity angles, slideslip and angle ofattack
150 200 250 300 350 400 450−15
−10
−5
0
5
10
15
20
Time (s)
atan
(v/u
)
Body velocity angles
150 200 250 300 350 400 450−10
0
10
20
30
Time(s)
atan
(w/s
qrt(
u2 +
v2 )
100
7.5.5 IMU Rotationrate Solution
The rotationrate p and q looks quite ok except for hugh noise in the measuredsignals. The rotationrate r, nose left/right, have some nasty spikes, and theyare all distributed as clockwise rotation. The spikes can be a big problem ifthe gyros are modelled with low noise. The spikes may be caused2 by the ADCcard used by the PC104 onboard computer. Di�erent methods can be appliedto eliminate the spikes. The �lter as it is will compensate this �noise� and willbe eliminated through the error feedback to the INS.
7.5.6 INS-Inertial Navigation System
The Inertial Navigation System provides the user and navigation �lter with
� acceleration
� velocity
� position
� angle velocity
� attitude
2Ben Grosolsky at the Aeronautical Deparment said he had some trouble with reading the
status �ag from the ADC card
101
7.5.7 INS acceleration
This is the acceleration solution provided by the INS. The acceleration is cal-culated using the IMU acceleration and subtracting the gravity vector and cen-tripetal acceleration.
In the �gures we see the acceleration in north, east and down direction. Herethe vehicle is view as a rigid body under these accelerations in an inertial frame.The dynamics of the airframe have been cancelled at this stage.
Navigation frame acceleration
100 150 200 250 300 350 400 450−40
−20
0
20
40
time (s)
Nor
th
INS ACCELERATION (m/s2)
100 150 200 250 300 350 400 450−40
−20
0
20
40
time (s)
Eas
t
100 150 200 250 300 350 400 450−40
−20
0
20
40
time (s)
Dow
n
102
7.5.8 INS velocity
From these �gures we can see that the UAV �ies in a symetric path. It is veryhard to visualize how the �ight path look like in tree dimensions. The last�gure is the absolut value of [u; v; w], as we see the velocity reaches the samemagnitude as the velocity in body frame.
Navigation frame velocity
100 200 300 400 500−40
−20
0
20
40
60
time (s)
Nor
th
INS Velocity (m/s)
100 200 300 400 500−60
−40
−20
0
20
40
time (s)E
ast
100 200 300 400 500−10
−5
0
5
10
time (s)
Dow
n
100 200 300 400 5000
10
20
30
40
50
time (s)
Vel
ocity
(m
/s)
103
7.5.9 INS omega, eulerangle velocities
This is the angle velocities of the vehicle in navigation frame. We see thatthe spurioses from the body frame rotationrate [0; 0; r]T have been transformeddown and are visible in the pitch and yaw rates. From the yaw rate we see thatthe vehicle almost always turns to the left.
Left turn in navigation frame
1. Starts with wings in horizontal position.
2. Tips the wing to the left ( negative yaw velocity).
3. Does a left turn and then back
4. Rotates the wings back to horizontal position ( positive yaw velocity).
Tips the wing to the left ( negative yaw velocity). to horizontal position.
INS frame angle velocities
104
7.5.10 INS attitude
When � is plotted over time we see that sometimes the angle drops to �50o.This is when the aircraft does a sharp turn. At some points the roll is near�90o and then the wings are almost in vertical position. At 360 and 400 s wecan see very sharp turns and that the z-axis accceleration [0; 0; az]BODY have avery high magnitude here.
100 150 200 250 300 350 400 450−100
−50
0
50
100
time (s)
Phi
INS ATTITUDE (Degrees)
100 150 200 250 300 350 400 450−20
0
20
40
time (s)
The
ta
100 150 200 250 300 350 400 450−400
−200
0
200
400
time (s)
Psi
105
7.5.11 The Position Innovation
A good innovations signal is white noise. No bias and all frequencies represen-teed. The innovation looks very good but it seems like the GPS receivers loosestrack of one or more satellites during the �ight, especially in the sharp turns.This can explain the �jumps� in the GPS solution. This observation noise is notnormaldistributed in some parts and this is a problem. Perhaps this noise canbe modelled with Markow Chains.
Reasons for bad GPS solutions
� Bad GPS signals, to much noise.
� Lost track of one or more satellites.
� Begin to track more satellites.
� The measurement noise of the pseudoranges are not gaussian.
If you look at the �gure below and say at 330 s, you can see a heavy spikethat is visualized in section 7.6. This innovation spuriose can be masked awayif a tighter mask is used. Now the limit is at 3�. If 2� was used this wouldprobably be masked away.
Position Innovation
100 150 200 250 300 350 400 450−10
−5
0
5
10
time
Nor
th
INNOVATION POSITION (m)
100 150 200 250 300 350 400 450−10
−5
0
5
10
time
Eas
t
100 150 200 250 300 350 400 450−20
−10
0
10
time
Dow
n
106
7.5.12 The Velocity Innovation
The vehicle velocity are measured by the GPS receivers and are a very accurate,down to a resolution of 0.02-0.03 m/s. It seems that there were some kindof lag in this information and an higher standard deviation was put on theseobservations.
Airvehicle Velocity Innovation
100 150 200 250 300 350 400 450−5
0
5
time (s)
Nor
th
INNOVATION SPEED (m/s)
100 150 200 250 300 350 400 450−5
0
5
time (s)
Eas
t
100 150 200 250 300 350 400 450−5
0
5
time (s)
Dow
n
107
7.5.13 The Attitude Innovation
On Brumby airframe,see 1.2.2, four gps antennas are placed all over the airframe.One antenna on each wing, one near the nose and one at the back. Withthese four antennas and the GPS receivers and some software it is possibleto calculate a GPS attitude solution, look in ?? for more information. Theattitude observations are added to the INS error �lter after takeo�. The reasonfor this is that the airframe bounces a lot during taxing and takeo�. Only afew attitude observations are feed to the �lter. The attitude algorithm rejectsa lot of solutions that are obvious wrong. In the �gure below we have somecluster of observations at 260s and 360s. We can see from the �gure that theattitude angles of the �lter are not a�ected that much by the GPS observations.The GPS attitude observations seems to be two degrees o� in roll, pitch andyaw. My explanaition is that the IMU or the GPS antennas are not calibratedagainst the airframe.
Airvehicle Attitude Innovation
100 150 200 250 300 350 400 450−5
0
5
time
Rol
l
INNOVATION ATTITUDE (Degrees)
100 150 200 250 300 350 400 450−10
−5
0
5
10
time
Pitc
h
100 150 200 250 300 350 400 450−10
−5
0
5
10
time
Yaw
108
7.6 The Flight Playback
A MatlabTM script was written to do playback simulation of the �ight. Theinertial sensors and the GPS information are logged to harddisk during the�ights. This data have been preprocessed and then run through the navigation�lter. The output from this �lter is used for the �ight playback from takeo�until landing. This gives an idea how accurate the produced �lterdata are. Itcan be very hard to visualize data just in �gures. The following pictures showsinteresting parts of the �ight.
Flight playback at 162s when Brumby is accelerating for takeo�
Here we see the aircraft accelerating on the ground for takeo�. There is a smallright turn and then correction, and my idea here is that this is from the spikesthat are in the IMU data, in z-direction.
−85
−80
−75
−70
−65
−60
−55
−50
−45
−5
0
5
10
15
20
25
Time 162.1 to 166.1 s. o=GPS Observerations, −=Estimated Flight Path
Nice left and right turn with climbing
Here we see the aircraft �rst in a right turn and rolls over to a left turn andclimbs at the same time. This section contains good GPS observations andtherefor the estimated trajectoria is very good.
109
−540 −530 −520 −510 −500 −490 −480 −470
160
180
200
220
240
260
280
220
230
240
Time 354.3 to 358.3 s. o=GPS Observerations, −=Estimated Flight Path
Flight playback at 328s with a bad GPS solution
Here we see how the GPS solution suddenly jumps sideways approximate 15meter. The �lter adjust fast to the new �GPS trajectoria� because the inertialaccelerometers is modelled with a relative high noise. We can lower this noise,and reject observations beyond 2� calculated from S.
110
260
280
300
320
−510−500
−490−480
−470−460
−450−440
−430−420
−410
200
210
Time 328.2 to 332.3 s. o=GPS Observerations, −=Estimated Flight Path
Flight playback at 238s with a bad GPS solution
The same here, suddenly the GPS solution changes very dramatically in height.
−340
−320
−300
−280
−260
−240
−120−110
−100−90
−80−70
−60−50
−40
152154156158
Time 238.1 to 242.1 s. o=GPS Observerations, −=Estimated Flight Path
111
Playback at 356s
At 356s the aircraft are in a sharp left turn and the GPS solution changes inheight.
−600 −580 −560 −540 −520 −500220
240
260
280230
240
Time 356.3 to 360.3 s. o=GPS Observerations, −=Estimated Flight Path
112
Flight playback at 258s with a bad GPS solution
180
200
220
240
260
280
−520
−510
−500
−490
−480
160
170
180
Time 258.2 to 262.2 s. o=GPS Observerations, −=Estimated Flight Path
Flight playback at 258s with higher accuracy on the inertial sensors
180
200
220
240
260
280
−520−510
−500−490
−480
160
170
180
Time 258.1 to 262.1 s. o=GPS Observerations, −=Estimated Flight Path
113
Hungarian notation
If the code is to be rewritten these hungarian notations should be added.
Variabletype Hungarian Notation Example
matrix m DoubleMatrix mF(9,9)vector v DoubleVector vX(9)void void Tmp( int i)
7.7 The conclusions
7.7.1 Bias and nonlinearity states in �lter
To make the navigation �lter even more accurate the navigation �lter shouldbe expanded to hold states to estimate the biases and nonlinearity terms of theinertial sensors. Optimal would be to have the bias and the driftrate for the rategyros. This is 2*3 more states. With this the states would increse from 9/10to 15/16 states, this would leed to an approximate increase of computationsoperations with 163=103 but is nessesary for a high performance INS system.States to estimate the scalefactor errors in the accelerometers can also be added.There are no problem with this at this stage, because the biggest problem is thehugh vibrations in the airframe.
7.7.2 More e�cient algoritms
The navigation �lter is implemented in C++ using MPP as matrix library . Thepassing of information is done by passing matrixes. A lot of computer power arelost just in shu�ing this matrixes into functions as arguments and then returnnew matrixes as return values. Instead this could be more e�cient by sendingpointers to the software functions.
7.7.3 Vibration damping
Heavy oscillations in the airframe is caused by the engine, this vibrations mustbe damped in some way. This can be done in di�erent ways
� Vibration damping of the engine.
� Change the engine to a jetengine.
� Isolate the IMU from vibrations.
Personally I think that small jetengine should be used and that the IMU shouldbe vibrationisolated from the airframe.
7.7.4 Tilt sensors.
There must be a way to know if the airframe is horizontal or not. The test�ights starts with calibrating the IMU against the local gravity vector. It is
114
asumed that the aircraft is placed in horizontal position, but tilt sensors shouldbe installed in the airframe to make this for sure.
7.7.5 Placement of GPS antennas
There are four GPS antennas placed all over the airframe, one near the nose,one at the back and one on each wind. This makes a cross and are used tocalculate the attitude of the aircraft. This GPS attitude system now only givesa few GPS attitude observations. This system must use INS for aiding and thiswill give good observations. The attitude solution calculated using the GPSmust be calibrated, now it looks like there is an angle bias in the angle solution.
7.7.6 The angle classes
The attitude of the vehicle can be monitored by three di�erent angle classes
� Eulerangle representation.
� Direction cosine representation, Cbn matrix.
� Quaternion representation.
7.7.7 The eulerangle model
This model works very �ne, a lot of tuning was done and di�erent process noisewas tested. An interesting thing to do with this model is to expand it to holdthe GPS clock model, and to implement the pseudoranging.
7.7.8 The quaternion model
This model is very pleasant to work with and was used with success. All thepartial derivatives of the nonlinear system model turns out to be linear. Thesystem matrix is free from high order function such sinus and cosine. Becauseof this, the linearized system matrix can be calculated very fast. However thelinearized system matrix have a sizeof 10x10 and it is not sure that the simplestructure of the matrix will lead to less calculation. Matrix multiplication isof order N3 and the �lters contains a lot of matrix calculations. If the statetransition matrix is expanded to hold the clock model we suddenly have a 12x12matrix.
In the external quaternionangle class that was used for keeping track of theangle there seemed to be some problem. Let's explain it, say that we have asmall error in angle yaw. When we are using quaternion angle representation allfour angles will be a�ected, the error is spread out on all quaternion elements.This is not a problem but the error is added to the real quaternion angle andso far is seems to be ok. One property of the quaternionsangle representationis that the norm should be one. If the quaternions are normalized after theerror update, it seems likte the error in yaw will spread to both roll and pitch.Personally I think that the Eulerangle model is better to use.
115
7.7.9 Use of GPS pseudoranges
The GPS gives position solution if atleast four satellites is tracked. If moresatellites are tracked the solution becomes more accurate. The problem occurhowever when less than four satellites are tracked. If we instead use the pseu-dorange to each satelite, the geometric distance, as observations to a navigation�lter we can use information given with just one satellite present. This is theTightly Coupled INS/GPS. navigation loop. And for this loop the system ma-trixes must be expanded with the GPS clock model, see section 5.2.7. Only twomore states must be added, one is the GPS clock frequency o�set and the otherstate is the GPS clock frequency change rate.
�_x
_xc
�=
24 F 09x2
02x9 0 10 0
35� x
xc
�+ : : : (7.1)
7.8 Future Hardware Model
The future hardware in the airframe is connected through a CAN bus. Thebasic computer node will be based on the shark-DSP board developed at thedepartment. At the moment the software that communicates with the GPSreceivers are written under Linux and even if the OS3 in this project will beEmbedded NT this software will probably stay in the Linux enviroment and willbe ported very late in this project. There are four GPS receivers connected toa PC104 through serial ports.
3Operating System
116
Decentralized hardware
GPSREC
GPSREC
GPSREC
GPSREC
CAN-BUS
CAN-INTERFACECAN-INTERFACE CAN-INTERFACE
CAN-INTERFACE
INS
IMU
IMU
COM PORTS
COM PORTS
GPS
CCDCAMERA
SHARKPC104
PC104PC104
INSERRORFILTER
CAN-INTERFACE CAN-INTERFACE
SHARK
IMAGE PROCESSING
LASERSIGNALPROC.
RADARSIGNALPROC.
RADAR LASER
DATA
PROG
PROG
PROGLOG LOOP
CTRL
117
Bibliography
[1] Salah Sukkarei, Eduardo M. Nebot and Hugh F. Durrant Whyte. A HighIntegrity INS/GPS Navigation Loop for Autonomous Land Vehicle Appli-cation, Australian Centre for Field Robotics. Department of Mechanicaland Mechatronic Engineering. University of Sydney
[2] Bernard Etkin and Lloyd Du� Reid, Dynamics of Flight Stability an dControl, Wiley, ISBN 0-471-03418-5
[3] Robert Grover Brown and Patrick Y.C Hwang, Introduction to RandomSignals and applied Kalman Filtering, Wiley, ISBN 0-471-12839-2
[4] R.P.G. Collinson, Introduction to Avionics, Chapman and Hall, ISBN 0-7803-3433-7
[5] Peter S.Maybeck, Stochastic Models, Estimation and Control Volume1,Navateck Books and Software.
[6] Parkinson and Spilker, Global Position System Theory and Application Iand II, American Institute of Aeronautics and Astronautics, ISBN 1-56347-107-8,ISBN 1-56347-106-X
[7] C.K. Chui and G.Chen, Kalman Filtering with Real-Time Application ,Springer-Verlag, ISBN 3-540-18395-7
[8] Harold W.Sorenson, Kalman Filtering Theory And Application, IEEEPRESS, ISBN 0-87942-191-6
[9] Curtis F. Gerald and Patrick O. Wheatley, Applied Numerical Analysis,Addison-Wesley, ISBN 0-201-59290-8
[10] E L Houghton and N B Carruthers, Aerodynamics for Engineering Stu-dents, Edward Arnold, ISBN 0-7131-433-X
[11] Alan V.Oppenheim and Roland W. Schafer, Discrete-Time Signal Process-ing , Prentice Hall, ISBN 0-13-216711-9
[12] Alan V.Oppenheim and Alan S. Signals and System I, Willsky, PrenticeHall, ISBN 0-13-651175-9
[13] Arthur G. O. Mutambara, Decentralized Estimation and Control with Ap-plication to a Modular Robot, Merton College Oxford
[14] Mohinder S. Grewal and Angus P. Andrews, Kalman Filtering Theory andPractice, Prentice and Hall, ISBN 0-13-211335-X
118
[15] Lennart Ljung, System Identi�cation Theory for the User, Prentice andHall, ISBN 0-13-881640-9
[16] Eykho�, System Identi�cation , Wiley, ISBN 0-471-24980-7
[17] Eric Nettleton, GPS Satellite and Receiver Position Estimation
[18] F.A. Evans and James C. Wilcox, Experimental Strapdown RedundantSensor Inertial Navigation System
[19] Min H. Kao and Donald H. Eller, Multicon�guration Kalman Filter Designfor High-Performance GPS Navigation
[20] Report written by Peter Gibbens and other researchers. Final Report, Uni-versity of Sydney. Department of Aeronautical Engineering.
[21] Decentralized Filtering and Redundancy Management for Multisensor Nav-igation
[22] Navstar GPS Navstar, Global Positioning System Standard Posi-tioning Service Signal Speci�cation United States Air Force, 1995http://www.navcen.uscg.mil/gps/geninfo/gpsdocuments/sigspec/default.htm
[23] James L. Farrell, Integrated Aircraft Navigation,ISBN 0-12-249750-3
[24] Arnold and Maunder, Gyrodynamics and its Engineering Applications
[25] Krakiwsky, Geodesy The Concept, ISBN 0-444-87775-4
[26] Cochin, Analysis and design of the gyroscope for inertial guidance
[27] Meriam, Dynamics
[28] Beta
[29] Navigation and Course Estimation
[30] Don J. Torrieri, U.S. Army Statistical Theory of Passice Location Systems
[31] Alberto Isidori, Nonlinear Control Systems,ISBN 3-540-19916-0
[32] Stuart Bennet, Real-Time Computer Control, ISBN 0-13-764176-1
[33] Karl. J Astrom, Computer Controlled System Theory and Design, ISBN0-13-314899-8
[34] S. Grime and H.F. Durrant-Whyte, Data Fusion In Decentralized SensorNetworks, University of Oxford
[35] John. J Leonard and Hugh Durrant-Whyte, Directed Sonar Sensing forMobile Robot Navigation,ISBN 0-7923-9242-6
[36] Oskar Sander Real-Time GPS Attitude Determination for Control of anUnmanned Aircraft
[37] D.H. Titterton and J.L. Weston Strapdown inertial navigation technology,Peter Peregrinus Ltd
119
Chapter 8
Appendix
1
8.1 Data Speci�cations
8.1.1 Desk Computer Platform
The desk computer used for simulation and processing of data.
Variable Value
Type PCProcessor Pentium III 450MhzMemory 160MBHarddisk 4.2 + 8.4 GBGraphic Card Dimond Viper V550Operating System Redhat 6.0 and Windows NT 4.0NT Program Used Matlab, Visual C++, Borland C++ BuilderLinux Program Used Matlab, Maple, Latex, gcc, x�g
8.1.2 Target System Computer Platform
The Target System uses a computer in PC104 format with Linux as the oper-ating system. This System is mainly used for logging data and are connectedto other parts of the system through an CAN-bus. The ground station comuni-cates with this platform using a serial or ethernet cable.
Variable Value
Type PC-104Processor Pentium 200MhzMemory 128MBHarddisk > 512MBOperating System Slackware LinuxExtra 2 A/D Boards and one extra serial board
8.1.3 Motion Pack IMU
The rigid mouted IMU in the airframe is a IMU from Systron-Donner. TheIMU have three accelerometers and three gyros mounted ortogonal.
Systron-Donner Motion Pack Data Rate Sensor Acceleration Sensor
Ranges +� 200; 500o=sec 3,5,10gBias Variation over Temperature 3o=sec from 22o 100 g=oCLong Term Stability (1 year) < 2:0o=s < 1mgBandwidth > 60Hz >300HzOutput Noise (0 to 100Hz) 0:01o=src 7mV
2
Startup Time < 1.0 secOperating Temperature �40oC to 80oCShock 200 gInput Voltage +-15VDC +-10%
8.1.4 Data speci�cation of the Watson IMU
This IMU have two tilt sensors that gives elevation angle and bank angle. Thesesensors are used in the leveling process when the IMU is calibrated against thelocal gravity vector.
Watson IMU data Value
Rate Range +� 50o=secRate Bias 2% F.SRate Resolution < 0:05o=sAcceleration Range +- 2g'sAcceleration Bias <0.5% F.SAcceleration Accuracy <0.5% F.SAcceleration Resolution Better than 2mgSensor Alignment < 0:25o- all sensorsFrequency Responce DC-20 Hz, >70 Hz for ratesPower Requirement < 600mA, 10-16V DCOutput Format Digital, RS-232, Analog
8.1.5 Tetrad IMU
8.1.6 GPS Receiver
The GPS Recivers used in the airframe is from the Canadian Marconi Company,they communicates with the onboard computer through the serial ports.
Receiver Data Value
Manufacturer Canadian Marconi Company (CMC)Model ALLSTAR OEM (CMT-1200) REV.GNumber of Channels Maximum 12 satellites tracked simutaneouslyMax Velocity 1800 km/hMax Acceleration 4gPower 5.0V, 1.7WDimensions 67x102x19 mmWeight 93gSerial Communications 2xRS232 asynchronous ports. 300 to 38,400 BAUDSerial Protocol NMEA 183v2.0 and CMC binary
3
8.2 Numerical Integrators
Block Itegrator
I =
NXk=0
ak�T (8.1)
Trapetz integrator
I =
NXk=0
ak�1 + ak2
�T (8.2)
Simpson integrator
I =
NXk=0
ak�2 + 3ak�1 + ak5
�T (8.3)
Runge-Kutta integrator
I =NXk=0
ak�3 + 2ak�2 + 2ak�1 + ak
6�T (8.4)
8.3 Gaussian Distrubutions
Stocastic variables in one dimension is modelled using a standard gauss dis-tribution. When you add more dimensions it become more complicated withrepresentations, in two dimensions the probability density function looks like ahill. In tree dimensions the probability density function can be visualized as adensity cloud
4
One Dimensional Probability Density Function
−4 −3 −2 −1 0 1 2 3 40
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4Gaussian Distribution 1dim
Pro
babi
lity
dens
ity (
r)
Radius r (m)
Two Dimensional ProbabilityDensity Function
−4
−2
0
2
4
−4
−2
0
2
40
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Radius r (m)
Gaussian Distribution 2dim
Probability density (r)
5
Three Dimensional Probability Density Function
−4
−2
0
2
4
−4
−2
0
2
4−4
−3
−2
−1
0
1
2
3
4
y (m)
Gaussian Distribution 3dim
x (m)
8.4 Con�dence Ellipses
We have a multi dimensional stocastic variable x that is gaussian distributed.In this example we bound the dimension of x to R3. The probability functionof x is
P (x) =exp[� 1
2 (x�E[x])TC�1 (x�E[x])]p
(2�)mjC(x)j (8.5)
We search the eigenvalues �i and eigenvectors zi of C�, i = 1::3. This can thenbe put into this quadratic formula.
�z1 z2 z3
�24 �21 0 00 �22 00 0 �23
35�1 �
z1 z2 z3�T
= C2� (8.6)
And can be rearranged into
z21C2��
21
+z22
C2��
22
+z23
C2��
23
= 1 (8.7)
Which makes a clear picture of what we got here. It is no doubt that this can berepresented as a ellipsiod in R3. Volyme of the ellipsoid is equal to probability1�� from C�, normally C0:05 is used. This way is just a way to view the uncer-tanity of a stocatic variable. It says that a stocastic variable with a probabilityof 0.95 lies within or on the boundary of the ellipsiod. The eigenvalues, zi, of Cspans the ellipsiod in R3. The form of the ellipsiod is controlled by �1; �2; �3.If we have �1 = �2 = �3 = � then we will have a sphere.
6
3 Dimensional Con�dence Oval
P=(x,y,z)
z3
z2
z12
σC
C σ
C σ1
3
8.4.1 Two Dimensional Con�dence Ellipse
In two dimensions the con�dence section will be an ellipse in on a two dimen-sional sheet. In this case we say that a stocastic variable lies within the areabounded by the ellipse with a probability of 1� �.
2 Dimensional Con�dence Ellipse
z1
z2
σC σσ1
2C σ
7
8.5 Rayleigh Distribution
We look at the probability density function in respect to the radius from zero,this is called the Rayleigh distribution. From this we get that the highest prob-ability to �nd the stocastic variable is not in the center, but instead at thedistance � from the origin.
Rayleigh probability and density functions
0 0.5 1 1.5 2 2.5 3 3.5 40
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Pro
babi
lity
dens
ity (
r)
Radius r (m)
Rayleigh distribution
0 0.5 1 1.5 2 2.5 3 3.5 40
0.2
0.4
0.6
0.8
1
Pro
babi
lity
(r)
Radius r (m)
8.6 Wienerprocess-Randomwalk
AWienerprocess w(t) is integrated whitenoise and can be used to modell a lot ofdi�erent physical phenomeias. It was developed to modell Brownsk motion andcan be used to modell the drift in gyros, accelerometers and crystal oscillators.
8
1
S
White noise w(t)
� 2 N(0; 1), E[w(t)] = 0
and E[w(t)2] = t
_x(t) = b�(t) = b _w(t) (8.8)
x(t) = b
Z�(t)dt (8.9)
x(ti+1) = x(ti) + b
Z ti+1
ti
�(s)ds (8.10)
With the assumption that �(t) is constant in between ti and ti+1 the continoustime modell will become.
x(ti+1) = x(ti) + bpti+1 � ti�(ti) (8.11)
9
