Contents. Preface. 1 Introduction State of the art Outline... 2

Transcription

1 Preface This thesis is written to achieve the Masters of Science degree in Engineering Cybernetics from the Norwegian University of Science and Technology. It is an continuation of the work made in the preproject last autumn. The thesis was made with cooperation with the Norwegian Defense Research Establishment. I would like to thank my supervisors Leif Hanssen at the Norwegian Defense Research Establishment and Oddvar Hallingstad at the University Graduate Center at Kjeller for their guidance. I would also like to thank all the others at the Norwegian Defense Research Establishment who have assisted me in any way. The original intention was that the software development for the unit responsible for collecting data from the sensor should be completed during the preproject. Unfortunately further software development was needed. The calibration of the sensors was a more time consuming process than rst anticipated. As a result, there was less time to do all the simulations that was planned. I prioritized to do measurements and look at the results of these measurements. Kim Mathiassen Kjeller, 4 June 2010 I

2 II

3 Summary This thesis designs a multipurpose navigation unit. The unit is equipped with a GPS which is the primary navigation sensor, when the GPS satellites are available. If the GPS satellites becomes unavailable, a inertial system with aid sensors is used for navigation. A unit that collects data from a GPS, gyroscopes, accelerometers, magnetometers and a barometer has been developed in a project leading to this thesis. The software development was not completed. The software responsible for communicating between the ADIS16405 sensor, which contains three gyroscopes, three accelerometers and three magnetometers, and the microcontroller was completed in this thesis. The software responsible for communicating between the barometer and the microcontoller was optimized in order to prevent timing errors. The menu system has been extended and sensor data is now saved in dierent les. Navigation equations has been found and is augmented with noise states. The augmented equation is used as the process equation in the Unscented Kalman lter. The augmented noise states is an approximation of icker noise from the gyros. It was found that a rst order Gauss-Markov process was the noise process which gave a change of covariance over time that was most similar the icker noise of the tested noise processes. A lter for the gyroscope output is designed. This lter is intended to remove icker noise from the gyroscope measurements. Measurements equations for the magnetometer and barometer is also found. All sensors have been calibrated and the error model coecients has been estimated. The gyroscopes and accelerometers have less noise than specied in their datasheet. Because the lack of a calibration equipment the magnetometer has not been calibrated accurately. The inertial system has been simulated by Monte Carlo simulations to quantify the error sources. The gyroscope noise is the main error source, and noise from the sensors that provide data to the initial attitude calculation is the second largest error source. The standard deviation of the position error is approximately 65 m after 60 seconds when the system is stationary. This is found by using Monte Carlo simulations. It would be interesting to nd how much this error could be reduced by using a better gyroscope. We assumed that the better gyroscope had on tenth of the noise compared to the estimated nois values from the real gyroscope. This reduced the standard deviation of the error to approximately 13 m after 60 seconds. Simulations show that the standard deviation of the position error could be reduced from 65 m after 60 seconds to approximately 20 m after 60 seconds by using magnetometers and barometer in addition to the inertial sensors. The real world tests have a very large position deviation. It is likely that this is caused by unmodeled eects, like magnetic disturbance. The Unscented Kalman lter also contributes to the error by not propagating the mean of the process equation correctly. III

4 IV

5 Contents Preface Summary I III 1 Introduction State of the art Outline Background Mathematical background Vectors The rotation matrix Angular velocity Navigation Strapdown systems Reference frames Initializing the AHRS Continuous navigation equations Discrete navigation equations Noise Allan Variance White noise Flicker noise Brownian motion Gauss-Markov process Evaluating possible approximations of icker noise Kalman lters Linear lter Nonlinear systems Converting sensor data to navigation information Pressure to altitude Earth's magnetic eld Sensor errors and calibration GPS model Gyroscope model Accelerometer model V

6 2.6.4 Magnetometer model Pressure sensor model Quality of model t Data acquisition DAQ unit Software updates to DAQ unit User documentation Menu File format Possibility of implementing navigation algorithm on DAQ unit Positioning algorithm GPS Inertial sensors Selecting noise model for gyroscope Kalman lter equations for inertial sensors Filtering gyro output GPS and inertial sensors Aid sensors Magnetometer as aid sensor Altimeter as aid sensor Magnetometer and Altimeter as aid sensors Using all sensors Implementation details Summary Results Calibration Gyroscope Accelerometer Magnetometer Barometer Monte Carlo simulations Quantifying the error sources Verifying the UKF Simulation of lter covariance Real world tests Stationary test on rate board Stationary tests outdoors Pedestrian and vehicle tests Conclusion 97 7 Further work 98 A Abstract submitted to IPIN 103 VI

7 B Microcontroller software changelog 106 B.1 Changelog C Sensor calibration software 180 C.1 Calibration program in C C.1.1 sensor_param.c C.1.2 stat.c C.1.3 read_meas.h C.1.4 read_meas.c C.1.5 save_meas.h C.1.6 save_meas.c C.1.7 uc_daq.h C.1.8 build.sh C.1.9 Meschach library C.2 Calibration program in Matlab C.2.1 magn.m D Matlab code for position estimation 210 D.1 Functions D.1.1 Math functions D.1.2 Position and frame functions D.1.3 Process and measurement functions D.1.4 Sensor parameter loading D.1.5 QUEST implemanteation D.1.6 Kalman lter implementations D.1.7 Monte Carlo simulations D.2 Estimation of position from IMU, magnetometer and barometer D.2.1 Estimation algorithm D.2.2 Batch script to calculate position from all measurements D.3 Estimation of velocity from GPS D.3.1 Estimation algorithm E Real world tests 255 E.1 Stationary tests E.1.1 Outdoor test E.1.2 Outdoor test E.2 Pedestrian test E.2.1 Test E.2.2 Test E.3 Vehicle test E.3.1 Test E.3.2 Test VII

8 List of Tables 2.1 Parameters for WGS-84 [5]. The four exact parameters are marked with Summary of frames used throughout the text Process equations Measurement update equations Noise covariance Noise parameters for ADIS16405 gyroscopes Calibration parameters for ADIS16405 gyroscopes. Note that M gy = [m gy,x m gy,y m gy,z ] T Calibration parameters for ADIS16405 accelerometers. Note that M ac = [m ac,x m ac,y m ac,z ] T Calibration parameters ADIS16405 magnetometer Calibration parameters MC5534C barometer VIII

9 List of Figures 2.1 Overview over strapdown inertial navigation (based on Fig 9.1 in [59]) Log-log graf over σ AV AR ved Angle random walk [31] Log-log graf over σ AV AR ved Bias instability [31] Log-log graf over σ AV AR with all noise sources [31] Log-log graph over the power spectral density of dierent noise processes. For rst order Gauss-Markov process β = 2. For all processes Q = Simulation of (2.144) with a = 1, σ = 1, Q = 1 and x(0) = 0, 1. The black graph is the estimated power spectral density of x, the red graph is a ltered version of the black graph using the lter y k1 = 0.75y k 0.25x k. The blue graphs are 1/f and 1/f 2 lines Simulation of (2.144). Zoomed version of gure Monte Carlo simulation of power spectral density of (2.145). The Power spectral density has been simulated 100 times and the solid graph shows the average result. Parameters used are x min = 0.5, x max, n = 1 and T = Log-log graph over the power spectral density of a rst order Gauss-Markov process. Solid line is a continuous process and dashed line is the discretization of the continuous process. Parameters used is β = 0, 5, T = 0, 1, Q = 1 and f c = Log-log graph over the power spectral density over the lters used to approximate icker noise. The graph shows the corresponding discrete versions of the noise processes. Parameters used is Qp = , Q w = 1, , S max = 0.1 and T = Log-log graf over the power spectral density response of the AR lter creating icker noise from section with a nit set of coecients The elements of the magnetic eld (based on [9, 45]) The crosses represents satellites. To the left is an example of poor satellite position. The case to the right has a lesser region of uncertainty, and will give a better position estimate Relationship between parameters of distribution The DAQ unit IX

10 4.1 The standard deviation of δθ using Q w = (2/60) 2 and Q p = (0, 007) 2, found in the datasheet for ADIS16405 [11]. The solid line is the result obtained by Monte Carlo simulations, the dashed line is a Brownian motion approximation, the dash dotted line is a Gauss-Markov approximation and the dotted line is a 30th order AR lter approximation Filter response for the lter used for lter gyroscope measurements Allan variance for ADIS16405 gyroscopes with dynamic range set to 75 /s Allan variance for ADIS16405 x-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets Allan variance for ADIS16405 y-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets Allan variance for ADIS16405 z-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets Allan variance for ADIS16405 gyroscopes with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from the estimated Allan variance parameters The three datasets for the magnetometer and the same data sets with the estimated parameters Calibration data for the barometer The standard deviation of the position error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the noise found in the sensor calibration Standard deviation of the velocity error by dierent noise sources. The model (2.104) has been simulated 100 times using the noise found in the sensor calibration The standard deviation of the position error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the one tenth of the noise found in the sensor calibration The standard deviation of the velocity error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the one tenth of the noise found in the sensor calibration The standard deviation of the dierence between the velocity from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix The standard deviation of the dierence between the position from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix X

11 5.14 The standard deviation of the dierence between the position from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix The standard deviation of the position error for a selection of dierent sensor congurations The standard deviation of the position error for a selection of dierent sensor congurations The standard deviation of the velocity error for a selection of dierent sensor congurations The standard deviation of the velocity error for a selection of dierent sensor congurations Calculated position for the rst 15 seconds using the data set STA-IN Calculated position for the rst 15 seconds using the data set STA-IN Calculated altitude using the data set STA-IN Calculated altitude using the data set STA-IN Pressure measured from the barometer in data set STA-IN Calculated deviation from initial position using the data set STA-IN Calculated deviation from initial position using the data set STA-IN Calculated length of velocity vector using the data set STA-IN Calculated length of velocity vector using the data set STA-IN Standard deviation of the position calculated by the Kalman lter using the data set STA-IN Standard deviation of the position calculated by the Kalman lter using the data set STA-IN Standard deviation of the velocity calculated by the Kalman lter using the data set STA-IN Standard deviation of the velocity calculated by the Kalman lter using the data set STA-IN Standard deviation of the quaternion calculated by the Kalman lter using the data set STA-IN Standard deviation of the quaternion calculated by the Kalman lter using the data set STA-IN XI

12 List of Algorithms 2.1 Iterative approach to convert from ellipsoidal to Cartesian coordinates (based on algorithm in [15]) Closed-loop approach to convert from ellipsoidal to Cartesian coordinates [15] QUEST (created from [56]) QUEST with special case n = 2(created from [56]) Discrete Extended Kalman lter (based on [57]) Unscented Kalman lter (based on [35, 61]) Square root of matrix XII

13 List of acronyms AHRS Attitude and Heading Reference System DOP Dilution of precision ECEF Earth-Centered-Earth-Fixed EKF Extended Kalman Filter ENU East North Up coordinates FFI Norwegian Defense Research Establishment GDOP Geometrical Dilution of precision GNSS Global Navigation Satellite System GMT Greenwich Mean Time GST Greenwich Sidereal Time GPS Global Positioning System HDOP Horizontal Dilution of precision IGRF International Geomagnetic Reference Field INS Inertial Navigation System IMU Inertial Measurement Unit MEMS Micro-Electro-Mechanical Systems NED North East Down coordinates NTNU Norwegian University of Science and Technology PDOP Positional Dilution of precision TDOP Time Dilution of precision UERE User Equivalent Range Error VDOP Vertical Dilution of precision XIII

14 UKF Unscented Kalman Filter WGS-84 World Geodetic System 1984 WWM World Magnetic Model XIV

15 Nomenclature Mathematical background a a a a a b a A R a b q a b ω ω a ab Geometric vector Euclidean norm of a geometric vector Algebraic vector Euclidean norm of a algebraic vector Algebraic vector represented in frame b Skew symmetric form of a Matrix Rotation matrix from frame b to frame a Quaternion representing the rotation from frame b to frame a Angular velocity vector Angular velocity vector of velocity between frame a and b expressed in frame a Navigation p v f g γ (φ, λ, h) Position vector Velocity vector Specic force vector Gravitation vector Gravitation model vector Latitude, longitude and height above mean sea level Noise σ 2 AV AR N B S(ω) Q Q T Allan variance Angle random walk coecient Bias instability coecient Power spectral density Power spectral density coecient Covariance Sampling time XV

16 Kalman lters x v z w F G Φ Γ H ˆP 0 Q R Q R I f k (x k, v k ) h k (x k ) State vector Process noise vector Measurement vector Measurement noise vector Process matrix, continuous system Process noise matrix Process matrix, discrete system Process noise matrix, discrete system Measurement matrix Initial covariance Power spectral density of process noise Power spectral density of measurement noise Covariance of process noise Covariance of measurement noise Information matrix Nonlinear process equation Nonlinear measurement equation Converting sensor data to navigation information p B H B a Pressure The magnetic eld vector The horizontal magnetic eld vector The magnetic eld vector in frame a Sensor errors and calibration ω Measured angle velocity ã Measured acceleration B Measured magnetic eld p Measured pressure S xx Scale factor error of sensor xx M xx Cross-coupling error of sensor xx b xx,f Bias error of sensor xx n f Noise process of sensor xx σ xx,x Noise process of sensor xx for x axis SSE Error sum of squares SST Total corrected sum of squares R 2 Coecient of determination Sensor xx can be gy, ac, ma and ba for gyroscopes, accelerometer, magnetometer and barometer. XVI

17 Other notations Table 2.1 on page 10 shows the WGS-84 parameters, Table 2.2 on page 12 shows a summary of frames used in this thesis. Tables 4.1 on page 69, 4.2 on page 70 and 4.3 on page 70 shows the process equations, measurement equations and noise covariance used in the Unscented Kalman lter. XVII

18 XVIII

19 Chapter 1 Introduction Low cost GPS devices has made it more and more common to equip personnel, vehicles and other equipment with GPS to track their position. The GPS gives accurate position information when satellites are visible, but fail to give position information when it looses satellite signals. This might happen inside buildings, in urban areas and in highly vegetated areas. Micro-Electro-Mechanical Systems (MEMS) sensors is getting more and more popular within consumer electronics [21], which leads to lower prices on MEMS sensors. By adding low cost inertial sensors to the GPS unit we can be able to track the position even when the satellites are not visible. 1.1 State of the art Many research projects has been conducted over the last years to nd solutions to the problem mentioned above. [55] has created a personal navigation system using accelerometers, gyroscopes, magnetometers, barometer and Doppler radar. The Doppler radar measures the velocity of the person relative to the environment and is referred to as a velocimeter. The system is intended to track ground soldiers in the U.S. Army during operations. They showed that the velocimeter had signicant impact on the position accuracy. [43, 44] uses a inertial measurement unit (IMU), a magnetometer and a GPS to create a pedestrian navigation system. The article states that the main error source in positioning is the determination of the azimuth and uses knowledge of walking characteristics to estimate the azimuth using a Kalman lter. Other approaches than inertial navigation have been suggested. In order to limit the drift in the above mentioned systems, systems that estimate the step length and direction is developed. [20] uses a IMU and magnetometer placed on the foot of a person, and tries two approaches for estimating the position. The rst is a inertial system with Zero Velocity Updates, which means that every time the foot strikes the ground is detected and the velocity is updated. This can signicantly bound the drift of the system. The second approach is to estimate the number of steps, stride length and heading to calculate the position. [4] has created a system for rst responders that use similar sensors, but a dierent method for step and stride estimation. A system for positioning of dismounted soldiers is presented in [42]. This system uses a IMU, magnetometer and barometer and is able to distinguish between steps going forwards, backwards and sideways. It is also possible to improve the accuracy of the system by using a map of a building, if it is indoor 1

20 navigation, and use the information in the map to improve navigation. [19] describes a indoor navigation system that uses map information. Both the GPS and INS has drawbacks, GPS by relying on satellite signals and INS drifting out of position. Pseudolites are ground based GPS-like transmitters with can send GPS signals where the satellites cannot. [47] shows that the GPS accuracy was increased by using Pseudolites. [6] presents several methods that two or more vehicles equipped with GPS receivers can collaborate by sharing navigation information. Two vehicles might not have the same satellites, but have line of sight, and can thereby share satellite information. In this thesis we will develop a multipurpose navigation unit that can be used by military personnel, vehicles and other equipment in areas with dicult satellite signal conditions such as inside buildings. A similar project was undertaken some years back [28, 29] at Norwegian Defense Research Establishment (FFI). This project experienced large gyroscope drift and did only use an IMU and GPS for positioning. This thesis will address these problems by using newer sensor technology and including more sensors to aid the position estimation. The sensors where chosen in the preproject [52], leading to this thesis. The unit that collects measurements from the sensors was also developed in the preproject, but the software was unfortunately not nished. This thesis will nish the software development, nd algorithms to estimate position from the sensors. The thesis will also test the performance of the navigation unit with the use of dierent sensor congurations, both in simulation and in the real world. The questions we would like to answer in this thesis is how dierent sensor conguration aect position accuracy and how long it is possible for the navigation system to have a good estimate of the position? 1.2 Outline Chapter 2 will present the necessary theory and background information needed in this thesis. Chapter 3 will describe the test unit and software updates made. In Chapter 4 the position algorithms are presented and some implementation details are given. Chapter 5 shows the results and Chapter 6 contains the conclusion. At last Chapter 7 gives an account of further work needed to improve the unit. The main results of this thesis is submitted in an abstract to the Conference of Indoor Positioning and Indoor Navigation and is under review when this thesis was printed. The abstract is given in Appendix A. 2

21 Chapter 2 Background 2.1 Mathematical background This section will give a brief review of the main topics needed to understand this thesis Vectors A vector u is described by its magnitude and direction [13, 26]. This description does not necessary relay on any coordinate frame. If a Cartesian coordinate frame a is introduced, with orthonormal unit vectors a i, then the vector u can be represented as a linear combination of the unit vectors a i in the space R n by n u = u i a i (2.1) where i=1 u i = u a i (2.2) This vector is called geometric vector. The same vector could be represented as a column vector or algebraic vector showed below u = u 1. u n (2.3) The length of the vector used in this text is the Euclidean norm dened by u = u = u u 2 i (2.4) The scalar product between the two geometric vectors u and v is dened by u v = u v cos θ (2.5) where θ is the angle between the two geometric vectors. If the vectors have a reference to a frame, the scalar product can be calculated by the relationship u v = u T v (2.6) 3

22 The vector cross product between the two geometric vectors u and v is dened by u v = n u v sin θ (2.7) where n is a unit vector orthogonal to both u and v. The vectors u, v and n forms a right hand system [13]. With reference to a Cartesian frame a in R 3 space the vector cross product can be evaluated from w = u v = a 1 a 2 a 3 u 1 u 2 u 3 v 1 v 2 v 3 By introducing the skew symmetric form of the vector u, which is dened by u = 0 u 3 u 2 u 3 0 u 1 u 2 u 1 0 the cross product of the two algebraic vectors u and v is (2.8) (2.9) w = u v (2.10) The rotation matrix It can be benecial to have more than one coordinate frame [13, 26]. We now introduce two frames in R 3 space, frame a and b. These frames has orthonormal unit vectors, frame a has a 1, a 2, a 3 as unit vectors and b has b 1, b 2, b 3. A vector v can be represented in both frames. In this text we use the notation v = 3 vi a a i = i=1 3 vi b b i (2.11) i=1 where vi a = v a and vi b = v b. To distinguish between which frame the coordinates in the algebraic vector is referring to we use superscript. The vector v is given in algebraic vectors with coordinates in the two frames a and b v a = v a 1 v a 2 v a 3 and v b = v b 1 v b 2 v b 3 (2.12) To transform an algebraic vector from b to a we introduce the rotation matrix R a b. The transformation is given by v a = R a b v b (2.13) 4

23 Simple rotations A rotation between a xed axis is called a simple rotation [13]. In the rotation matrices given below x, y and z represents the rst, second and third axis of the coordinate system. If we have the frames a and b then R x (φ) will rotate a vector around a 1 by an angle φ. The two other matrices do the same, but for a dierent axis. R x (φ) = R y (θ) = R z (ψ) = cos φ sin φ 0 sin φ cos φ cos θ 0 sin θ sin θ 0 cos θ cos ψ sin ψ 0 sin ψ cos ψ (2.14) (2.15) (2.16) Euler angles The Euler angles of roll-pitch-yaw are commonly used to describe rotation between earth xed frames and body frames [13]. The rotation is based on the simple rotations and is given by R a b = R z (ψ)r y (θ)r x (φ) (2.17) First there is a rotation of ψ about the z a axis. Then there is a rotation of θ about the rotated y axis and at last there is a rotation of φ about the rotated x axis. There are also other ways to combine the simple rotations to create other Euler angles. The classical Euler angles are dened by R a b = R z (ψ)r y (θ)r z (φ) (2.18) One disadvantage with the Euler angles is that it has only three parameters, and therefore may produce singularities. On the other hand Euler angles produces a clear physical interpretation of the rotation matrix. Angle axis Another interpretation of the rotation matrix is the angle-axis parameterization [13]. The rotation between the frames a and b is about the vector k by an angle θ. The coordinates of k is the same in both frames a and b, meaning that k a = k b = k. To rotation matrix as a function of k and θ is given by where I is the identity matrix. R a b (k, θ) = cos θi sin θk (1 cos θ)kk T (2.19) 5

24 Quaternions The above methods has the disadvantage that they all depend on trigonometric functions. These functions have singularities. By using Euler parameters and quaternion algebra it is possible to create a rotation matrix without using trigonometric functions. It is also possible to rotate a vector by using the quaternion product. Euler parameters is dened by A quaternion is a vector dened by where q i R and i, j and k satisfy the equations [26] η = cos θ 2 ɛ = k sin θ 2 (2.20) q = q 0 q 1 i q 1 j q 1 k (2.21) ii = jj = kk = ijk = 1 (2.22) It should be noted that quaternions sometimes are dened with the real part last in the quaternion vector q, instead of rst as dened here [50]. We now create a unit quaternion vector [ ] η q = (2.23) ɛ where η and ɛ are Euler parameters and where q T q = 1. The rotation matrix is then given as a function of Euler parameters by R a b (η, ɛ) = I 2ηɛ 2ɛ ɛ (2.24) The quaternion product between the quaternions q 1 = [α 1 β T 1 ] T and q 2 = [α 2 β T 2 ] T is given by [ q 1 q 2 = α 1 α 2 β 1 T β 2 α 1 β 2 α 2 β 1 β 1 β 2 ] (2.25) The inverse unit quaternion of q is dened by [ ] η q = ɛ It can be shown that the equation below is true [13] [ 0 Rv ] = [ η ɛ ] [ 0 v ] [ η ɛ ] (2.26) (2.27) By using the above equation it is possible to perform rotations by using the quaternion product instead of using the rotation matrix. If we have three frames, a, b and c, and we know the quaternions q a b (rotation from frame b to frame a) and qb c (rotation from 6

25 frame c to frame b). It is possible to calculate the quaternion q a c (rotation from frame c to frame a) by using the following equation Normally we would calculate the rotation as q a c = q a b q b c (2.28) v a = R a b R b cv c (2.29) By using the quaternion product this problem can also be formulated as [ ] [ ] 0 0 v a = q a c v c q a c (2.30) Instead of multiplying the two matrices R a b and R b c we perform the quaternion product between q a b and qb c and thereby reduces the computational eort Angular velocity The rotation matrix may change over time. By using the fact that the rotation matrix satisfy the equation and time dierentiate this product we get R a b (R a b ) T = I (2.31) d ( R a b (R a b ) ) T = R dt a b (R a b ) T R a b ( R a b ) T = 0 (2.32) The matrix R a b (R a b ) T is a skew symmetric matrix that can be written as (ω a ab ). Let the vector ω ab be the geometric vector of ω a ab. ω ab is the angular velocity vector of frame b relative to frame a. The dierential equation of the rotation matrix can be written as R a b = (ω a ab) R a b = R a b (ω b ab) (2.33) Consider the composite rotation R a d = R a b R b cr c d. It can be shown that the angular velocity for the composite rotation is the sum of the angular velocities according to [13] Dierentiation of coordinate vectors ω ad = ω ab ω bc ω cd (2.34) The relationship between the time derivate of a vector u in the frames a and b can be found by dierentiation the equation This gives the relation [13] u a = R a b u a = R a b u b (2.35) ( ub (ω b ab) u b) (2.36) 7

26 Figure 2.1: Overview over strapdown inertial navigation (based on Fig 9.1 in [59]) Quaternions When using quaternions for rotations it is useful to also have the time derivate of the quaternions. It can be shown that the time derivate of the quaternion can be expressed as [13] q = 1 [ ] 0 q (2.37) 2 ω By calculating the quaternion product we get q = Ω(ω)q (2.38) where Ω(ω) = 1 [ ] 0 ω T 2 ω ω (2.39) q = [ ] T q 0 q 1 q 2 q 3 (2.40) ω = [ ] T ω x ω y ω z (2.41) We also introduce Ω to be Ω(ω) = 1 2 [ 0 ω T ω ω ] (2.42) 2.2 Navigation In this section we will present all theory regarding navigation that is needed in this thesis. This includes reference frames, navigation equations and how to initialize the navigation algorithms Strapdown systems A strapdown navigation system is a navigation system where the sensors are attached rigidly to the body of the system, opposed to gimbaled systems where the sensors is mounted on a stable platform [62]. A strapdown system can be divided into submodules [59]. Figure 2.1 shows this schematically. The inertial measurement unit typically 8

27 consists of accelerometers and gyroscopes. The Attitude and heading reference system computes the attitude of the system by using the gyroscope measurements. Because the gyroscopes measure angular rate and not attitude directly the attitude computer must be initialized. This problem and possible solutions are discussed in a later section. The inertial navigation system consist of the Attitude and heading reference system (AHRS) and the navigation computer that calculates the position based on the attitude and the accelerometer measurements. The navigation computer must also be initialized with both position and velocity. This can be done by measurements from a Global Navigation Satellite System (GNSS), manual entry or from other types of sensors. The velocity can also be initialized by requiring that the inertial navigation system is stationary when it starts up. [22] Reference frames In order to know the position of an object it is essential to have a reference. The earth is a natural choice of reference, but other reference systems are useful. We shall now look upon four reference frames that shall be used throughout this text. The inertial frame is a reference system where the laws of Newtonian mechanics apply. This implies that the reference system is at rest or has a constant velocity along a straight line. To indicate that a vector is in the inertial frame the vector is denoted with a superscript i. It can be shown that [27] p i = f i g i (2.43) where f i is the specic-force vector, p i is the acceleration and g i is the gravitational vector, all represented in the inertial reference frame. The inertial frame is centered to earth and share its polar axis, but does not rotate with it. The frame does not rotate with the earth because then it would not fulll the laws of Newtonian mechanics [5]. The inertial frame consists of the three basic unit vectors x i, y i and z i. z i points towards mean direction of the rotating axis of the earth, x i points towards the vernal equinox and y i completes the system to be a three-dimensional right-hand Cartesian system. This denition is the same as in [27]. The next frame is the earth-centered-earth-xed frame, or ECEF frame. Throughout this text it will be referred to as the earth frame and is denoted with a superx e. The basic vectors of this system is x e, y e and z e. Because the GPS module outputs position in the World Geodetic System 1984 (WGS-84) we will use this realization of the earth frame. The WGS-84 standard models the earth as a geocentric ellipsoid of revolution [5, 27]. The standard denes four exact parameters in which all the other parameters are derived. The parameters are given in table 2.1. The z e axis points to the mean direction of the rotating axis of the earth, which means that z i = z e. The x e axis points towards the intersection of equator with the Greenwich meridian. y i completes the system to be a three-dimensional right-hand Cartesian system [27]. The GPS does not provide output in this Cartesian coordinate system, but in ellipsoidal of latitude φ, longitude λ and height h above the mean sea level. The conversion from ellipsoidal 9

28 Table 2.1: Parameters for WGS-84 [5]. The four exact parameters are marked with Parameter Value a, semi-major axis m ω ie, angular velocity rad/s GM, Geocentric gravitational constant m 3 /s 2 1/f, reciprocal attening 298, b, semi-minor axis , 3142 m γ e, normal gravity at equator 9, m/s 2 γ p, normal gravity at pole 9, m/s 2 m = ωiea 2 2 b/gm, dimensionless const. 0, e 2 = 1 (b 2 /a 2 ), rst eccentricity 0, coordinates to Cartesian coordinates is given by [27] p e (N(φ) h) cos φ cos λ p e x (φ, λ, h) = p e y = (N(φ) h) cos φ sin λ ( ) (2.44) p e b 2 z N(φ) h sin φ a 2 where N is the radius of curvature in the prime vertical given by N(φ) = a 2 a2 cos 2 φ b 2 sin 2 φ (2.45) and a and b is given in table 2.1. To convert from Cartesian coordinates to ellipsoidal coordinates is more dicult. The longitude can be computed explicitly by the following formula [15] λ = arctan2(p e y, p e x) (2.46) where arctan2 is the the four quadrant inverse tangent function. The function atan2 in Matlab implements this function. The latitude φ and height above mean sea level h is more dicult. One can use an iterative approach or an approximate closed-loop solution. The iterative approach is given in algorithm 2.1. This algorithm nds h with an accuracy of ɛ h and φ with an accuracy of ɛ φ. The closed-loop approximation given in algorithm 2.2 is one commonly used approach which is accurate up to low-earth orbit. In this algorithm N(φ) is calculated as in (2.45). In both algorithms a and b are constants given in table 2.1. The local-level frame is a tangent plane on the WGS-84 ellipsoid. There are several ways to dene a right-hand Cartesian system on the plane. Two commonly used approaches are the NED frame and the ENU frame. The origin is an arbitrary point in the earth frame. The local-level frame has the basic vectors x l, y l and z l. In the NED frame x l points north, y l points east and z l is the normal of the plane and is perpendicular to the ellipsoid pointing to the center of the ellipsoid. In the ENU frame x l points east, y l points north and z l is the normal of the plane and is perpendicular to the ellipsoid 10

29 Algorithm 2.1 Iterative approach to convert from ellipsoidal to Cartesian coordinates (based on algorithm in [15]) Input: p e, ɛ h, ɛ φ Output: φ, λ, h // Initialize h 0 = 0 N 0 = a e 2 = 1 b2 a 2 p e xy = (p e x) 2 (p e y) 2 k = 1 // Repeat until the error is below ɛ h for h and ɛ φ for φ repeat (sin φ) k1 = p e z N k (1 e 2 )h k ( ) p e φ k1 = tan 1 z e 2 N k (sin φ) k1 N k1 = p e xy a 1 e2 ((sin φ) k1 ) 2 p e xy h k1 = cos(φ k1 ) N k1 k = k 1 until h k 1 h k < ɛ h and φ k 1 φ k < ɛ φ λ = arctan2(p e y, p e x) Algorithm 2.2 Closed-loop approach to convert from ellipsoidal to Cartesian coordinates [15] Input: p e Output: φ, λ, h p e xy = (p e x) 2 (p e y) 2 e 2 = 1 b2 a 2 (e ) 2 = a2 b 2 b 2 ( ) a p e θ = tan 1 z b p ( e xy ) p e φ = tan 1 z (e ) 2 b sin 3 θ p e xy e 2 a cos 3 θ λ = arctan2(p e y, p e x) h = pe xy cos(φ) N(φ) 11

30 Table 2.2: Summary of frames used throughout the text Notation Frame i Inertial frame e Earth frame (WGS-84) l Local-level ENU frame b Body frame pointing from the center of the ellipsoid. In this text we will use the ENU frame [15,27]. To convert between NED and ENU frames the following rotation matrix can be used R NED ENU = (2.47) The matrix is found by observing that x NED = y ENU, y NED = x ENU and z NED = z ENU. The body frame is a right-handed three-dimensional Cartesian frame related to an object. In this case the object is the DAQ unit and it is used as origin. The basic vectors x b, y b and z b is dened to be the same as the axis used by the ADIS16405 sensor on the DAQ unit. This frame is used to determine the relative attitude with respect to the local-level frame. A summery of the frame used throughout this text is given in table 2.2. In the next sections we will describe how to transform between the four frames presented above. Transformation between the inertial frame and the earth frame The transformation from the earth frame to the inertial frame can be achieved by a simple rotation around the common z axis, and is dened to R i e = R z ( Θ(t)) (2.48) where R z ( ) is simple rotation around the z-axis dened in section and Θ(t) is the Greenwich sidereal time (GST). Θ(t) will not be needed for nding the navigation equations as will be seen later in this chapter. [27] Transformation between the earth frame and the local-level frame The earth frame and the local-level frame does not have the same origin. If the point p e 0 is the origin of the local-level frame, then the transformation of the vector p l to the vector p e is given by p e = p e 0 R e l (φ, λ)p l (2.49) where R e l (φ, λ) is R e l (φ, λ) = sin φ sin λ cos λ cos φ sin λ sin φ cos λ sin λ cos φ cos λ cos φ 0 sin φ (2.50) 12

31 It is also possible to only use the rotation matrix if the vector is independent of the location, like a velocity vector. The velocity vector v l is transformed to v e by [27] v e = R e l (φ, λ)v l (2.51) Transformation between the local-level frame and the body frame For simplicity the origin of the local-level frame and the body frame is dened to be the same. The rotation matrix R l b transforms from the local-level frame to the body frame. This matrix depends on the attitude of the body and must therefore be calculated from the sensor readings Initializing the AHRS In [22] there is mentioned four dierent methods for initialization of the AHRS. The rst is optical alignment where the system is align with line-of-sight reference points. This could be known reference point on the ground or by using star trackers in space. The second method is gyrocompass alignment. This method uses the sensed accelerometer output to nd vertical alignment and use the gyroscope to sense the earth rotation and thereby nd north. This require a very accurate gyroscope and can not yet be achieved with a low-cost MEMS gyro. The third method is transfer alignment where the INS is matched with an already operating INS. This is commonly done with missiles launched from air crafts. The last method is GNSS-aided alignment. By matching the position output from the GNSS system with the output of the INS it is possible to estimate the alignment of the system. In this project we will use a modied version of the gyrocompass alignment alignment method. Our system [52] has a 3D magnetometer as an aid sensor. From the magnetometer we get a vector which we can match with the known magnetic eld of the earth using a model of the earth's magnetic eld. We also get a vector from the accelerometer which we can match to the known gravitation. The platform should be stationary when measuring the gravitation in order to make measured vector accurate. We now have two vectors in the ENU frame and two vectors in the earth frame. From this we can create a rotation matrix that will rotate from the ENU frame to the earth frame [17]. The solution to this problem is given in the next section. Attitude determination based on vector matching The problem of determining attitude by vector matching is often referred to as Wahba's problem, because Whaba was one of the rst to publish a solution to this problem [17]. In our case we have two vectors in one coordinate system that are known and the same two vectors are measured in another coordinate system, and we want to nd the rotation between these system. We want to use quaternions to represent the rotation matrix in our system, therefore we want to nd an algorithm that nds the quaternions directly, not nding the rotation matrix rst and then calculating the quaternions. [51] has tested several known methods for nding the quaternion which represents the rotation. The QUEST algorithm has the best performance in both test scenarios in [51], but has a higher computational cost than the Direct Quaternion algorithm. When the Direct Quaternion 13

32 algorithm uses a singularity avoidance scheme the QUEST algorithm is approximately 75 % slower. Since it is only necessary to run the algorithm once to initialize the AHRS the better performance is valued to be more important, and QUEST is chosen to be used. Algorithm 2.3 QUEST (created from [56]) Input: ˆv i, ŵ i, a i, σ i i [1, n] Output: q opt, P q // Calculate constants S = B B T = n i=1 a i(ŵ iˆv T i ˆv i ŵ T i ) z = n i=1 a i(ŵ i ˆv i ) σtot 2 = ( n ) 1 i=1 σ 1 i // Find Lagrange multiplier λ max σ = 1trS 2 κ = tr(adjs) δ = dets a = σ 2 κ b = σ 2 z T z c = δ z T Sz d = z T S 2 z solve λ 4 (a b)λ 2 cλ (ab cσ d) = 0 by Newton-Raphson's method to get λ max // Find optimal quaternion q opt α = λ 2 max σ 2 κ β = λ max σ γ = (λ max σ)α δ x = (αi [ βs ] S 2 )z [ ] q 1 γ q opt = = q γ2 x 2 x // Find the( covariance matrix P q P Q = 1 4 σ2 tot I n i=1 a iŵŵ ) T [ ] [ ] [ ] q q T 0 0 T q q T T P q = q q Iq 0 P Q q q Iq The QUEST algorithm is an optimal algorithm which determines the attitude that achieves the best weighted overlap of an arbitrary number of vectors. The algorithm seeks to satisfy the equations [56] Aˆv i = ŵ i i = 1,, n (2.52) where A is an orthogonal matrix (the attitude matrix), ˆv 1,, ˆv n is a set of reference unit vectors in the reference coordinate system, ŵ 1,, ŵ n is a set of observation unit vectors in the body coordinate system and n is the number of vectors. The Quest algorithm minimizes the loss function L(A) = 1 2 n a i ŵ i Aˆv i 2 (2.53) i=1 14

33 Algorithm 2.4 QUEST with special case n = 2(created from [56]) Input: ˆv 1, ˆv 1, ŵ 1, ŵ 2, a 1, a 2, σ 1, σ 2 Output: q opt, P q // Calculate constants S = a 1 (ŵ 1ˆv T 1 ˆv 1 ŵ T 1 ) a 2 (ŵ 2ˆv T 2 ˆv 2 ŵ T 2 ) z = a 1 (ŵ 1 ˆv 1 ) a 2 (ŵ 2 ˆv 2 ) σtot 2 = (σ1 1 σ2 1 ) 1 // Find Lagrange multiplier λ max σ = 1trS 2 κ = tr(adjs) δ = dets cos (θ v θ w ) = (ˆv 1 ˆv 2 )(ŵ 1 ŵ 2 ) ˆv 1 ˆv 2 ŵ 1 ŵ 2 λ max = a 2 1 a 2 2 2a 1 a 2 cos (θ v θ w ) // Find optimal quaternion q opt α = λ 2 max σ 2 κ β = λ max σ γ = (λ max σ)α δ x = (αi [ βs ] S 2 )z [ ] q 1 γ q opt = = q γ2 x 2 x // Find the covariance matrix P q P Q = 1 4 σ2 tot I ( σ2 2 1)ŵ σtot 2 1 ŵ T 1 ( σ2 1 1)ŵ σtot 2 2 ŵ T 2 (ŵ 1 ŵ 2 )(ŵ 1 ŵ T 2 ŵ 2 ŵ T 1 ) ŵ 1 ŵ 2 2 [ ] [ ] [ ] q q T 0 0 T q q T T P q = q q Iq 0 P Q q q Iq 15

34 Introducing the quantities n n B = a i ŵ iˆv T i σ = trb = a i ŵ i ˆv i (2.54) i=1 S = B B T = n a i (ŵ iˆv T i ˆv i ŵ T i ) z = i=1 i=1 i=1 n a i (ŵ i ˆv i ) (2.55) the problem can be reduced to maximize the function g(q) = q T Kq subject to q T q = 1 (2.56) where [ S σi z K = z T σ ] (2.57) It can be shown that K q opt = λ max q opt (2.58) This means that the optimal quaternion is the eigen vector for the largest eigen value of K. Algorithm 2.3 shows the QUEST algorithm. For the special case of only two vectors there is possible to simplify some of the equations. This is showed in algorithm 2.4. In this thesis we will only use the two vector version, the Newton-Raphsons method is therefore not covered. For detailed information on the Newton-Raphsons please refer to [41]. Both algorithms 2.3 and 2.4 can experience singularities if both γ and x equals zero. This can occur if the rotation is close to π. [56] suggests a sequential rotation scheme to avoid this problem, but at the same time it is emphasized that γ needs to become quite small before this method is needed. The algorithm is meant to initialize the AHRS and will therefore use the average value of the rst samples from the accelerometer and magnetometer in the algorithm. Since the samples are noisy it is likely that average will change if the number of samples change. If the algorithm fail we may increase the number of samples we use to calculate the average and run the algorithm again. This time we probably will have dierent vectors that would not create a singularity. To achieve optimal performance a i is chosen to be a i = σ2 tot σ 2 i (2.59) where σ 2 tot is ( n ) 1 σtot 2 = (σi 2 ) 1 i=1 (2.60) and σ 2 i is the variance of the reference vector i pluss the variance of the observation vector i. 16

35 2.2.4 Continuous navigation equations We are now going to derive the navigation equations in a continuous state space model. First we will nd the navigation equations in the earth frame. From this equations we will nd the navigation equations in with velocity in the local-level frame and position in the earth frame. At last we will include the rotation between the local-level frame and body frame to be able to use measurements from the gyroscopes and accelerometers. The navigation equation in the inertial frame can be found in straightforward manner. The sum of forces is the dierence between the acceleration of the system p i and the gravitational acceleration g i p i = f i g i (2.61) We use the notation p = [p x p y p z ] for position instead of x to avoid confusing the position vector with x axis, the position x and the basic vectors. By rearranging the terms above we get the following model v i = f i g i (2.62) ṗ i = v i (2.63) where v is the velocity vector [27]. This formulation is however not useful because we want to know the position in reference to the earth. The relationship between the inertial frame i and the earth frame e is p i = R i ep e (2.64) where R i e is the rotation matrix from frame i to frame e. The rotation matrix is formulated in (2.48) in section Dierentiating this equation we get ṗ i = R i eṗe Ṙi ep e (2.65) p i = R i e pe 2Ṙi eṗe R i ep e (2.66) By using the dierential equation for matrices found in (2.33) we get ṗ i = R i e(ṗ e (ω e ie) p e ) (2.67) p i = R i e( p e 2(ω e ie) ṗ e ( ω e ie) p e (ω e ie) (ω e ie) p e ) (2.68) where ω e ie is [13] ω e ie = ω i ie = 0 0 ω ie (2.69) where ω ie is angular rotation rate of the earth dened in the WGS-84 standard in table 2.1. Apart from the acceleration p e of the object relative to the earth frame in (2.67) there are three other types of accelerations in the earth frame. 2(ω e ie) ṗ e is the Coriolis acceleration that appears when a object moves in a rotating frame. ( ω e ie) p e is the 17

36 tangential acceleration caused by the change of angular velocity of the earth frame. And at last (ω e ie) (ω i ie) p e is the centrifugal acceleration due to the rotation of the earth frame with respect to the inertial frame. We assume that the earth rotate at a constant speed. This implies that ( ω e ie) = 0. With this and the fact that R i er e i = I leads to p e = R e i pi 2(ω e ie) ṗ e (ω e ie) (ω e ie) p e (2.70) By replacing p i with the specic-force equation (2.61) we get R e i pi = R e i (f i g i ) = f e g e (2.71) where f e is the specic force vector in the earth frame and g e is the gravitation vector in the earth frame. Putting (2.71) into (2.70) yields p e = f e g e 2(ω e ie) ṗ e (ω e ie) (ω e ie) p e (2.72) The gravitation vector and the centrifugal acceleration is combined to ḡ e = g e (ω e ie) (ω e ie) p e (2.73) Substituting (2.73) into (2.72) gives us the specic-force equation in the earth frame p e = f e ḡ e 2(ω e ie) ṗ e (2.74) and the rst part of the derivation is completed [27]. In the next part of the derivation we want to transform the equation (2.74) into the local-level frame. This can be achieved by introducing the velocity vector v l in the locallevel frame. The relationship between the velocity in the earth frame and the local-level frame is given by The time derivative is then ṗ e = R e l v l (2.75) p e = R e l ( v l (ω l el) v l ) (2.76) Substituting (2.75) and (2.76) into (2.74) and premultiplying with R l e yields In [27] it is shown that v l = f l ḡ l ((ω l el) 2R l e(ω e ie) R e l )v l (2.77) (ω l el) 2R l e(ω e ie) R e l = (ω l il) (ω l ie) (2.78) where (ω l il ) is the rotation of the local-level frame with respect to the inertial frame and (ω l ie) is the earth rotation, both represented in the local-level frame. This leads to the navigation equations in the local-level frame v l = f l ḡ l ((ω l il) (ω l ie) )v l (2.79) ṗ e = R e l v l (2.80) These equations produce the position in the earth frame by using the force from the local-level frame [27]. 18

37 Including measurements and attitude When using accelerometers the force is measured in the body frame, and it is therefore necessary to convert the force measurements to the local level frame. This is easily done with the equation f l = R l bf b (2.81) where R l b is the matrix describing the attitude of the object. The dierential equation for R l b is Ṙ l b = R l b(ω b lb) (2.82) Since ω b lb is not directly sensed by the gyros it is necessary to transform the equation. This is done by dividing ω b lb into two parts ω b lb = ω b li ω b ib = ω b il ω b ib (2.83) where ω b ib is measured by the gyros and ωb il is the rotation rate of the local-level frame with respect to the inertial frame. ω b il may be expressed in the local-level frame as [5,15] ω l il = φ ( λ ω ie ) cos φ ( λ ω ie ) sin φ (2.84) where ω ie is angular rotation rate of the earth dened in the WGS-84 standard in table 2.1 [27]. The matrix dierential equation (2.82) can be transformed into vector dierential equation using quaternions. The author was not able to nd this derivation in a book, therefore the derivation in the rest of this section is made by the author. Let the quaternion q l b = [η ɛ] represent the rotation matrix Rl b. By rotating ω b il with the quaternion q l b we get If we insert the result above into (2.83) we get ω b il = R b l ω l il (2.85) = (I 2ηɛ 2ɛ ɛ ) T ω l il (2.86) = (I 2ηɛ 2ɛ ɛ )ω l il (2.87) ω b lb = (I 2ηɛ 2ɛ ɛ )ω l il ω b ib (2.88) The dierential equation for q l b is given by [13] η = 1 2 ɛt ω b lb (2.89) ɛ = 1 2 (ηi ɛ )ω b lb (2.90) 19

38 First we will calculate η, then ɛ. Inserting (2.88) into (2.89) yields η = 1 2 ɛt ((I 2ηɛ 2ɛ ɛ )ω l il ω b ib) (2.91) = 1 2 (ɛt 2ηɛ T ɛ 2ɛ T ɛ ɛ )ω l il 1 2 ɛt ω b ib (2.92) We can use the fact that ɛ T ɛ = 0 T to reduce the equation. This is correct because η is now reduced to For ɛ we do the same as for η, producing (ɛ T ɛ ) T = (ɛ ) T ɛ = ɛ ɛ = 0 (2.93) η = 1 2 ɛt ω l il 1 2 ɛt ω b ib (2.94) ɛ = 1 2 (ηi ɛ )(I 2ηɛ 2ɛ ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.95) = 1 2 (ηi 2η2 ɛ 2ηɛ ɛ ɛ 2ηɛ ɛ 2ɛ ɛ ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.96) = 1 2 (ηi 2η2 ɛ ɛ 2ɛ ɛ ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.97) By using the facts a a a = (a T a)a and η 2 a T a = 1 found in [13] we get ɛ = 1 2 (ηi ɛ 2(η 2 ɛ T ɛ)ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.98) = 1 2 (ηi ɛ 2ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.99) = 1 2 (ηi ɛ )ω l il 1 2 (ηi ɛ )ω b ib (2.100) Combining the η and ɛ into the quaternion q l b yeilds [ ] q l b = = η ɛ 1 [ ] ɛ T 2 ηi ɛ ω l il 1 [ ] ɛ T 2 ηi ɛ ω b ib (2.101) = 1 [ ] 0 (ω l il ) T 2 ω l il (ω l q l il ) b 1 [ ] 0 (ω b ib ) T 2 ω b il (ω b q l ib ) b (2.102) By using the denitions of Ω(ω) and Ω(ω) in section we can write the dierential equation as q l b = ( Ω(ω b ib ) Ω(ω l il) ) q l b (2.103) Using R(q l b ) as the rotation matrix Rl b created from the quaternion q l b, as in (2.24). Combining (2.79), (2.80) and (2.103) into one system yeilds v l ṗ e q l b = R(q l b )f b ḡ l ((ω l il ) (R l e(φ, λ)ω e ie) )v l = R e l (φ, λ)v l = ( b Ω(ω ib ) Ω(ω l il )) q l b (2.104) all of the components of the equations above is previously dened in this section except ḡ, φ and λ. These will be explained in the next sections 20

39 Gravity model In order to compensate for the gravitation we need an accurate gravitation model. The gravity normal to the ellipsoid is γ given by [5] 1 k sin 2 φ γ(φ) = γ e 1 e2 sin 2 φ (2.105) where γ e and e 2 is WGS-84 parameters given in table 2.1, φ is the latitude and k is [5] k = bγ p aγ e 1 (2.106) where a, b and γ p is WGS-84 parameters given in table 2.1. When the object is above the surface of the ellipsoid the gravity vector will be slightly o the normal to the ellipsoid. This will produce a gravity component along the north direction. For a point h meters over the mean sea level the downward component of the normal gravity is [5] ( γ h (φ, h) = γ(φ) 1 2 ( 1 f m 2f sin 2 φ ) ) h (2.107) a where a, f, m is WGS-84 parameters and φ is the latitude. The north component is [5] In vector form the normal gravity is [5] γ n (φ, h) = 8, h sin(2φ) (2.108) γ l = 0 γ n (φ, h) γ h (φ, h) (2.109) This can replace the gravity vector ḡ l in the system (2.104) by setting ḡ l = γ l. Dierential equations for latitude and longitude To be able to use the system (2.104) we need to nd φ and λ, because ω l il depend on these two values. First we dene the meridian radius of the curvature to be [5] R m (φ) = a(1 e 2 ) ( 1 e2 sin 2 φ ) 3/2 (2.110) where φ is the latitude and a and e is WGS-84 parameters. Then we dene the prime radius of curvature to be [5] a R p (φ) = ( 1 e2 sin 2 φ ) (2.111) 1/2 where φ is the latitude and a and e is WGS-84 parameters. In [5] the dierential equations for latitude and longitude is given as v n φ = R m h v e λ = (R p h) cos φ (2.112) (2.113) 21

40 where h is the height above mean sea level, φ is the latitude, v n is the velocity in the north direction, v e is the velocity in the east direction and R m and R p is dened previously in this section. By using this we can rewrite ω l il from (2.84) to ω l il = vn R mh v e ω R ph ie cos φ v e tan φ ω R ph ie sin φ (2.114) Now all components in the system (2.104) is well dened and it is possible to use the system to calculate the position to an object. We have now found the dierential equations for the latitude and longitude and it is therefore possible to calculate these values directly by replacing the p e vector with the (φ, λ, h) vector. The change in the height above sea level is simply the velocity in this direction, thus ḣ = v u (2.115) By putting φ, λ and h into one system we get 1 φ 0 R mh 0 λ = 1 (R ph) 0 0 cos φ ḣ v l ω ie (2.116) We now have two ways of calculating position in the earth frame, either by calculating the Cartesian coordinates by using (2.80) or by calculating the ellipsoidal coordinates by using (2.116) Discrete navigation equations By using Euler's method [13] to create a discrete model of the continuous nevigation equations in (2.104) yields v l [k 1] = T (R(q l b [k])f b [k] ḡ l [k]) (I T ((ω l il [k]) (R l e(φ[k], λ[k])ω e ie[k]) ))v l [k] p e [k 1] = p e [k] T R e l (φ[k], λ[k])v l (2.117) [k] q l b [k 1] = ( I T ( b Ω(ω ib [k]) Ω(ω l il [k]))) q l b [k] where T is the sampling time. Creating a discrete model of (2.116) by using Eulers methods yields 1 φ[k 1] φ[k] 0 R m[k]h[k] 0 λ[k 1] = λ[k] T 1 (R p[k]h[k]) 0 0 cos φ[k] v l [k] (2.118) h[k 1] h[k] Noise All real world data from sensors experience noise. This section will explain the most common noise models. 22

41 2.3.1 Allan Variance Allan variance was created by Allan to characterize the stability of oscillator. It is a method to nd the underlying random processes that can be used to create a model of a sensor. The method is used by IEEE to characterize noise in gyroscopes in the IEEE standard Std [31]. The standard identies ve basic gyro noise terms, angle random walk, rate random walk, bias instability, quantization noise and rate ramp. This text will only consider angle random walk and bias instability because the gyroscopes used only have these two terms given in their datasheet [11]. To calculate Allan variance from a M size dataset with sampling time T we divide the dataset into clusters. First we form clusters of length T and nd the average of each cluster. Then take the dierence of the average of the successive clusters squared and add them up. Then divide by a scaling factor. Repeat the procedure for 2T,, kt where k < M/2 [58] Allan variance is dened by [31] σ 2 AV AR(τ) = 1 2 < (Ω km Ω k ) 2 >= 1 2τ 2 < (θ k2m 2θ km θ k ) 2 > (2.119) where <> is the ensemble average and τ = mt is the length of the clusters. θ i is the angle calculated from the following denition θ(t i ) = ti t i 1 Ω( t)d t (2.120) where t i = T i. Allan variance can be estimated from the following formula σ 2 AV AR(τ) = M 2m 1 (θ 2τ 2 k2m 2θ km θ k ) 2 (2.121) (M 2m) The relationship between Allan variance and power spectral density is given by Angle random walk σ 2 AV AR(τ) = 4 0 k=1 S Ω (f) sin4 (πfτ) (πfτ) 2 df (2.122) Angle random walk is due to white noise in the measurement. This noise has a power spectral density given by S ARW (f) = N 2 (2.123) where N is the angle random walk coecient. solving the integral we get By inserting (2.123) into (2.122) and σ AV AR (τ) = N 2 τ (2.124) Figure 2.2 shows a graph where (2.124) is plotted. From the gure we see that σ AV AR falls with 1/2 per decade. The value of N can be read from the graph at τ = 1 [31]. 23

42 Figure 2.2: Log-log graf over σ AV AR ved Angle random walk [31] Figure 2.3: Log-log graf over σ AV AR ved Bias instability [31] 24

43 Figure 2.4: Log-log graf over σ AV AR with all noise sources [31] Bias instability Bias instability is due to icker noise in the measurements. This noise type has a power spectral density given by [31] {( ) B 2 1 f f 2π f 0 S BI (f) = (2.125) 0 > f 0 where f 0 is the cut o frequency and B is the bias instability coecient. By using (2.122) and (2.125) and approximate we get σ AV AR (τ) B 2 2 ln 2 (2.126) π Figure 2.3 show a graph of the bias instability. The dashed line corresponds to (2.126). The bias instability coecient can be found by nding the lowest point in a Allan variance graph and calculate the coecient from (2.126) [31]. Overview There often exist dierent noise types in a measurement. These can be identied from an Allan variance plot because the dierent noise types appear in dierent regions of the plot. Figure 2.4 show a typical Allan variance plot and where to nd the dierent noise types. If the noise sources can be considered uncorrelated one can add the contribution from each noise source by using the following formula [31] σ 2 tot = σ 2 ARW σ 2 BI (2.127) Estimating Allan Variance parameters using regression To estimate the parameters Angular Random Walk and Bias Instability from measurements we will use regression. We use the least square method from statistics [60]. When 25

44 the Allan Variance is calculated from the measurements we want to t it to the function σ AV AR (τ) = N 2 τ 2 B2 ln 2 (2.128) π By dening the a 1 to be N 2 and a 2 to be B 2 2 ln 2 and substituting τ with mt we get π Now we dene the square error to be SSE = = σ AV AR (mt ) = a 1 mt a 2 (2.129) M (σ AV AR (mt ) σ AV AR (mt )) 2 (2.130) m=1 M m=1 ( σ AV AR (mt ) a ) 2 1 mt a 2 (2.131) where σ AV AR (mt ) is the calculated Allan variance for the averaging time mt. By differentiating with respect to a 1 and a 2 we get (SSE) a 1 (SSE) a 2 = 2 = 2 M m=1 M m=1 1 mt ( σ AV AR (mt ) a ) 1 mt a 2 ( σ AV AR (mt ) a ) 1 mt a 2 By solving with respect to a 1 and a 2 we get ) M m=1 (σ AV AR (mt ) σ AV AR(mT ) mt (2.132) (2.133) a 1 = M ( 1 m=1 ) (2.134) 1 mt m 2 T 2 M ( ) 1 m=1 σav AR (mt ) a 1 mt a 2 = (2.135) M where a 2 is simply the average of the dierence between the calculated Allan variance 1 σ AV AR (mt ) and a 1. a mt 1 has a more complex function which is sum of calculated Allan variance minus the sum of the calculated divided on averaging time. This is divided on the sum of averaging times minus the sum averaging time squared White noise White noise is dened to be a stationary random process with a constant power spectral density. [18] S(ω) = Q (2.136) It contains all frequencies and is therefore named white after white light, which also contains all visible frequencies. White noise is often used to model noise from real data, for instance data sampled by sensors. The autocorrelation for white noise is [18] R(τ) = Qδ(τ) (2.137) 26

45 Figure 2.5: Log-log graph over the power spectral density of dierent noise processes. For rst order Gauss-Markov process β = 2. For all processes Q = 1. where δ(τ) is the Dirac delta function. The noise has unlimited bandwidth, but this is not possible in a physical system. Physical systems experience bandlimited white noise, but the unlimited bandwidth white noise serves as a good approximation when the noise of the system is wide band compared to the bandwidth of the system. White noise is also used to create other types of noise, as we shall see in the sections below. When the white noise has a Gaussian distribution it is called Gaussian white noise. [8, 18] In the discrete case we have a white sequence which is dened as sequence of zero mean, uncorrelated random values. This implies that the autocorrelation is given by [8, 38] E[w m w l ] = Qδ ml (2.138) where δ ml = 1 if m = l and δ ml = 0 when m l and w m and w l is mth and lth random value of the sequence. It is possible to create a discretization of continuous white noise by using the formula [38] Q = Q T (2.139) where T is the time step, to nd the autocorrelation for the discrete white sequence. When the random values has a Gaussian distribution the noise sequence is called a Gaussian white sequence. 27

46 2.3.3 Flicker noise Flicker noise is also often called 1/f noise or pink noise and has a power spectral density [38] S(ω) = Q ω (2.140) It is not possible to create this noise from Gaussian white noise in a simple manner. Kaulakys and Ruseckas claims to have created a nonlinear stochastic dierential model that can create 1/f noise [39, 40], but the author is unable to reproduce the results in these articles. The theory is based on a point process I(t) = a k δ(t t k ) (2.141) where δ(t t k ) is the Dirac delta function, t k is a set of occurrence times and a is the contribution of each signal. The set of occurrences is described by the equations [39] t k1 = t k τ k (2.142) τ k1 = τ k σɛ k (2.143) where ɛ k is normally distributed uncorrelated variables with zero mean. This point process has a power spectral density proportional to 1/f. The paper [39] then derives the stochastic dierential equation to be dx dt = σ2 a 3 x4 σ a 3/2 x5/2 v(t) (2.144) This model has been simulated with Matlab and Simulink. Figure 2.6 shows the results. The power density spectrum has a 1/f behavior on a limited portion of the frequency band shown in gure 2.7. The paper also states a simulation model from the dierential equation as [39] x k1 = x i n ( ) x n min 2 x n 4 xn4 i T x 4 i x n i T x 5/2 i T ɛi (2.145) max This model has also been simulated. Figure 2.8 shows the result. At higher frequencies the power spectrum density has sub 1/f 2 behavior, but is not proportional to 1/f. [39] claims that this process should exhibit 1/f noise over a wide frequency spectrum, but the author are unable to reproduce this. It is possible to create 1/f noise to use in simulations by ltering a white noise sequence. The desired power spectral density is S(f) = Q/f and a digital lter dened by H(z) = will create a power spectral density of [38] S(f) = 1 (1 z 1 ) 1/2 (2.146) QT 2 sin πft 28 (2.147)

47 Figure 2.6: Simulation of (2.144) with a = 1, σ = 1, Q = 1 and x(0) = 0, 1. The black graph is the estimated power spectral density of x, the red graph is a ltered version of the black graph using the lter y k1 = 0.75y k 0.25x k. The blue graphs are 1/f and 1/f 2 lines. Figure 2.7: Simulation of (2.144). Zoomed version of gure

48 Figure 2.8: Monte Carlo simulation of power spectral density of (2.145). The Power spectral density has been simulated 100 times and the solid graph shows the average result. Parameters used are x min = 0.5, x max, n = 1 and T = 0.01 where Q = Q/T. It is showed in [38] that the pulse response of the lter is given by h 0 = 1 (2.148) ( h k = k 1 ) hk 1 (2.149) 2 k This pulse response has innite length and can therefore not be used as a simple model for 1/f noise, but works well for nite length simulations because it is only necessary Nth rst pulse responses if the simulation length is N to calculate the noise. There is also possible to create a AR lter with similar characteristics. For a AR lter x n = a 1 x n 1 a 2 x n 2 a 3 x n 3 w n (2.150) where w n is a white noise sequence, the lter coecients are [38] Brownian motion a 0 = 1 (2.151) ( a k = k 3 ) ak 1 (2.152) 2 k Brownian motion is dened to be the integral of Gaussian white noise [38] ẋ = w (2.153) 30

49 This noise is also called a Wiener process, because of N. Wiener work on developing the mathematical treatment of the process. It can be showed that this process is nonstationary with a autocorrelation [8, 38] { Qs for t s R(t, s) = Qt for t < s (2.154) where Q is the power spectral density of the white noise process w. The correlation time is undened for Brownian motion and therefore it is said that Brownian motion has innite memory. It can also be showed that the power spectral density for Brownian motion process is [38] S(ω) = Q ω 2 (2.155) Figure 2.5 shows a plot of this function. The spectral density of the Brownian motion is proportional to 1/f 2 and is therefore in some texts also referred to as 1/f 2 noise. It is possible to create a discretization of the Brownian motion using the dierence equation [38] x k1 = x k w k (2.156) where w k is the kth element of a white noise sequence with a variance Q = QT. This model can be viewed as taking a random size step at each interval, and thereby making a random walk. Because of this the model is often called a random walk model. By using z-transform we can calculate the power spectral density of the discrete model. The result is [38] S(ω) = QT ( ) 2 sin ωt 2 (2.157) 2 where T is the time step. This a periodic function and does not sample the Brownian motion perfectly. However for small frequencies S(ω) Q/T ω 2 and is therefore a good approximation on low frequencies Gauss-Markov process = Q ω 2 (2.158) By passing white noise through a lter it is possible to generate a special class of noise processes called Gauss-Markov process. It if possible to create Gauss-Markov processes of dierent orders by using dierent orders on the lter, but in this text we will only cover rst order Gauss-Markov prosesses. The equation for a rst order Gauss-Markov process is [18] ẋ = βx w (2.159) 31

50 Figure 2.9: Log-log graph over the power spectral density of a rst order Gauss-Markov process. Solid line is a continuous process and dashed line is the discretization of the continuous process. Parameters used is β = 0, 5, T = 0, 1, Q = 1 and f c = 10. where w is Gaussian white noise with a autocorrelation of Qδ(τ). The autocorrelation of the rst order Gauss-Markov process is [18] R(τ) = Qe β τ (2.160) Because the autocorrelation only is depended on τ the process is stationary. The correlation time is 1/β. The power spectral density is S(ω) = 2β Q (2.161) ω 2 β 2 Figure 2.5 shows that S(0) has a nite value, while the Brownian motion process has innite value when ω = 0. When the frequency get higher The Gauss-Markov process get the same 1/f 2 characteristics as the Brownian motion process. The Gauss-Markov process can be discretized with a time step T to the following stochastic dierence equation [8, 38] where w k has a variance of x k1 = e T β x k w k (2.162) Q = 1 2 Q(1 e 2T β ) (2.163) 32

51 By using z-transform the power spectral density function can be shown to be S(ω) = QT 1 e 2T β 2e T β cos ωt (2.164) This is clearly not the same as the continuous model. Figure 2.9 shows both the power spectral density of the continuous process and the discrete process. They match for low frequencies, but the spectral density of the discrete process start to raise around f c /2 due to aliasing [8, 38] Evaluating possible approximations of icker noise We have now seen several noise processes and their characteristics and that it is dicult to create a simple model of icker noise. We would now want to nd which noise process that approximates a icker noise process most accurately. The derivations and discussion found in this section is created by the author. There are two approaches that shall be investigated, rst if a Brownian motion process or a Gauss-Markov process can be used to approximate icker noise and second if a nite version of the innite AR lter that created icker noise could be used as a good approximation. First we assume that noise has both a white noise portion (S w (f)) and a icker noise portion (S p (f)). The power spectral density of the noise can therefore be written as S(f) = S w (f) S p (f) = Q w Q p 2πf (2.165) For high values of f the icker noise portion will be much smaller than the white noise portion, and for low frequencies the icker noise portion will have the highest values. Therefore it is important to have a good approximation on low frequencies up to the point where the white noise becomes the dominate noise. We dene the intersection between the white noise portion and the icker noise portion to be the point where icker noise goes from being the dominant noise to be the non dominant noise. The frequency this happens is denoted f c and is found by setting S w (f c ) = S p (f c ). Solving the equation gives f c = Q p 2π Q w (2.166) We can now create a approximation of icker noise from a Brownian motion process and a Gauss-Markov process by requiring that the power spectral density shall be Q w at f c. A Brownian motion process has one parameter, the autocorrelation coecient of the white noise Q b. By solving the power spectral density function of a Brownian motion process with the requirement we get Q b = Q 2 p Q w (2.167) Figure 2.10 shows the Brownian motion process compared to the ideal 1/f process. The discrete version of the Brownian motion has been used. From the gure we see that the 33

52 Figure 2.10: Log-log graph over the power spectral density over the lters used to approximate icker noise. The graph shows the corresponding discrete versions of the noise processes. Parameters used is Q p = , Qw = 1, , S max = 0.1 and T = 1 Brownian motion process and the ideal 1/f process intersects the white noise line at the same point, but the Brownian motion process approaches innity when f approaches zero faster than the ideal 1/f process. The Brownian motion process will therefore create a larger uncertainty that the ideal 1/f process. A rst order Gauss-Markov process has two parameters, β and Q gm. As we have seen in the previous section the rst order Gauss-Markov process has a nite value when f = 0 while icker noise approaches innity. Therefore in addition to the rst requirement we want to have a high value S max when f = 0. These two requirements results in these two equations Q w = 2β Q gm (2πf c ) 2 β 2 (2.168) S max = 2β Q gm β 2 (2.169) Solving the equations gives β = Q p Qw Q w S max Q (2.170) w Q gm = Q p S max 2 Qw (S max Q w ) (2.171) 34

53 Figure 2.11: Log-log graf over the power spectral density response of the AR lter creating icker noise from section with a nit set of coecients. Figure 2.10 shows the Gauss-Markov motion process compared to the ideal 1/f process. The process intersects the white noise line at the same point as the ideal 1/f process, but does not approach innity as f approaches zero. There might be possible to tune the Gauss-Markov process to create the same amount of uncertainty that the ideal 1/f process creates by adjusting S max. The last approach is to use the AR lter described in section with a nite set of coecients. Figure 2.11 shows the power spectral density of the AR lter for dierent number of coecients. The higher number of coecient the more accurate approximation of the noise. The main drawback of this method is the computational load. For a lter of size n one must compute n multiplications and n 1 summations. The increased memory usage is n numbers. Compared to the two other approximations this process has signicantly larger computational load and memory usage. Figure 2.10 shows the AR lter process compared to the ideal 1/f process using 30 coecients. The 1/f characteristics occur mainly below the white noise line. To increase the region where the AR lter produces 1/f characteristics we need to increase the lter length. All three approximations shall be reviewed in section by using them to model gyro bias instability with a Kalman lter. 2.4 Kalman lters The Kalman lter is an optimal estimator for linear systems [8,18]. An optimal estimator is an estimator that minimizes the the mean square error. The lter exist both using a 35

54 continuous system and a discrete system. The continuous system is dened as ẋ(t) z(t) = F (t)x(t) G(t)v(t) = H(t)x(t) w(t) (2.172) where E[x(t 0 )] = x 0 E[x(t 0 )v T (t)] = 0 E[(x(t 0 ) x 0 )(x(t 0 ) x 0 ) T ] = P 0 E[v(t 0 )] = 0 E[x(t 0 )w T (t)] = 0 E[v(t)v T (τ)] = δ(t τ) Q(t) E[w(t 0 )] = 0 E[v(t)w T (τ)] = 0 E[w(t)w T (τ)] = δ(t τ) R(t) (2.173) The discrete system is dened as x k1 = Φ k x k Γ k v k (2.174) z k = H k x k w k where Linear lter E[x 0 ] = x 0 E[x 0 v T k ] = 0 E[(x 0 x 0 )(x 0 x 0 ) T ] = P 0 E[v k ] = 0 E[x 0 w T k ] = 0 E[v kv T l ] = δ klq k (2.175) E[w k ] = 0 E[v k w T l ] = 0 E[v kv T l ] = δ klr k The solution to the linear case (2.172) is [8, 18, 23] ˆx(t) = F (t)ˆx(t) K(t)(z(t) H(t)ˆx(t)) (2.176) ˆP (t) = F (t) ˆP (t) ˆP T (t)f (t) G(t) Q(t)G(t) K(t) R(t)K(t) (2.177) K(t) = ˆP (t)h T (t) R 1 (t) (2.178) Where ˆx(t) is the estimated state vector, ˆP (t) is the covariance matrix for the estimated state vector. K is the Kalman lter gain. ˆx(t 0 ) and ˆP (t 0 ) is known. The solution to the discrete case (2.174) is divided in two parts, the time update and the measurement update [8, 18, 23]. The time update is given by The measurement update is given by x k1 = Φ k ˆx k (2.179) P k1 = Φ k ˆP k Φ T k Γ k Q k Γ T k (2.180) ˆx k = x k K k (z k H k x k ) (2.181) K k = P k H T k (H k P k H T k R k ) 1 (2.182) ˆP k = (I K k H k ) P k (2.183) In the above equations x k is the predicted state vector, ˆx k is the estimated state vector, P k is the covariance matrix for the predicted state vector, ˆP k is the covariance matrix for the estimated state vector and K k is the Kalman gain. It is assumed that ˆx 0 and ˆP 0 is known. 36

55 Information lter The information lter calculates the inverses of the covariance matrices [3]. This means that I = P 1, where I is the information matrix. This enables calculation when one or more states are unknown, and thus the covariance for the unknown state is innity and the inverse covariance is zero. The time update to this problem is given by [3, 57] Q k = Γ k Q k Γ T k (2.184) Ī k1 = Q 1 k Q 1 k and the measurement update is given by [3, 57] Φ k (Îk Φ Q 1 k k Φ T k ) 1 Φ T k Q 1 k (2.185) Î k = Īk H T k R 1 k H k (2.186) K k = (Îk) 1 H T k R 1 k (2.187) The time and measurement updates for the state vector is the same as the discrete Kalman lter and is therefore not given. There is possible to formulate the time update another way, to avoid taking the inverse of Q. This formulation is [3] A k = Φ T k Î k Φ Nonlinear systems k (2.188) ) 1 Γ T k A k (2.189) Ī k1 = A k A k Γ k ( Γ T k A k Γ k Q 1 k In many applications the Kalman lter can not be used because the system is nonlinear. Several methods to adapting the Kalman lter to nonlinear systems exists and two of them will be presented below. First we dene the nonlinear system to in the discrete case as where the assumptions is given in (2.175) Extended Kalman lter (EKF) x k1 = f k (x k, v k ) z k = h k (x k ) w k (2.190) The extended Kalman lter is a method to adapt the original Kalman lter to work with a nonlinear system [57]. By linearizing the nonlinear equations in every step k we get matrices that can be used with the equations for the linear case. If the system and measurement equations are given in (2.190) then the algorithm for the extended Kalman lter is presented in algorithm 2.5. If the system and measurement equations are linear the extended Kalman lter is identical to the original Kalman lter. The extended Kalman lter is a rst order approximation and [36] and [61] points on several disadvantages with the extended Kalman lter. First the linearized transforms may not approximate the error propagation well. This can introduce large errors in the states and covariance in the lter and sometimes this can lead to divergence of the lter. There exist higher order versions of the extended Kalman lter, but the increased implementation and computational complexity make them dicult to use. A second 37

56 Algorithm 2.5 Discrete Extended Kalman lter (based on [57]) Input: x 0, P 0, Q k, R k, f k (x k, v k ), h k (x k ), k max Output: x k, ˆx k, P k, ˆP k // Initialize the lter k = 0 for k < k max do // Linearize measurement equation H k = h k x T xk // Perform the measurement update ˆx k = x k K k (z k H k x k ) K k = P k H T k (H k P k H T k R k ) 1 ˆP k = (I K k H k ) P k // Linearize system equation Φ k = f k x T ˆxk Γ k = f k v T ˆxk // Perform the time update x k1 = f k (ˆx k, 0) P k1 = Φ k ˆP k Φ T k Γ k Q k Γ T k end for problem is that the Jacobian matrix may only be used if it exists, it can for instance be singular. The last problem is that calculating Jacobian matrices is an error-prone process. The Jacobian equations are often large and when converting them to code human error is likely to occur. As we will see in the next section the unscented Kalman lter addresses these problems. Unscented Kalman lter (UKF) The unscented Kalman lter is based on the principle that it is easier to approximate the probability density functions than the nonlinear functions. This is achieved by using the Unscented transform [35,36,61]. In this text we use the Scaled Unscented Transformation [34], which guarantees that the mean and covariance has a second order accuracy. By using the unscented transform we do not need to calculate Jacobians or Hessians and the computational complexity is the same as the extended Kalman lter [34]. A set of sigma points is selected so that the points have the same mean and covariance as the underlying distribution, which is x and P xx. If the random variable has dimension 38

57 L then 2L 1 points are selected by the following scheme [61] X = [ x x (L λ)p xx x (L λ)p xx ] (2.191) W m 0 = λ/(l λ) (2.192) W0 c = λ/(l λ) (1 α 2 β) (2.193) = Wi c 1 = k [1, 2L] 2(L λ) (2.194) W m i where λ = α 2 (Lκ) L is a scaling parameter. The constants α, β and κ are parameters that can be used to optimize the transform. Prior knowledge about the distribution is given to the transform by the constant β. For Gaussian distributions β = 2 is optimal [34]. The spread of the sigma points around x is determined by the constant α, which is set a small positive value is suggested by [61]. κ is a secondary scaling parameter which is set to zero by [61]. X is a matrix with 2L 1 sigma vectors X i. All these vectors are used as inputs to the nonlinear function to produce the transformed sigma points Y = [Y 0 Y 2L ] Y i = f(x i ) i [0, 2L] (2.195) The above operation is written as Y = f(x ) in algorithm 2.6. The mean is given by the weighted average of the transformed points. ȳ = 2L i=0 W m i Y i (2.196) The covariance is given by the weighted outer product of the transformed points P yy = 2L i=0 W c i (Y i ȳ)(y i ȳ) T (2.197) The unscented Kalman lter algorithm is given in algorithm 2.6, and is based on the algorithms found in [61] and [35]. The notation is changed to be similar to the notations used throughout this text. The unscented Kalman lter addressed many of the drawbacks of the extended Kalman lter. Greater lter accuracy and approximately equal computational complexity has been mentioned. The extended Kalman lter could experience problems with singularities in the Jacobian matrices. This problem is not present in the unscented Kalman lter because the calculation of Jacobian matrices is unnecessary. 2.5 Converting sensor data to navigation information Not all sensor data is directly usable in the navigation equations. These data needs to be converted in order to be useful. In this section we will describe how to convert pressure information to altitude information and magnetic eld data into attitude information. 39

58 Algorithm 2.6 Unscented Kalman lter (based on [35, 61]) Input: x 0, P 0, Q k, R k, f k (x k, v k ), h k (x k ), k max Output: x k, ˆx k, P k, ˆP k // Initialize the lter k = 0 // Set scaling parameters α = 10 3, κ = 0, β = 2, λ = α 2 (L κ) L // Calculate weights W0 m = λ/(l λ) W0 c = λ/(l λ) (1 α 2 β) for i = 1,..., 2L do W m i = 1 2(Lλ) Wi c = 1 2(Lλ) [ end for X x 0 = x x 0 x x 0 (L λ) P 0 x x 0 (L λ) P ] 0 for k < k max do // Measurement update Z k = h k ( X x k) z k = 2L i=0 W m i ˆP z k = R k ˆP xz k = 2L i=0 Z i,k 2L i=0 W c i ( Z i,k z k )( Z i,k z k ) T W c i ( X x i,k x k)( Z i,k z k ) T xz K k = ˆP k ( ˆP z k) 1 ˆx k = x k K k (z k z k ) ˆP k = P z k K k ˆP kk T k // Calculate [ sigma ] points ˆP a ˆP k = k 0 [ 0 ] Q k ˆx a ˆxk k = 0 [ ] ˆX a k = ˆx a k ˆx a k (L λ) ˆP a k ˆx a k (L λ) ˆP a k // Time update X x k1 = f k ( ˆX x k, ˆX v k) x k1 = P k1 = 2L i=0 2L i=0 W m i X x i,k1 W c i ( X x i,k1 x k1)( X x i,k1 x k1) T k = k 1 end for // Where X a = [ x x v X 0 X n x X 0 v X n v // x a = [ x T v T ] and L = n x n v 40 ]

59 Figure 2.12: The elements of the magnetic eld (based on [9, 45]) Pressure to altitude It is possible to use a pressure sensor as an altimeter [32]. The US Standard Atmosphere 1976 is an approximation of the atmosphere. The atmosphere up to m is divided into two parts, the troposphere which is up to m and the lower stratosphere which is between m and m. Our navigation is land based, therefore we only need to consider heights below m. The equation for the troposphere is ( p = P 0 1 h T ) g R T grad grad (2.198) T 0 where p is pressure, h is height above sea level, P 0 is zero altitude pressure, T 0 is zero altitude temperature which is 288, 15 K, T grad is the temperature gradient in the troposphere which is 6, K/m, R is the specic gas constant which is 287, 052 J/(K kg) and g is the gravity constant. By rearranging the above equation to be height above sea level as a function of pressure yields h(p) = T 0 T grad ( 1 p P 0 T grad ) R g (2.199) These functions does not take special weather conditions into account. Neither that the pressure changes with the weather. To compensate for pressure changes due to weather one can calibrate the zero altitude pressure by the following formula P 0,cal (h 0, p 0 ) = p 0 ( 1 h 0 T grad T 0 ) g R T grad (2.200) where P 0,cal is the calibrated zero altitude pressure, h 0 is the known altitude and p 0 is the pressure at this altitude Earth's magnetic eld The earth magnetic eld can be approximated as a dipole where eld points downwards in the northern hemisphere [10]. Figure 2.12 shows how the eld is related to the earth. 41

60 B is the total eld vector. Note that this is not the same vector notation used in this text. Uppercase is used because the literature on magnetic elds use uppercase. The vector is boldface to emphasize that it is not a scalar and it is not italic as the matrices. H is the horizontal eld, given by [45] H = X 2 Y 2 (2.201) The angle of declination D is the angle between the geographic north and the magnetic north. The angle of inclination or dip I is the angle between the horizontal plane created by X and Y and the total eld vector B. D and I is dened as [45] D = tan 1 Y X I = tan 1 Z H (2.202) Because the earth magnetic eld is not a perfect dipole extensive mapping of earth magnetic eld has been undertaken. From this measurements it has been created anomaly maps in order to compensate for the anomalies. Based on the anomaly maps there has been several attempts to crate a mathematical model of the earth magnetic eld spherical harmonic models. International Geomagnetic Reference Field (IGRF) and World Magnetic Model (WMM) [9]. We will use the IGRF model. This model is dened as [1] V (r, θ, λ, t) = R n max n=1 ( R r n1 ) n (gn m (t) cos mλ h m n (t) sin mλ)pn m (θ) (2.203) m=0 where r is the distance from the earth center, θ is the colatitude (90 φ) and λ is the longitude. R is a reference radius, gn m (t) and h m n (t) is the coecients at time t and Pn m (θ) is the Schmidt semi-normalized associated Legendre functions of degree n and order m. The latest IGRF model is the 11th generation and the coecients can be downloaded from Here it is also possible to download a Fortran program that calculates the eld based on the geocentric coordinates r, θ and λ and time t. An online calculator exists on gifs/igrf_form.shtml. By using the fact that [9] B = V (2.204) where B = [B r, B θ, B λ ] we get the eld from the model. To convert this into a NED frame we use [45] X = Bθ Y = B λ Z = B r (2.205) To convert this into the ENU frame that is used in this text the rotation matrix given in (2.48) can be used. The IGRF-11 Fortran program has been converted into a C program by Universidade do Algarve. This C program can be used in Matlab by using the mex function in Matlab to compile the C code to a Matlab binary le. This program can be downloaded from From this program we get the magnetic eld in the local-level frame and it is possible to compare this with the measured eld in the body frame to improve the attitude estimation. The creators of the IGRF model has created a guideline for the usage of the model [48]. These guidelines warns about using the IGRF model near the earth's surface because of 42

61 Figure 2.13: The crosses represents satellites. To the left is an example of poor satellite position. The case to the right has a lesser region of uncertainty, and will give a better position estimate. small elds created by for instance cars, buildings and magnetization of crustal rocks. In addition natural elds from electric currents in the ionosphere and magnetic storms will also contribute to the error. A root meas square error of approximately 200 nt is mentioned, but it is also stated that the error could be larger and smaller. We will use an RMS error 800 nt as an approximation of the model error. 2.6 Sensor errors and calibration In order to know the accuracy of our position estimate we need to know the accuracy of the sensor providing the position estimate. The following section will describe sensor errors in the sensors used in the DAQ unit and how to calibrate these sensors to minimize the errors when it is possible GPS model Before we can create a model of the position error from the GPS we need to review how to measure position error in more than one dimension and how the position estimate from the GPS is aected by satellite position. Dilution of precision (DOP) Dilution of precision, or DOP, is a number that characterizes the satellite geometry. If the satellites is spread out in the sky the GPS will produce a better estimate than if the satellites where close together. Because the distance estimate from the GPS receiver to the satellite has a uncertainty there will be a region of uncertainty when using several satellites. When the satellites has a good geometry this region will be smaller. This is shown in gure The gure clearly shows that the uncertainty region in the case to the right has a smaller region of uncertainty. The gure is a simplication in two dimensions, but the principle is the same [14]. The error in the position estimate from the GPS has many sources. Error in the satellite clock, signal delay in the ionosphere and atmosphere and multi-path is some of the error sources. If we assume that these errors can be reduced to a single constant σ UERE, which is known as the total user equivalent range error. We also assume that the error is equal in all directions and over all observation. The typical value for σ UERE is 43

62 around 25 m for commercial GPS systems. The dierent DOP values may be represented as a function of σ UERE like this [46] GDOP = P DOP = HDOP = V DOP = σ 2 E σn 2 σ2 U σ2 T (2.206) σ UERE σ 2 E σn 2 σ2 U (2.207) σ UERE σ 2 E σn 2 (2.208) σ UERE σ 2 U (2.209) σ UERE T DOP = σ 2 T σ UERE (2.210) where σe 2, σ2 N, σ2 U is the variance in the east, north and upward direction of the position estimate. σt 2 is the variance to the clock error. GDOP is the geometrical DOP, PDOP is the positional DOP, HDOP is the horizontal DOP, VDOP is the vertical DOP og TDOP is the time DOP. The relationship between the dierent DOP values is given by [46] P DOP 2 = HDOP 2 V DOP 2 (2.211) GDOP 2 = P DOP 2 T DOP 2 (2.212) Because the DOP values are a function of the position of the GPS receiver and the satellites these values can be predicted by using an almanac over the GPS satellites. It is worth noting that the DOP values are dimensionless. Distance root mean square (drms) and Circular error probable (CEP) There exists several methods to quantify the horizontal error of a position estimate. Distance root mean square (drms) is one of these, and it is dened as drms = σx 2 σy 2 (2.213) From the denition of drms above combined with the denition of HDOP we get drms = HDOP σ UERE (2.214) Generally speaking σx 2 and σy 2 must not be of equal size. Figure 2.14 shows a 1σ-ellipse to a probability distribution and introduces the parameters σ l and σ s (long and short). If the distribution is close to a circle ( σs σ l 1) the probability that the position from the GPS is within a circle with a radius of drms and has the true position as a center is 0, 63. If the ellipse is outstretched ( σs σ l 0) the probability will be close to 0, 69 Circular error probable (CEP) is another horizontal error measurement. CEP is de- ned to the radius of a circle that contains 50% of the error distribution if the circle is centered on the real location. CEP can be approximated for a two dimensional Gaussian distribution to be CEP 0.59 (σ l σ s ) (2.215) 44

63 Figure 2.14: Relationship between parameters of distribution. If we assume that the expectation value is zero it is possible to approximate CEP as a function of drms. If CEP 0.75drms (2.216) the circle created by the CEP radius will contain between 43% and 54% of the error distribution. It is also possible to express CEP as a function of HDOP and σ UERE as CEP 0.75 HDOP σ UERE (2.217) It is possible to use other probabilities than 50%. CEP95 is follows the same denition as CEP except that the probability is set to 95% instead of 50%. If we express CEP95 as a function of HDOP and σ UERE we get CEP HDOP σ UERE. With some calculation we get the relationship CEP CEP 95. [37] Gyroscope model When modeling a gyroscope there are many error sources. [59] outlines error sources g- insensitive bias (B f ), g-sensitive bias (B gx and B gz ), anisoelastic bias (B axz ), scale-factor error (S x ), cross-coupling (M y and M z ) and zero-mean random bias (n x ). The model is given below ω x = (1 S x )ω x M y ω y M z ω z B f B gx a x B gz a z B axz a y a z n x (2.218) The gyroscopes used in this thesis are probably not accurate enough to scene all these errors. We also wanted to limit the time used to calibrate the sensors, therefore we 45

64 will use a reduced error model. The g-sensitive bias is low, according to the datasheet for ADIS16400 [11], and is therefore ommited. MEMS gyroscopes covered in [59] do not have anisoelastic bias specied in tables specifying typical error factors from MEMS gyroscopes. The ADIS16400 [11] does not specify the anisoelastic bias either, therefore is this error coecient also omitted. This is gives the new error model ω x = (1 S x )ω x M y ω y M z ω z B f n x (2.219) This model can be rewritten to cover a tree dimensional sensor by introducing the following vectors and matrices ω = ω x ω y ω z a = a x a y a z b gy,f = B xf B yf B zf (2.220) and S gy = S x S y S z M gy = 0 M x,y M x,z M y,x 0 M y,z M z,x M z,y 0 (2.221) Then the sensor error model can be written as ω = (I S gy )ω M gy ω b gy,f n gy (2.222) where ω is the measurement and ω is the true value. Calibration In order to calibrate the gyro a set of measurements are needed. Two test methods are going to be used in this thesis, stability tests and rate transfer tests [59]. In the stability tests the gyro is aligned on a surface with the plane axis pointing towards known directions, for instance North and West. When the position of the gyro axis is known one can remove the eects of the rotation of the earth. Then one can nd the Allen Variance and the bias values. If several tests are made with the gyro aligned in dierent positions regarding gravity, the acceleration dependence of the gyro drift can be found. In the rate transfer tests the gyro is placed on a rate board. The rate board can be set to rotate at dierent rates and a series of test is conducted in order to nd the scale factor error. It is also possible to nd cross-coupling coecients with this test by also sampling from the gyros that are experiencing no rotation. To characterize the noise we use the Allan Variance method and the data from the stability tests, see section for Allan Variance method. In order to estimate the parameters b gy,f, S gy and M gy we use least square method [60]. First the parameter b gy,f is estimated from the stability tests and then the parameters S gy and M gy are estimated from the rate transfer tests. The least square method used to t a linear function to a data set is called linear regression in statistics [60]. The error is dened to be the length from the sample to the 46

65 line squared. The problem is to nd the linear function that minimize the sum of the squared errors. This sum is denoted as SSE below. SSE = n (y i a bx i ) 2 (2.223) i=1 Here are a and b the parameters to be found and y i and x i are samples. In our case y i and x i represents the known rotation rate and the measured rotation rate. By dierentiating with respect to a and b we get (SSE) a n = 2 (y i a bx i ) i=1 (SSE) b n = 2 (y i a bx i )x i (2.224) i=1 First we nd the bias a. This is possible because x i is always zero in the stationary tests. By setting (SSE) to zero and solving we get a n i=1 a = y i (2.225) n This is simply the mean of the measurements, which is not an unexpected result. Now that a is known we can solve the other equation. By setting (SSE) to zero we get b n i=1 b = y ix i a n i=1 x i n (2.226) i=1 x2 i We now have the tools needed to estimate the parameters in (2.222). The known values ω always have some zeros, and therefore the equation is reduced to three linear equations on the form y = a bx for the two test methods, and we can use the method described above. When calculation b on a computer it can be benecial to modify the formula to avoid rounding errors in the computer. By dividing on n on top of and below the fraction line we get ( n i=1 y ) ( n ix i i=1 a x ) i n n b = ) (2.227) ( n i=1 x2 i = n ( n i=1 f ) ( 1(x i, y i ) n i=1 a f ) 2(x i, y i ) n n ( n i=1 f ) (2.228) 3(x i, y i ) From the above equation we see that there are three very equal terms. These terms can be calculated by a batch algorithm in order to prevent unnecessary rounding errors. By dening the function n g(x k, y k ) = k i=1 f(x i, y i ) 47 k (2.229)

66 we can nd the next step by k1 i=1 g(x k1, y k1 ) = f(x i1, y i1 ) k 1 = 1 k 1 f(x k1, y k1 ) k k 1 g(x k, y k ) (2.230) The algorithms starts with k = 2 and g(x 1, y 1 ) = f(x 1, y 1 ). This is a generalization of the approach found in [25] Accelerometer model The accelerometer are subject to the following error source according to [59], g-indepented bias (B f ), scale factor error (S x ), cross coupling error (M y adn M z ) and vibro-pendulous error (B v ). The vibro-pendulous error is present when the sensor is subject to vibration along the sensitive and pendulum axes simultaneously. We are unable to calibrate this parameter because of lack of equipment. This reduces the error model to ã x = (1 S x )a x M y a y M z a z B f n x (2.231) where ã x is the measured acceleration in along the x axis, a x, a y and a z are the real acceleration along the x, y and z axis and n x is white noise with a variance σac,x. 2 By dening the vectors and matrices a x a = a y a z B xf b ac,f = B yf B zf (2.232) and S ac = S x S y S z M ac = 0 M x,y M x,z M y,x 0 M y,z M z,x M z,y 0 (2.233) we can create an error model for all three accelerometers on vector form. model is This error ã = (I S ac )a M ac a b ac,f n ac (2.234) where ã is the measurements and a is the true value. Calibration To calibrate to accelerometer we will use multi-position tests [59]. The accelerometer will be rotated with one axis perpendicular to the gravity vector and the two other axis will experience an acceleration between g and g. With this test it is possible to estimate the scale-factor, scale-factor linearity, null bias error and axis alignment error. To estimate these factor we use linear regression [60]. The problem is somewhat dierent from the estimation problem for the gyroscopes. There we had only one axis with changing values. Now we have two axis with changing values. Therefore it is necessary to use multiple linear regression. First we will show how to reduce the problem to a multiple linear regression problem, then we will show how the multiple linear regression is solved. 48

67 When the sensor is rotating around the z-axis and the z-axis is perpendicular to the gravity eld then the length of the vector measured by the x-axis sensor and y-axis sensor should be equal to γ h, which is the gravity force perpendicular to the local level plane found in section The sensor output from the z-axis sensor is assumed to be pointing towards north and should be γ n. The expected eld can be expressed as a = γ h ã2 x ã 2 y ã x ã y γ n (2.235) Here is g the gravity constant, a is the expected measurement and ã is the actual measurement. The reason to express a as a function of ã is that a is unknown. If we plotted the true eld in the xy-plane we would get a circle. With the equation above we will create a circle if sensor is rotated when sampling. The above equation introduces an error because one of the sensor outputs might be higher or lower than they should be. If this is the case then the resulting vector a will have a wrong direction. This error will be small because we sample at all angles. The error model of the x-axis sensor is given in (2.234). This equation, and the error models for the other axis, can be written as y i = b 0 b 1 x 1i b 2 x 2i b 3 x 3i (2.236) Where i represents the ith sample. Using the least square [60] method we get y = SSE = n (y i b 0 b 1 x 1i b 2 x 2i b 3 x 3i ) 2 (2.237) i=1 We can express this using matrix notation by dening the following matrix and vectors y 1 1 x 11 x 21 x 31 y 2 1 x 12 x 22 x 32. y n Now SSE can be written as X =.... b = 1 x 1n x 2n x 3n b 0 b 1 b 2 b 3 (2.238) SSE = (y Xb) T (y Xb) (2.239) By setting (SSE) b = 0 and solving the equation with respect to b we get b = (X T X) 1 X T y = X y (2.240) This equation only holds if the term X T X is nonsingular. The X matrix is often called the pseudoinverse of X [12]. In order to increase the numeric accuracy when computing we can use the same method as in section We dene A to be X T X and g to be X T y. Then we divide both sides by n getting the equation 1 n Ab = 1 n g (2.241) 49

68 where and 1 n A = n n i=1 x 1i 1 n n i=1 x 2i 1 n n i=1 x 3i 1 n 1 n 1 n n i=1 x n 1i n i=1 x n 2i n i=1 x 3i 1 n n i=1 x2 1 n 1i n i=1 x 1 n 1ix 2i n i=1 x 1ix 3i n i=1 x 1 n 2ix 1i n i=1 x2 1 n 2i n i=1 x 2ix 3i n i=1 x 1 n 3ix 1i n i=1 x 1 n 3ix 2i n i=1 x2 3i 1 n g = 1 n n i=1 y i 1 n n i=1 y ix 1i 1 n n i=1 y ix 2i 1 n n i=1 y ix 3i 1 (2.242) (2.243) Each of the matrix elements can be written as in (2.229) and by using (2.230) we get a method for calculation the elements in the matrix. It should be noted that A is a symmetric matrix, and therefore only ten sums needs to be calculated, instead of 16. It is possible to nd the inverse of 1 A by using LU factorization. LU factorization is n explained in [54] Magnetometer model The error model for the magnetometer is similar to the gyroscope and accelerometer case and is postulated to be B = (I S ma )B b ma,f n b (2.244) where B is the measured magnetic eld, S ma is the scale factor error, B is the true magnetic eld, b ma,f is the bias and n b is white noise. Calibration To calibrate the magnetometers we use an improved version of a calibration algorithm found in [10]. In this algorithm the scale factor and bias is dened in another way. B x = X sf Bx X off (2.245) B y = Y sf By Y off (2.246) where B x and B y is the compensated output, Bx and B y is the measured magnetic eld, X sf and Y sf is the scale factor and X off and Y off is the bias. To convert these parameters to the notation used in the error model (2.244) we use the equations S x,ma = X 1 sf 1 (2.247) S y,ma = Y 1 sf 1 (2.248) b x,ma = X 1 sf X off (2.249) b y,ma = Y 1 sf Y off (2.250) To calibrate the sensor we make a set of measurements where two of the axis are in the horizontal plane and rotate around the third axis. We nd the minimum and maximum 50

69 of the two axis in the horizontal plane. In our case we use the x-axis and y-axis, and denote the minimum and maximum values of the two axis as X min, X max, Y min and Y max. To calculate the scale factor and bias we use the equations [10] X sf = max(1, Y max Y min X max X min ) (2.251) Y sf = max(1, X max X min ) (2.252) Y max Y ( min ) Xmax X min X off = X sf X max (2.253) 2 Y off = Y sf ( Ymax Y min 2 Y max ) (2.254) This method does not scale the measured data to the IGRF eld. An improvement to the 2 H method has been made by the author. The factor X max X min has been added to the scale factor equations to scale the measurements to the IGRF model. The new scale factor equations are 2 H X sf = max(1, Y max Y min ) X max X min X max X min (2.255) 2 H Y sf = max(1, X max X min ) Y max Y min Y max Y min (2.256) (2.257) It should be noted that this is a coarse calibration technique and it can be used as a eld calibration algorithm Pressure sensor model The pressure sensor MS5534C measure both pressure and temperature. The datasheet [33] describes an algorithm to compensate for temperature error and also loads factory calibration constants, which is implemented on the DAQ unit. Since the data already has compensation for temperature and with factory calibration constants it is reasonable to assume that the sensor produces small errors. Therefore we postulate a model with only scale-factor error, bias and noise, given below The noise term is assumed to be white noise. p = (1 S ba )p B ba,f n ba (2.258) Calibration To estimate the scale-factor error and bias we use ordinary linear regression. This is very similar to the regression used when estimating the gyro error coecients. In the gyro case we rst found the bias, and then found the scale-factor because we had two dierent data sets. Now we can estimate both with the same dataset. The SSE and the derivation of 51

70 SSE with respect to a and b is the same as in section 2.6.2, but the result is dierent. Now we get b = n i=1 (x i x)(y i ȳ) n i=1 (x i x) 2 a = ȳ b x (2.259) Where ȳ and x is the average of y and x Quality of model t To nd out how well the error model t the data set we introduce the coecient of determination, R 2. [60] states that This quantity is a measure of the proportion of variability explained by the tted model, or a measurement of how good the model explains the dataset. This quantity is always between 0 and 1. To calculate this quantity we need to introduce some new terms. As we have seen the error sum of squares is SSE = n (y i ŷ i ) 2 (2.260) i=1 Here ŷ i represents the model. The total corrected sum of squares represents the variation in the response that would ideally explain the model and is dened as SST = n (y i ȳ i ) 2 (2.261) i=1 Where ȳ i is the average of y i dened by 1 n n i=1 y i. Now R 2 is dened to be R 2 = 1 SSE SST (2.262) As said the coecient of determination is a measurement on how good the model ts the dataset. If R 2 = 1, 0 the models ts perfectly, if the R 2 0 the model ts poorly. But what values of R 2 is acceptable. [60] indicates that R 2 values when calibrating measurement units should be high, perhaps exceed 0,99. When using regression one should avoid over tting, by including to many model terms. The sensor models presented in the previous sections has only constant and linear terms and are based on errors that can occur when creating the sensors. Therefore these models vulnerable to over tting. 52

71 Chapter 3 Data acquisition Data for this project is collected with a data acquisition unit (DAQ unit) created in the preproject for this master thesis [52]. This chapter will present the DAQ unit and software improvements made. 3.1 DAQ unit The DAQ unit collects data from a GPS, gyroscopes, accelerometers, magnetometers and a barometer. The GPS is a UP500 from Fastrax [16]. This unit has an accuracy of 1, 8 m measured in CEP95. The gyroscopes, accelerometers and magnetometers are all inside a ADIS16405 from Analog devices [11]. The gyroscopes has a angel random walk coecient of 2.0 / hr and a bias instability coecient of 0, 007 /s. The accelerometer has a noise density of 8.66 mg and the magnetometer has a noise density of 2, 59 mgauss. The barometer is a MS5534C from Intersema [33]. The datasheet does not specify a noise density. The data is sampled and saved on a SD card. Figure 3.1 shows the DAQ unit. 3.2 Software updates to DAQ unit Unfortunately all the software was not completed in the preproject and additional improvements were needed to be able to use the DAQ unit. All the changes are given in Appendix B. Software for both the ADIS16405 and MS5534C sensors did not work. In the case of the ADIS16405 sensor the error was simply that the SPI parameters was congured wrong. One of the conguration bits in the datasheet for the AVR micro controller (MCU) [2] where given in negative logic and a N before the parameter name to indicate this while the ADIS16405 datasheet used positive logic. The barometer module is written by FFI, but needed some modications to work with this design. First of all the barometer module used a counter in the MCU that was already used by another module. The barometer module used the cycle counter, but was changed to use a timer instead. Second the module sent a message to the barometer to start the sampling and waited until the sampling was completed. This took 33 ms and caused the MCU to loose many samples from the other sensors. By introducing a cooperative multitasking like scheme this was avoided. The read barometer function was 53

72 Figure 3.1: The DAQ unit divided in ve parts and will only do one part before the MCU checks if the other sensors has read data and need to clear the buer. When the MCU saves the samples to the SD card it now creates folders with todays date and time. The date and time i gotten from the GPS. The samples are now saved in dierent les, one le for each sensor. All the NMEA messages from the GPS are saved in a separate le in case they are needed. The le format is described in the user documentation section later in this chapter. All the samples has a counter value associated with it to be able to determine the timing between the sensors. In order to sample longer the SDRAM has been divided dierently. The menu module has been updated. It is now possible to congure the dynamic range of the gyroscopes in the ADIS16405 sensor. The complete menu is found under the user documentation section. The main display has been updated to show heading information calculated from the magnetometers, for debugging purposes when collecting data. The heading calculation algorithm assumes that the device is level with the earth surface, and does not include calibration information. 54

73 3.3 User documentation This is an updated version of the user manual presented in [52], and includes all the changes Menu Below is an account of the entire menu system. One gets into the menu system by pressing any key except the cancel key on the DAQ unit. The left key is used as an enter key and the right key is used as a cancel key. The joystick is used to navigate in the menu system. Sample sensor data Sample sensor data and saves it on a SD card. Section describes the le format. Sample 10 s Collect data for 10 seconds Sample 30 s Collect data for 30 seconds Sample 5 min Collect data for 5 minutes Sample 25 min Collect data for 25 minutes Status Show status information from dierent sensors GPS status Status information from Fastrax UP500 Rec Min Nav info Shows the Recommended minimum navigation information Satellites info Shows how many satellites that are visible and which satellites Precision (DOP) Shows the (Dilution of precision), HDOP, VDOP and PDOP INS status Shows raw register information from the ADIS16405 sensor. See [11] System status Shows the registers DIAG_STAT, SUPPLY_OUT, FLASH_CNT Control register Shows the registers MCS_CTRL, SMPL_PRD, SENS_AVG Alarm magnitude Shows the registers ALM_MAG1, ALM_MAG2 Alarm control Shows the registers ALM_CTRL, ALM_SMPL1, ALM_SMPL2 Gyro o Shows the registers XGYRO_OFF, YGYRO_OFF, ZGYRO_OFF Accl o Shows the registers XACCL_OFF, YACCL_OFF, ZACCL_OFF, Magn hif Shows the registers XMAGN_HIF, YMAGN_HIF, ZMAGN_HIF Magn sif Shows the registers XMAGN_SIF, YMAGN_SIF, ZMAGN_SIF DAQ status Shows Data Acquisition information SD/MMC status Shows the partition type of the SD card, how large the SD card is and how much free space there is on the SD card SDRAM status Shows the size of the SDRAM and perform a write test on the SDRAM and reports how many write errors that are found Sample status Displays last sample date and time Live sensor data Shows sensor data in real time 55

74 GPS position Shows basic navigation information from the GPS. Does not show any information if the GPS does not have enough satellites INS data Show data from the ADIS16405 sensor. All data is in integers Raw gyro data Raw data from the gyroscopes Raw accl data Raw data from the accelerometer Raw magn data Raw data from the magnetometer Raw temp data Raw data from the temperature sensor Barometer data Show the pressure in mbar and sensor temperature in /C Conguration Congure the DAQ device Version Shows the rmware version Backlight Changes the display backlight strength ADIS16405 Sets the dynamic range of the gyroscopes to 75 /s, 150 /s or 300 /s. Default is 75 /s File format When the sampling process is nished the DAQ unit creates ve les containing the data. These les are created in a directory which is named after the date and time of the sampling. The le gps.txt contains the GPS information, the le ins.txt contains all sensor data from the ADIS16405 sensor, the le bar.txt contains sensor data from the barometer, the le nmea.txt contains all the NMEA messages that was sent from the GPS during the sampling process and the le info.txt contains some debugging information and the data in the registers SMPL_PRD and SENS_AVG from the ADIS These registers describes the dynamic range of the gyroscopes and the sampling time. The le gps.txt uses the following syntax <oc>,<cc>,<time>,<lat>,<latd>,<lon>,<lond>,<q>,<alt>,<hdop>,<vdop> oc - overflow count cc - cycle count time - GMT from the GPS lat - latitude latd - latitude direction ('N' or 'S') lon - longitude lond - longitude direction ('E' or 'W') q - quality, 1 for fix, 0 for no fix alt - antenna altitude obove sea level hdop - horizontal dilution of precition vdop - vertical dilution of precition The cycle count is a 16 bit counter that restarts when it overows. The overow counter count all the overows from the cycle counter. The cycle counter runs on 250 khz. The time eld is in the format HHMMSS.DDD, where H is hour, M is minute, S is second and D is the decimal value of the second. The time is in Greenwich Mean Time (GMT). 56

75 The rest of the elds are taken from the NMEA messages GGA and GSA. Please refer to the NMEA standard [7, 53] for explanation on these elds. The le ins.txt uses the following syntax <oc>,<cc>,<xgy>,<xgy>,<xgy>,<xac>,<xac>,<xac>,<xma>,<xma>,<xma>,<temp> oc - overflow count cc - cycle count xgy - raw output from x gyro ygy - raw output from y gyro zgy - raw output from z gyro xac - raw output from x accelerometer yac - raw output from y accelerometer zac - raw output from z accelerometer xma - raw output from x magnetometer yma - raw output from y magnetometer zma - raw output from z magnetometer temp - sensor temperatur The cycle counter and overow counter are the same as in gps.txt. All the raw data from the ADIS16405 sensor are signed integers. Information on how to convert data from the raw output to real data is given in the datasheet [11] The syntax of bar.txt is <oc>,<cc>,<p>,<temp> oc - overflow count cc - cycle count p - pressure in mbar temp - sensor temperatur where cycle counter and overow counter are the same as in gps.txt. The pressure is in mbars and is already compensated for temperature. The pressure measurements is sampled at approximately 11, 6 Hz By using proprietary NMEA messages described in [49] the GPS was congured to send GGA and GSA messages with a frequency of 5 Hz and GSV and RMC with a frequency of 1 Hz. These messages are saved in the le nmea.txt. 3.4 Possibility of implementing navigation algorithm on DAQ unit The Matlab implementation of the navigation system uses approximately 0.03 seconds to 0.06 seconds to calculate one step in the Unscented Kalman lter. The Kalman lter has to make approximately 420 steps to calculate one second of real time navigation information. The Matlab implementations uses approximatly between 12.6 seconds to 25.2 seconds to calculate one second. These measurements are done on a computer with a 2.93 GHz Intel Xeon X5570 processor with four cores and 16 Gb RAM. It is possible that the code can be optimized to decrease the computational load of the navigation 57

76 system. C code compiled to run directly on the microcontroller will run faster than Matlab code. The microcontroller runs at 64 MHz and it is highly unlikely that the microcontroller can run the navigation system in real time. The optimization and the fact that C code runs faster than Matlab code will not be enough to compensate for the decreased computational speed on the microcontroller. Even the server that runs Matlab is unable to estimate the position real time. It might be possible to implement the navigation system in hardware on a FPGA. A FPGA will probably have the computational power to calculate the Unscented Kalman lter real time. 58

77 Chapter 4 Positioning algorithm This chapter will cover the deduction of the process and measurement equations to be used to estimate velocity and position. First we will create a lter that estimates the velocity from GPS measurements only. This lter can be used to initialize other lters with an initial velocity. In the second section we will create the equations needed to use the Unscented Kalman lter to estimate position using the inertial sensors only. This includes selecting an error model for icker noise from the gyroscopes and creating an augmented process equation with the navigation equations and noise states. The initial conditions are also given. In the third section the aid sensors is included by creating measurement update equations for the aid sensors. The fourth section gives an account for implementation diculties and considerations. The last section contains a summary of the equations used with the Kalman lter to estimate position. 4.1 GPS When using the GPS we want to estimate the velocity vector because this is needed when initializing the inertial navigation system. The linear process equation [ v l [k 1] p e [k 1] ] [ = I 0 T R e l (φ, λ) I ] [ v l [k] p e [k] ] [ I 0 ] [ v l v ] (4.1) where v l is the velocity in the local-level frame, p p is the position in the earth frame, T is the sampling interval, R e l (φ, λ) is the rotation matrix given in section The latitude φ and longitude λ is obtained from the GPS and the error on this values are ignored to create a linear system with a time varying process matrix Φ k. v l w is white noise. This noise is introduced because it is unknown how the velocity change. The power spectral density of the noise can be chosen depending on application. If it can be assumed that the user is a pedestrian the standard deviation σ v for the acceleration may be set to 3 m/s 2. This equals a person going from zero to a velocity of 6 m/s in 2 s having a constant acceleration. For a vehicle the value is dierent. Assuming that the vehicle accelerate from zero to 100 km/h in 12 s with a constant acceleration yields a standard deviation σ v of approximately 2.3 m/s 2. Both the pedestrian and the vehicle has limited acceleration upwards. Acceleration upwards comes from moving up or down a hill and with an assumption that the acceleration upwards is ten percent of the acceleration in 59

78 the north-east plane. These values may have other values that yields better performance for lter, but it's beyond the scope of this text to investigate this further. The covariance for the process is then The measurement update is Q v = σ 2 v σ 2 v σ2 v (4.2) z k (φ, λ, h) = p e k w e k (4.3) where z k (φ, λ, h) is the position in the earth frame measured by the GPS. The position is a function of latitude φ, longitude λ and height above sea level h. These values are given by the GPS and it is straight forward to calculate them using (2.44). By using the result of the function as a measurement the measurement equation is linear and H = [0 I]. w e k is white noise with time varying power spectral density. The GPS outputs the covariance in the local frame and therefore the power spectral density must be rotated to the earth frame. The covariance of the measurement noise is R k = R e l (φ, λ)r l GP S(R e l (φ, λ)) T (4.4) where R l GP S is the covariance in the local-level frame. R l GP S can be found by using (2.208) and (2.209) and assuming that the error is equal in the north and east direction 1 HDOP 2 σ R l 2 UERE GP S = 1 0 HDOP 2 σ 2 2 UERE 0 (4.5) 0 0 V DOP 2 σuere 2 To initialize the lter one use the rst measurement from the GPS as the initial position. This means that [ ] 0 x 0 = p e (4.6) (φ 0, λ 0, h 0 ) where φ 0, λ 0 and h 0 is the rst output from the GPS. The velocity is initialized to zero because it is completely unknown. Because the velocity is completely unknown the variance is innity. We will therefore use the information lter to be able to represent innite uncertainty. The initial information matrix is [ ] 0 0 I 0 = 0 R 1 (4.7) 0 where R 0 is the power spectral density of the rst measurement. We have now presented all information needed to use the discrete Information lter presented in section Inertial sensors In this section we will create position estimation algorithms using inertial sensor measurements. 60

79 4.2.1 Selecting noise model for gyroscope In section we modeled 1/f noise in several dierent ways. We are now going to evaluate these methods with respect to variance. We want to have a noise model that generates a variance close to the true variance. The model δω = ω ω (4.8) represents the dierence between the measured angular velocity ω and the true angular velocity ω. We are now only going to evaluate dierent noise models, therefore all other error sources are omitted and the model becomes δω = v w v p (4.9) where v w is the white noise component and v p is the 1/f noise component. Creating an discrete error model of the angle by rst integrating the angular velocity and then creating a discrete model by using Euler's method yields [52] δθ[k 1] = δθ[k] T (v w [k] v p [k]) (4.10) where T is the sampling time and δθ[k] is the one dimensional attitude error. v w is white noise with a power spectral density of Qw and v p is 1/f noise with a power spectral density of Q p /f. This model can be simulated using Monte Carlo methods [52]. We shall now investigate which of the approximations to 1/f noise presented in section that is closest to the true error. We had three approximations to the 1/f noise, Brownian motion, Gauss-Markov process and an AR ltered white noise. By augmenting the model (4.10) with the discrete model for a Brownian motion process we get [ ] [ ] [ ] [ ] [ ] δθ[k 1] 1 T δθ[k] T 0 v1 [k] = (4.11) v p [k 1] 0 1 v p [k] 0 1 v 2 [k] where v 1 [k] and v 2 [k] is two white noise sequences. Another model is obtained by augmenting the model (4.10) with the discrete Gauss-Markov process [ ] [ ] [ ] [ ] [ ] δθ[k 1] 1 T δθ[k] T 0 v1 [k] = v p [k 1] 0 e T β (4.12) v p [k] 0 1 v 2 [k] where v 1 [k] and v 2 [k] is two white noise sequences. The AR lter from can be written as a state space model in the following manner δθ[k 1] T δθ[k] T v w1 [k] v n [k 1] v n [k] 0 v n 1 [k 1] = v n 1 [k] v 1 [k 1] v 1 [k] 0 v 0 [k 1] 0 a n a n 1 a 1 0 v 0 [k] v w2 [k] (4.13) 61

80 where v w1 [k] and v w2 [k] is two white noise sequences, v n,, v 0 is the AR lter. v 0 is the output of the lter, v 1 is the output of the lter for last time step and v n is the output of the lter n steps ago. The model can be simplied to δθ[k 1] v[k 1] v 0 [k 1] = 1 0 T T 0 B b 0 a T 0 δθ[k] v[k] v 0 [k] T v w1 [k] 0 v w2 [k] (4.14) where 0 T = [0 0], v = [v n v 1 ] T, a = [a n a 1 ] T, b = [0 0 1] T and [ ] 0 I B = 0 0 T (4.15) Figure 4.1 shows standard deviation obtained by the three noise approximations presented above and the estimated true standard deviation obtained from Monte Carlo simulations. Equations for the Monte Carlo simulations are given in [25]. The simulations use T = 0, 01, 6000 steps and 1000 simulations. Noise parameters are Q w = N 2 = (2/60) 2 and Q p = B 2 = (0, 007) 2. The values are found in the datasheet for ADIS16405 [11]. The parameters for the three noise approximations are calculated with the equations found in section In addition S max = 10 for the Gauss-Markov process and the AR lter is of 30th order.the standard deviation for the noise models are found using the discrete covariance equations for the linear Kalman lter, given in section 2.4.1, because all the noise models are linear. The Gauss-Markov model seems to have the same asymptotic behavior as the true standard deviation, but is slightly lower. With the Brownian motion model the standard deviation increases faster than the true standard deviation. The AR model also seems to have the same asymptotic behavior as the true standard deviation, but it is higher. The AR lter has a dimension of 32 compared to 2 for the Gauss-Markov process, thereby making the Gauss-Markov process superior with respect to computing complexity. For this reasons and the Gauss-Markov is chosen as the noise model used to model 1/f noise Kalman lter equations for inertial sensors We now want to nd the process and measurement equations for the system with only inertial sensors on the form given in (2.174). The process equation is (2.117). This equation includes both the accelerometer measurements and the gyroscope measurements. Because these are the only measurements and they are used in the process equation the measurement update in the Kalman lter is omitted when using inertial sensors only. The equations for the calibrated output from the accelerometer and gyroscope is on the wrong form to include in process equation. By simple manipulations of (2.234) and (2.222) we get a = (I S ac M ac ) 1 (ã b ac,f n ac ) (4.16) ω = (I S gy M gy ) 1 ( ω b gy,f n gy ) (4.17) where ã is the measured acceleration, a is the calibrated acceleration, ω is the measured angular velocity, ω is the calibrated angular velocity, n ac and n gy is noise. The coecients 62

81 Figure 4.1: The standard deviation of δθ using Q w = (2/60) 2 and Q p = (0, 007) 2, found in the datasheet for ADIS16405 [11]. The solid line is the result obtained by Monte Carlo simulations, the dashed line is a Brownian motion approximation, the dash dotted line is a Gauss-Markov approximation and the dotted line is a 30th order AR lter approximation. 63

82 is described in section 2.6. By scaling the noise we can and dividing the gyro noise into a white noise part and a 1/f noise part we get a = E 1 ac (ã b ac,f ) v ac (4.18) ω = E 1 gy ( ω b gy,f ) v gy,w v gy,p (4.19) where v ac and v gy,w is the scaled white noise, v gy,p is the scaled 1/f noise, E ac = I S ac M ac and E gy = I S gy M gy. When we calibrated the gyroscopes we ignored the earth rotation because this is much smaller than the noise generated by the gyroscopes, therefore we can remove ω l il from the quaternion dierential equation in the process equation. Inserting (4.18) and (4.19) into the process equation (2.117) yields v l [k 1] = p e [k 1] = [k 1] = ( q l b T (R l b(e 1 ac (ã b [k] b b ac,f) v b ac) γ l [k]) (I T ((ω l il [k]) (R l eω e ie[k]) ))v l [k] p e [k] T R e l v l [k] I T ( 1 Ω(E gy ( ω b ib[k]) b b gy,f) Ω(v b gy,w) Ω((v b gy,p) )) q l b [k] (4.20) The rotation matrix R(q l b [k] is denoted as Rl b and R e l (φ[k], λ[k]) is denoted as R e l to increase readability of the equation. Using the Gauss-Markov representation for 1/f increases the state vector by three with the equation v b gy,p[k 1] = e T β v b gy,p[k] v b p,w (4.21) where v b p,w is white noise. The new state vector, the initial expectation value of the new state vector and the new noise vector are x k = v l [k] p e [k] q l b [k] v b p[k] x 0 = v l [0] p e [0] q l b [0] v b p[0] v = v b ac[k] v b gy,w[k] v b p,w[k] (4.22) where p e [0] is obtained from the GPS, v l [0] is set to zero and it is assumed that the DAQ unit is stationary when staring up. In later designs v l [0] could be estimated from the GPS output. q l b [0] is obtained using the QUEST algorithm and vb p[0] = 0. This leads to the initial covariance of the system and the covariance for the noise to be P 0 = P v, P p, P q, Q k = Q ac Q gy,w Q p,w (4.23) where P v,0 is set to zero. If one wants to use v l [0] estimated from the GPS then P v,0 must be estimated from the GPS output. P p,0 is obtained from the GPS using (4.4) and P q,0 is obtained from the QUEST algorithm. In the process noise matrix Q ac is the noise from the accelerometer, Q gy,w is the white noise from the gyroscopes and Q p,w is the 1/f 64

83 noise from the gyroscope. These can be found from Q ac = E 1 ac Q gy = E 1 gy Q p = E 1 gy Q ac,x Q ac,y Q ac,z Q gy,x Q gy,y Q gy,z Q p,x Q p,y Q p,z E T ac (4.24) E T gy (4.25) E T gy (4.26) where Q ac,x, Q ac,y and Q ac,z are the variance of the white noise from accelerometers for each axis, Q gy,x, Q gy,y and Q gy,z equals the angular random walk coecient N squared for each axis divided by the sampling time T and Q gy,x, Q gy,y and Q gy,z equals the bias instability coecient B squared for each axis divided by the sampling time T. These values are estimated in the calibration process. We have now found the necessary functions and coecients to use the Unscented Kalman lter presented in section to process the measurements from the inertial sensors. It should be noted that there are no measurement update equation, only time update part of the lter is used Filtering gyro output To remove som of the icker noise from the gyroscope measurements we want to try to lter the measurements from the gyroscopes. By using Matlab Filter Design & Analysis Tool, using the following parameters F s = 418 Hz (4.27) F c = 0.03 Hz (4.28) (4.29) we created a 10. order IIR Butterworth lter. The lter response is given in gure 4.2. By using the lter we hope to decrease the drift caused by the gyroscopes GPS and inertial sensors It is possible to use the GPS output combined with inertial navigation. By using the time update (4.20) presented in the previous section and the measurement update (4.3) in section 4.1 one gets a system that uses both GPS sensor and inertial sensors. Initializing this lter can be done in two ways. Either by starting the GPS lter and estimating the velocity or assuming that the velocity is zero. By using the GPS lter we can get a fairly good estimate of the velocity, but this might aect the attitude estimation. It is benecial to have the sensors stationary when measuring for the QUEST algorithm. If the sensors are stationary we can average the sensor output over time and thereby decrease noise. This increases the attitude accuracy. 65

84 Figure 4.2: Filter response for the lter used for lter gyroscope measurements 4.3 Aid sensors In this section we will expand the algorithms found in section 4.2 to include the aid sensors, magnetometer and barometer Magnetometer as aid sensor In order to use the magnetometer sensor data we must include a measurement update in the Kalman lter presented in section The approach is found in [50] and uses the quaternion equation (2.27) on the magnetic eld. This yields [ ] [ ] 0 0 B l = p l b B b p l b (4.30) where B l is the magnetic eld in the local-level frame, B b is the magnetic eld in the body frame and p l b is the quaternion that rotates from the body frame to the local-level frame. B l is gotten from the IGRF model and B b is measured by the magnetometer. Multiplying the quaternion p l b on both sides of the equation gives [ ] [ ] = B l p l b p l b B b (4.31) = ( Ω(B l ) Ω(B b ) ) p l b (4.32) We now dene v IGRF to a noise component that models the error between the IGRF model and the real world. The error model for the magnetometer is given in section Inserting the error model for the magnetometer and v IGRF into the above equation yields ( 0 = Ω(B l 1 v IGRF ) Ω(Ema( B ) b b ma,f ) v ma ) p l b (4.33) 66

85 where E ma = I S ma and v ma is the scaled version of n ma. The covariance of v IGRF is σigrf 2 I and the covariance for v ma is E 1 madiag(σ ma ) 2 E T ma Altimeter as aid sensor To use the barometer as an aid sensor we must expand the model found in section by including a measurement update. The method for converting pressure to height above sea level is given in section We will now adapt the equation to convert from pressure to height above sea level to a form usable by the Unscented Kalman lter. The equation to convert from height above sea level to pressure is ( p = P 0 1 h T ) g R T grad grad (4.34) T 0 In equation we measure the pressure p. By rearranging (2.258) we get p = (1 S ba ) 1 ( p B ba n ba ) (4.35) where S ba is the scale factor, B ba is the bias and n ba white noise. Inserting this into (4.34) and renaming n ba to v ba yields The covariance for v ba is σ 2 ba. ( p = (1 S ba )P 0 1 h T ) g R T grad grad B ba v ba (4.36) T Magnetometer and Altimeter as aid sensors It is possible to use both aid sensors. When the Unscented Kalman lter is in the measure update phase we simply select the update equation depending on which sensor data that are available. If only magnetometer data is available then we use (4.33). If only barometer data is available we use (4.36). If both sensors has data available then we use the following update equation [ 0 p ] = Using all sensors ( Ω(B l 1 v IGRF ) Ω(Ema( B ) b b ma,f ) v ma p l b ( ) g (1 S ba )P 0 1 h T grad R T grad T 0 B ba v ba (4.37) It is possible to use the GPS, magnetometer and barometer in the same lter. In the measurement update phase the appropriate measure equations are selected depending on which sensor data that is available. This might be the best way to use the system. When the accurate GPS position updates the lter it will increase the accurate on both the velocity and the attitude. It will also correct the position if the position has deviated far o course. If the GPS for some reason looses too many satellites the lter will continue to calculate the position with the available data. 67

86 4.4 Implementation details In the Unscented Kalman lter algorithm, showed in Algorithm 2.6, one needs to take the square root of the covariance matrix. There are several methods to create factorizations of matrices, Cholesky Factorization, LDL Factorization and Singular Value Decomposition are among some of them [30]. Cholesky Factorization can only be used on positive denite matrices, while LDL Factorization and Singular Value Decomposition can be used on positive semi-denite matrices. Algorithm 4.1 shows how we calculate the square root of a matrix in the Unscented Kalman lter implemented in this thesis. First we check if the matrix is positive denite. If it is positive denite it is possible to use the square root of matrix algorithm in Matlab (sqrtm). When taking the square root it is possible to get complex numbers. We do not want to have complex numbers in the system states or the covariance matrices, therefor we use the Cholesky Factorization algorithm when this happens. The chol function is the Cholesky Factorization algorithm which returns a matrix C when given a matrix A. This matrix has the property that CC T = A, which means that C is the square root of A. If the matrix is not positive denite the LDL Factorization is used. The ldl function is the LDL Factorization algorithm which returns the matrices L and D such that A = LDL T. D is a diagonal matrix and the square root of a diagonal matrix is simply the square root of all the diagonal elements. If none of the diagonal elements are zero then the square root of D will always be real. If one or more of the diagonal elements of D are negative we need to use Singular Value Decomposition. The function svd is the Singular Value Decomposition algorithm. This function returns U, S and V where S is a diagonal matrix with nonnegative elements. The matrices has the property USV T = A. Singular Value Decomposition will always create the square root of A. Algorithm 4.1 Square root of matrix Input: A where A is symmetric Output: B = A if A is positive denite then X = sqrtm(a) if X has one or more complex numbers then B = chol(a) else B = X end if else [L, D] = ldl(a) if λ 1 < 0 where λ 1 is the smallest eigenvalue of D then [U, S, V ] = svd(a) B = U S else B = L D end if end if Algorithm 4.1 and Singular Value Decomposition shall be compared in section 5.2.2, 68

87 Table 4.1: Process equations Name [ ] [ f(x k, v k ] ) [ v f l [k 1] I 0 v GP S p e = l [k] [k 1] T R e l (φ, λ) I p e [k] ] [ I 0 ] [ v l v vl l [k 1] = T (Rb(E 1 ac (ã b [k] b b ac,f) v b ac) γ l [k]) (I T ((ω f l il [k]) (R l eω e ie[k]) ))v l [k] INS,f p e [k 1] = p e [k] T R e l v l [k] [k 1] = ( I T ( 1 Ω(E gy ( ω b ib[k]) b b gy,f) Ω(v b gy,w) Ω((v b gy,p) )) q l b [k] q l b ] f INS v l [k 1] = p e [k 1] = q l b [k 1] = ( v b gy,p[k 1] = T (R l b(e 1 ac (ã b [k] b b ac,f) v b ac) γ l [k]) (I T ((ω l il [k]) (R l eω e ie[k]) ))v l [k] p e [k] T R e l v l [k] I T ( 1 Ω(E gy ( ω b ib[k]) b b gy,f) Ω(v b gy,w) Ω((v b gy,p) )) q l b [k] e T β v b gy,p[k] v b p,w to see if there are any benets from using Algorithm 4.1. In section on page 9 two methods for converting from Cartesian coordinates to ellipsoidal coordinates, one iterative and one closed-loop method. The closed-loop method was tested by converting a GPS measurements to Cartesian coordinates and back to ellipsoidal coordinates. This created dierent height than in the GPS measurements by over 10 meters, which is unacceptable for this application. Therefore the iterative method is used in the implementation of the navigation system. The accuracy in height is set to 10 5 meters. The QUEST algorithm shown in Algorithm 2.4 did not return a correct quaternion even if the vectors given to the algorithm did not have noise. There might be an error in the implementation of the algorithm that causes the quaternion to be calculated. This problem was solved by nding the eigenvalue and eigenvectors of the matrix K. The eigenvector corresponding to the largest eigenvalue is the optimal quaternion, as shown in section The quaternion are normalized after every iteration of the Kalman lter to prevent the quaternion from having a length not equal to Summary In this chapter we have presented several lters. An overview over the process equations are given in table 4.1, measurement equations in table 4.2 and the noise covariance in table 4.3. The diag( ) function transforms a vector into a matrix by placing the vector on the diagonal of the matrix. 69

88 Table 4.2: Measurement update equations Name h(x k, v k ) z k h GP S p e v e GP S p e (φ, λ, h) ( h MAG Ω(B l 1 v IGRF ) Ω(Ema( ) b b ma,f ) v ma p l b 0 ( ) g h BAR (1 S ba )P 0 1 h T grad R T grad T 0 B ba v ba p Name v e GP S Table 4.3: Noise covariance Covariance R e l (φ, λ)r l GP S(R e l (φ, λ)) T v l v diag(σ 2 v, σ 2 v, 1 10 σ2 v) v ac v gy,w E 1 ac diag(σ ac ) 2 E T ac E 1 gy diag(n x, N y, N z ) 2 E T gy /T v p,w 1 2 e 2T β )E 1 gy diag(b x, B y, B z ) 2 E T gy v ma E 1 madiag(σ ma ) 2 E T ma σ IGRF I v IGRF v ba σ 2 ba 70

89 Chapter 5 Results This chapter presents the results of this thesis. First the calibration results are presented. Then results obtained by simulations. At last results from real world tests are presented. 5.1 Calibration This section describes calibration of the sensors on the DAQ unit. The sensor has been calibrated to increase the accuracy of the position estimates. Some sensors did not have noise parameters in their datasheet. These parameters are also estimated in the following section. The programing language C is used to estimate the error model coecients. The code is given in Appendix C. C was chosen because it had fast IO rutines, and the calibration process needed to read a lot of data from le. It is time consuming to write C code and it is likely that it would have been faster to implement and run the calibration algorithms in Matlab. All methods used to calibrate the sensors are given in section Gyroscope The calibration of the gyroscopes is divided into two parts, the rst is estimating the noise parameters using the Allan variance and estimating sensor bias. The second part is estimating scale factor error and cross coupling. To estimate the Allan variance we use three datasets. The datasets where obtained by placing the DAQ unit on the rate board without turning on the power on the rate board. The rate board was provided by FFI. The reason we use three datasets is to get all the axes to experience the gravity eld. In dataset 0 the x axis pointed upwards, the y axis pointed east and the z axis pointed north. In dataset 1 the x axis pointed west, the y axis pointed upwards and the z axis pointed north. In dataset 1 the x axis pointed north, the y axis pointed west and the z axis pointed upwards. At rst the dynamic range of the gyroscopes was set to 75 /s. The Allan variance from dataset 0 is shown in gure 5.1. This Allan variance is not on the usual form found in gure 2.4 in section The datasheet for ADIS16405 says that one have to use a 16 bit Bartlett window FIR lter in order to set the dynamic range to 75 /s. This lter make the Allan variance become lower than it actually is for low averaging times. The hardware in the ADIS16405 prohibits to set the dynamic range to lower than 300 /s without the usage of a lter. This was tested on the device and found 71

90 Figure 5.1: Allan variance for ADIS16405 gyroscopes with dynamic range set to 75 /s to be true. Therefore the dynamic range was set to 300 /s to avoid ltering the sensor output. The result for the three datasets for the x axis sensor is found in gure 5.2 along with the Allan variance given in the datasheet. The dataset for the y axis is shown in gure 5.3 and the z axis in gure 5.4. The gures shows that the noise is signicantly lower than given in the datasheet. All of the datasets has very equal 1 slopes in the right of the 2 graphs, which represents the angle random walk. When the graphs moves towards the bias instability section the uncertainties becomes larger. The Allan variance parameters are estimated using the method described in section The result is presented in table 5.1. As seen in the table the parameters are lower than in the datasheet. The coecient of determination is close to 1 on the x axis estimation. This indicates that the estimation is very accurate. For the other two axis the coecients of determination is lower. The reason for this is that for these two axis the deviation between the datasets are larger in the bias instability region. The Allan variance using the estimated parameters are plotted in gure 5.5 along with the Allan variance from the datasheet. The bias was also estimated using the three datasets with the method presented in section The result is presented in table 5.2. The bias is not large, but the variance on the bias is quite large which indicate that the estimated bias has a large uncertainty. This is not unexpected because the gyroscopes experience bias drift. The second set of data measures the rotation of the DAQ unit. All axis has been rotated from 70 /s to -70 /s with a step of 10 /s between each measurement. Each measurement is of ve minutes length. Zeros angular velocity has been omitted. This results in 14 data sets for each axis. The result is presented in table 5.2. The scale factor error is around 2 % for x and z axis and 1 % for the y axis. The coecient of 72

91 Figure 5.2: Allan variance for ADIS16405 x-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets. Figure 5.3: Allan variance for ADIS16405 y-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets. 73

92 Figure 5.4: Allan variance for ADIS16405 z-axis gyroscope with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from three datasets. Figure 5.5: Allan variance for ADIS16405 gyroscopes with dynamic range set to 300 /s. The solid line is the Allan variance given in the datasheet, and the three other lines are from the estimated Allan variance parameters. 74

93 Table 5.1: Noise parameters for ADIS16405 gyroscopes Angular Random Walk (N) Bias Instability (B) Coecient of determination Datasheet / s /s X gyro / s /s Y gyro / s /s Z gyro / s /s Table 5.2: Calibration parameters for ADIS16405 gyroscopes. Note that M gy = [m gy,x m gy,y m gy,z ] T Parameter X axis Y axis Z axis Bias (b gy,f ) in /s Variance of bias in ( /s) Scale factor error (S gy ) Coecient of determination for S gy Cross coupling with x axis (m gy,x ) Cross coupling with y axis (m gy,y ) Cross coupling with z axis (m gy,z ) Coecient of determination for m gy,x Coecient of determination for m gy,y Coecient of determination for m gy,z

94 Table 5.3: Calibration parameters for ADIS16405 accelerometers. Note that M ac = [m ac,x m ac,y m ac,z ] T. Parameter X axis Y axis Z axis Bias (b ac,f ) in m/s Standard deviation of bias (σ ac ) in m/s Scale factor error (S ac ) Cross coupling with x axis (m ac,x ) Cross coupling with y axis (m ac,y ) Cross coupling with z axis (m ac,z ) Coecient of determination for S ac and M ac determination is very close to 1 for all the axes indicating that the result is very accurate. The scale factor error is not given in the datasheet other than the nonlinearity, which is 0,1 %. The scale factor error found is much higher that the nonlinearity in the datasheet. The cross coupling between the x and y axes is quite large. This might come from errors when mounting the DAQ unit on the xture that is mounted on the rate board. The coecient of determination for this parameter is relatively large compared to the other cross coupling parameters, but low compared to the scale factor Accelerometer To calibrate the accelerometers the rate board was tilted so that the rate board rotated in the gravity eld and the normal to the rate board pointed north. In this way we can use the gravity model from section and the calibration method described in There was collected three data sets on 25 minutes each. The rate board rotated with a velocity of 0.6 /s in order to rotate approximately three times for each data set. In data set 1 the xy plane experienced gravity while the z axis pointed north. In data set 2 it was the xz plane that was in gravity and in data set 3 it was the yz plane. In data set 2 y pointed north and in data set 3 x pointed north. The result of the calibration is shown in table 5.3. There is a small bias and the standard deviation is small, which indicates that the estimated bias is close to the true bias. The scale factor error is under 2 % for the x and y axis and a approximately 2.2 % for the z axis. The datasheet does not specify any scale factor error other than nonlinearity, which is at 0.1 %. The cross coupling error relatively small and could come from misalignment between the DAQ unit and the xture that the DAQ unit is mounted on. The coecient of determination is very close to 1 which indicates that the results represents a good model for the accelerometer error. The standard deviation of the bias can be considered to be the standard deviation of the noise model. The datasheet species the noise standard deviation to be m/s 2. As seen in the table the estimated noise is much lesser than the noise specied in the datasheet. 76

95 Table 5.4: Calibration parameters ADIS16405 magnetometer Parameter XY XZ YZ Mean XY 2 Bias (B ma,f ) in mgauss X axis Y axis Z axis Scale factor error (S ma ) X axis Y axis Z axis Magnetometer The magnetometer was calibrated using the method described in section The magnetometer was placed on the rate board with the power o, to prevent unwanted magnetic elds from the motor that rotates the board. Motors usually contains large amount of iron and therefore will this calibration have large errors, because we are unable to compensate for the magnetic disturbance from the iron. Ideally we should have calibrated the magnetometer with equipment that can neutralize all magnetic elds and create a known eld which can be measured by the magnetometer. This equipment is not available to us, and we therefore use the rate board to have a stable rotating board which is tangent to the earth surface. This will at least give us an indication on how well the magnetometer works. The magnetometer was rotated manually on the rate board while sampling. We sampled three data sets in which the xy, xz and yz plane was in the earth's tangent plane. The estimated bias and scale factor is presented in table 5.4. No other parameters is estimated because of the large uncertainties in the measurements. The IGRF model has a horizontal component H of approximately mgauss. The bias error is larger than the eld in both the x and y axis. The z axis has an error of approximately 28 % of the total eld. All this values are alarmingly high. The scale factor error is no better with an error between 28 % and 57 % on the three axis. It is possible that the rate board creates a eld that makes the values deviate from their true value. Additional calibration of the magnetometer should be conducted if possible. Figure 5.6 shows how the parameters aect the magnetometer data. Because the errors in the XY plane was much larger than the other measurements a second calibration measurement was made. This is also shown in Figure 5.4, and is labeled XY 2. The result from the second calibration is very similar to the rst calibration. This indicates that the magnetic disturbance was larger when calibrating the XY plane. The DAQ unit was further away from the rate board when measuring in the XZ and YZ plane because of the xture used to mount the DAQ unit on the rate board Barometer The barometer was calibrated by using a vacuum tank. The barometer was tested in 800, 700 and 600 mbar, and the sensor parameters was found using the method described 77

96 Figure 5.6: The three datasets for the magnetometer and the same data sets with the estimated parameters. Table 5.5: Calibration parameters MC5534C barometer Parameter Bias (B ba,f ) mbar Standard deviation of bias (σ ba ) mbar Scale factor error (S ba ) Coecient of determination in Section All the data was collected in one long sample sequence and when the pressure changed there was a transient period when the vacuum pump tried to stabilize the pressure. These transient periods were removed from the data set. The result is shown in Figure 5.7 and Table 5.5. There is a small bias. Because of the low value of the standard deviation on the bias it is likely that the bias is close to the true bias of the sensor. The scale factor error is a little large. The sensor is compensated for temperature in the DAQ unit. It is not veried that this compensation is accurate, because it is likely that the sensor is accurate enough for our application. The coecient of determination is close to 1 which indicates the the model ts closely to the real data. 78

97 Figure 5.7: Calibration data for the barometer 5.2 Monte Carlo simulations A number of Monte Carlo simulations has been done. First we quantify the error sources in the inertial system. Then we verify that the UKF is correctly implemented. At last we compare the dierent sensors congurations and how the ltering of the gyroscope output aects the position accuracy. When referring to the standard deviation of the position, velocity or quaternions we use the covariance matrix for these states and nds the eigenvalues of the covariance matrix. Then we use the following formula to nt the standard deviation σ = λ 1... λ n (5.1) where λ 1,..., λ n is the eigenvalues of the covariance matrix and σ is the standard deviation [24]. The Matlab code used to simulate the system is found in Appendix D Quantifying the error sources We will now quantify the error sources in the inertial system without any aid sensors. The process equation (2.104) is simulated Using Monte Carlo simulations [25]. The individual noise sources are uncertainty in the initial position (P p,0 ), noise errors from the sensors used in the QUEST algorithm (P q,0 ), noise from the accelerometer (Q ac ) and noise from the gyroscope (Q gy ). Figure 5.8 shows the standard deviation of the position error. The simulations are of a stationary system. The system has a standard deviation of the position error of approximately 65 m after 60 seconds. The main error source is the noise 79

98 Figure 5.8: The standard deviation of the position error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the noise found in the sensor calibration. from the gyroscopes. The second largest error is the error from calculation of the initial attitude. Figure 5.9 shows the standard deviation in the velocity. The error main error sources are the same as in the position gure. As the noise from the gyroscope is the main error source we want to nd out how much the position error is reduced by using another sensor. Figure 5.10 shows the same simulation as in Figure 5.8, but the Angle Random Walk and Bias Instability coecients has been reduced by one tenth. In this simulation the standard deviation for the position error has been reduced to 13 m after 60 seconds and the error in calculation of the initial attitude is the main error source the rst 60 seconds. Figure 5.11 shows the standard deviation for the velocity with reduced noise from the gyroscopes. The error from calculation the initial attitude is still the main error source, but it seems that the error from gyroscope noise increase faster and will eventually become the main error source Verifying the UKF To verify that the UKF implementation is working we have simulated both the navigation equation f INS and f INS,f and the UKF using these equations as the process equation. The sensor output has been generated and noise added to the sensor output. The simulations are of a stationary system. Both Algorithm 4.1 and the Singular Value Decomposition has been used to nd the square root of the covariance matrix. Figure 5.12 shows the standard deviation of the dierence between the velocity calculated using 80

99 Figure 5.9: Standard deviation of the velocity error by dierent noise sources. The model (2.104) has been simulated 100 times using the noise found in the sensor calibration. Figure 5.10: The standard deviation of the position error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the one tenth of the noise found in the sensor calibration. 81

100 Figure 5.11: The standard deviation of the velocity error caused by dierent noise sources. The model (2.104) has been simulated 100 times using the one tenth of the noise found in the sensor calibration. the process equations and UKF, over 50 runs. Ideally this should be zero, but as seen on the gure the error increases to between 0.1 m/s to 0.2 m/s the rst second. The error over time will be signicant. If one assumes that the error do not increase after the rst second, which is underestimated assumption, the position error will be 13 m after 60 seconds. The Singular Value Decomposition algorithm has the lowest error. Figure 5.13 shows the standard deviation of the dierence between the position calculated from the navigation equations and the UKF. The standard deviation is between 0.08 m and 0.1 m and will probably grow exponentially. Also here the Singular Value Decomposition algorithm has the lowest error. Figure 5.14 shows the standard deviation of the dierence between quaternions calculated by the navigation equations and the UKF. The dierence is zero except for the UKF using f INS,f as process equation, which has a very low error. The UKF does not calculate the mean of the states well. The navigation equations are nonlinear functions and this could be the reason why the UKF does not calculate the mean correctly. A higher order Runge Kutta method could be used to create discrete navigation equations from the continuous navigation equations. It might improve the results. Dierent factorization methods yields dierent results from the UKF. Two dierent factorizations methods has been tried, but both give an error. Therefore it is unlikely that the choice of factorization method is the error source. Singular Value Decomposition will be used in the UKF in the rest of this thesis, since this method had the smallest error. We will bear in mind that the UKF contributes to some of the error in the results. The QUEST algorithm sometimes produces a covariance matrix with negative deter- 82

101 Figure 5.12: The standard deviation of the dierence between the velocity from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix. Figure 5.13: The standard deviation of the dierence between the position from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix. 83

102 Figure 5.14: The standard deviation of the dierence between the position from simulation of the navigation equations and the UKF. msqrt uses Algorithm 4.1 and svd uses Singular Value Decomposition to nd the square root of the covariance matrix. minant. The covariance matrix should never have a negative determinant, because it should always be positive denite or positive semi-denite. This error could aect the UKF and should be looked into Simulation of lter covariance As seen in chapter 4 we want to test four dierent sensor congurations. One with inertial sensors only, another with inertial sensors and barometer, another with inertial sensors and magnetometer and the last with inertial sensors, barometer and magnetometer. In addition to the dierent sensors congurations we would like to test if ltering the gyroscope output might give an increased performance. Figure 5.15 shows a Monte Carlo simulation of ve runs with the standard deviation of the position error. The position error is the dierence between the position given by the UKF and the real position. The standard deviation when only using inertial sensors are close to the results found in section 5.2.1, where we only simulated the navigation equations and not the UKF. Additional sensor decrease the standard deviation signicantly. When using both barometer and magnetometer as aid sensors the standard deviation is decreased to 20 m after 60 seconds. I should be emphesized that this result has a large uncertainty because only ve runs have been made in the Monte Carlo simulation. Figure 5.16 shows the same as Figure 5.15 except that the gyroscope output has been ltered. This gives an unexpected high position error. The conguration that uses inertial sensors and a barometer has the lowest error. This is unexpected because having more 84

103 Figure 5.15: The standard deviation of the position error for a selection of dierent sensor congurations. sensors should increase the accuracy, which is the case in Figure The large error could come from the error in the UKF discovered in last section or it could be that the lter increases the error or a combination. Figure 5.17 shows the standard deviation of the velocity error from the same Monte Carlo simulation as Figure It seems like the usage of a magnetometer update stabilizes the system, causing the standard deviation of the velocity error to be sub linear. Figure 5.18 shows the standard deviation of the velocity error using ltered gyroscope output. This shows large errors for alle the sensor congurations. 5.3 Real world tests There where conducted a number of real world test. Three test where the DAQ unit was stationary, two test pedestrian tests, where a person was walking with the DAQ unit, and two test vehicle tests, where the DAQ unit was in a moving car. Because the results from the pedestrian test and the vehicle test where very poor these results will only be covered briey. All test data is given in appendix E. The Matlab code used to estimate the position is found in Appendix D Stationary test on rate board First we will take a look at the stationary test where the DAQ unit was mounted on the rate board used to calibrate the sensors. Figure 5.19 and 5.20 shows the position 85

104 Figure 5.16: The standard deviation of the position error for a selection of dierent sensor congurations. Figure 5.17: The standard deviation of the velocity error for a selection of dierent sensor congurations. 86

105 Figure 5.18: The standard deviation of the velocity error for a selection of dierent sensor congurations. calculated by the Kalman lter using dierent process and measurement update equations for the rst 15 seconds. The equations are given in table 4.1 and 4.2. The gures shows that all lters drift away from the start position. From these gures it looks like the barometer limits the drift. We shall look at the total position error shortly, but rst we will look at the altitude deviation. Figure 5.21 and 5.22 shows the altitude deviation from the start position. From the gures we see that the barometer update does not aect the altitude calculation much. This is strange because the barometer measures altitude above sea level directly. It looks like the magnetometer limits the altitude drift better than the barometer. Figure 5.23 shows the pressure measured by the barometer. It is very stable and can not explain why the barometer update do not adjust the altitude. Figure 5.24 and 5.25 shows the total position error. The total position error is in this case the length of the vector that starts in the initial position and ends in the calculated position. The lter which uses ltered gyroscope measurements and a barometer as aid sensor has very low error until around 25 seconds have past, then it starts to increase rapidly. All lters that have a magnetometer as an aid sensor has equal or lower error than the lter with ltered gyroscope measurements and a barometer as aid sensor. The lters without aid sensors has as expected the lowest performance. The lter with the best performance in this test is the lter with ordinary gyroscope measurements and with both a barometer and a magnetometer as aid sensors. This is also an expected result. It is hard to say whether the unltered or the ltered gyroscope measurements are best, as the lter that uses unltered gyroscope measurements in some cases perform better then the ltered gyroscope measurements and sometimes worse. It is important to note that 87

106 Figure 5.19: Calculated position for the rst 15 seconds using the data set STA-IN Figure 5.20: Calculated position for the rst 15 seconds using the data set STA-IN 88

107 Figure 5.21: Calculated altitude using the data set STA-IN Figure 5.22: Calculated altitude using the data set STA-IN 89

108 Figure 5.23: Pressure measured from the barometer in data set STA-IN we can not draw any conclusions on this test only, by the test serves as an indication on the performance of the lters. Figure 5.26 and 5.27 shows the length of the velocity vector. As this is a stationary test the velocity is an indication of how fast the position drifts from the initial position. The comments made in the previous paragraph also holds for these gures. The lter with both magnetometer and barometer as aid sensors and that uses unltered gyroscope measurements has the lowest velocity and will therefore has the lowest increase in position error. It looks like the lters that use magnetometer updates has a more linear increase in velocity than the other lters. Figure 5.28 and 5.29 shows the standard deviation of the position calculated by the Kalman lter. The standard deviation is calculated as the square root of the sum of eigenvalues of the position covariance matrix. The lters without aid sensors has a very large standard deviation. With a barometer as an aid sensor the error is between 80 m and 120 m after 70 seconds. This is much smaller than the real deviation, which is between 1000 m and 2000 m. The lters that have magnetometer updates have an arti- cially low standard deviation. The two lters with both barometer and magnetometer updates has a covariance that decreases right after the lter starts and after some time it starts to increase. This is unlikely to be true, because the position from the GPS is the only measurement related to the earth. All the other measurements are relative to this position. Figure 5.30 and 5.31 shows the standard deviation of the velocity calculated by the Kalman lter. As for the position, the uncertainty for the velocity in the lters without aid sensors are vary large. The lters that use barometer as the sole aid sensor has signicantly lower standard deviation. In the case where the lter uses magnetometer 90