Institutionen För Systemteknik Department Of Electrical Engineering Applications

Transcript

Institutionen för systemteknik Department of Electrical Engineering Examensarbete Sensor Fusion Navigation for Sounding Rocket Applications Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Mattias Nilsson och Rikard Vinkvist LiTH-ISY-EX- -08/4009- -SE Linköping 2008 Department of Electrical Engineering Linköpings universitet SE-581 83 Linköping, Sweden Linköpings tekniska högskola Linköpings universitet 581 83 Linköping Sensor Fusion Navigation for Sounding Rocket Applications Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Mattias Nilsson och Rikard Vinkvist LiTH-ISY-EX- -08/4009- -SE Handledare: Johanna Wallén isy, Linköpings universitet Anders Helmersson Saab Space Examinator: Fredrik Gustafsson isy, Linköpings universitet Linköping, 5 October, 2008 Avdelning, Institution Division, Department Datum Date Division of Automatic Control Department of Electrical Engineering Linköpings universitet SE-581 83 Linköping, Sweden Språk Language Rapporttyp Report category ISBN  Svenska/Swedish  Licentiatavhandling ISRN  Engelska/English    Examensarbete  C-uppsats  D-uppsats   Övrig rapport 2008-10-05 — LiTH-ISY-EX- -08/4009- -SE Serietitel och serienummer ISSN Title of series, numbering —  URL för elektronisk version http://www.control.isy.liu.se http://www.ep.liu.se Titel Title Navigering med sensorfusion i en sondraket Sensor Fusion Navigation for Sounding Rocket Applications Författare Mattias Nilsson och Rikard Vinkvist Author Sammanfattning Abstract One of Saab Space’s products is the S19 guidance system for sounding rockets. Today this system is based on an inertial navigation system that blindly calculates the position of the rocket by integrating sensor readings with unknown bias. The purpose of this thesis is to integrate a Global Positioning System (GPS) receiver into the guidance system to increase precision and robustness. There are mainly two problems involved in this integration. One is to integrate the GPS with sensor fusion into the existing guidance system. The seconds is to get the GPS satellite tracking to work under extremely high dynamics. The first of the two problems is solved by using an Extended Kalman filter (EKF) with two different linearizations. One of them is uses Euler angles and the other is done with quaternions. The integration technique implemented in this thesis is a loose integration between the GPS receiver and the inertial navigation system. The main task of the EKF is to estimate the bias of the inertial navigation system sensors and correct it to eliminate drift in the position. The solution is verified by computing the position of a car using a GPS and an inertial measurement unit. Different solutions to the GPS tracking problem are proposed in a pre-study. Nyckelord Keywords GPS, INS, EKF, PLL, FLL Abstract One of Saab Space’s products is the S19 guidance system for sounding rockets. Today this system is based on an inertial navigation system that blindly calculates the position of the rocket by integrating sensor readings with unknown bias. The purpose of this thesis is to integrate a Global Positioning System (GPS) receiver into the guidance system to increase precision and robustness. There are mainly two problems involved in this integration. One is to integrate the GPS with sensor fusion into the existing guidance system. The seconds is to get the GPS satellite tracking to work under extremely high dynamics. The first of the two problems is solved by using an Extended Kalman filter (EKF) with two different linearizations. One of them is uses Euler angles and the other is done with quaternions. The integration technique implemented in this thesis is a loose integration between the GPS receiver and the inertial navigation system. The main task of the EKF is to estimate the bias of the inertial navigation system sensors and correct it to eliminate drift in the position. The solution is verified by computing the position of a car using a GPS and an inertial measurement unit. Different solutions to the GPS tracking problem are proposed in a pre-study. Sammanfattning En av Saab Space produkter är navigationssystemet S19 som styr sondraketer. Fram till idag har systemet varit baserat på ett tröghetsnavigeringssystem som blint räknar ut position genom att integrera tröghetsnavigerinssystemets sensorer med okända biaser. Syftet med detta exjobb är att integrera en GPS med tröghetsnavigeringsystemet för att öka robusthet och precision. Det kan i huvudsak delas upp i två problem; att integrera en GPS-mottagare med det befintliga navigationsystemet med användning utav sensorfusion, och att få satellitföljningen att fungera under extremt höga dynamiska förhållanden. Det första av de två problemen löses genom ett Extended Kalman filter (EKF) med två olika linjäriseringar. Den första linjäriseringen är med Eulervinklar och är välbeprövad. Den andra är med kvaternioner. Integrationstekniken som implementeras i detta Examensarbete är en lös integration mellan GPS-mottagaren och tröghetsnavigeringssystemet. Huvudsyftet med EKF:en är att estimera bias i tröghetsnavigeringsystemets sensorer och korrigera dem för att eliminera drifter i position. Lösningen verifieras genom att räkna ut positionen för en bil med GPS och en inertiell mätenhet. Olika lösningar till satellitföljningen föreslås i en förstudie. v Acknowledgments First we would like to thank Anders Helmersson at Saab Space for his help and support during our work. We would also like to thank our supervisor Johanna Wallén and our examiner Fredrik Gustafsson at the department of Electrical Engineering. Last but not least we would like to thank FOI and Fredrik Eklöf for lending us some valuable equipment critical to our work. vii Contents 1 Introduction 1.1 Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Problems and Methods . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Previous Research . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1 2 2 2 Inertial Navigation 2.1 Coordinate Frames . . . . 2.2 Inertial Measurement . . . 2.3 Choosing Reference Frame 2.4 Strap Down Navigation . . . . . 5 5 6 8 8 3 Global Positioning System 3.1 Brief History . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Calculating the Position . . . . . . . . . . . . . . . . . . . . . . . . 3.3 The GPS Signal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 9 10 11 4 GPS Receiver 4.1 The Design . . . . . . . 4.2 Carrier Tracking . . . . 4.3 Code Tracking . . . . . 4.4 Loop Filters . . . . . . . 4.5 GPS Receiver Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 15 16 20 20 21 5 GPS INS Integration 5.1 Loose Integration . . . . . . . 5.2 Classic Tight Integration . . . 5.3 Tight Alternative Integration 5.4 Deep Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 25 26 26 26 . . . . . . . . . . . . . . . . . . Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 29 30 32 34 36 . . . . . . . . . . 6 Navigation Equations 6.1 Notation . . . . . . . . . . . . . . . . 6.2 Deriving the Navigation Equations . 6.3 Linearization with Quaternion Angle 6.4 Linearization with Euler Angles . . . 6.5 Measurement Equation . . . . . . . . ix x 7 The 7.1 7.2 7.3 7.4 7.5 Contents Extended Kalman Filter Discretization of a Linear System . . Discretization of a Nonlinear System Extended Kalman Equations . . . . The Measurement Update . . . . . . Implementation . . . . . . . . . . . . . . . . . 39 39 40 41 42 43 8 Hardware Used for Testing and Verification of the System 8.1 Proposed Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 A Complete Integration Test of the System . . . . . . . . . . . . . 47 47 49 9 Results 9.1 Test Procedure . . . . . . . . . . 9.2 Filter Tuning . . . . . . . . . . . 9.3 Estimating Position . . . . . . . 9.4 Attitude Estimation Performance 9.5 Velocity Estimation Performance 9.6 Bias Estimation Performance . . 9.7 GPS interruption . . . . . . . . . . . . . . . . 51 51 52 53 53 54 54 54 10 Conclusions and Future Work 10.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 65 66 Bibliography 69 A Quaternions A.1 Quaternion operations . . . . . . . . . . . . . . . . . . . . . . . . . A.2 Quaternion Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . A.3 Quaternion to Rotation Matrix . . . . . . . . . . . . . . . . . . . . 71 71 72 72 B Euler Angles B.1 Euler Angle Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . B.2 Euler Angle to Quaternion conversion . . . . . . . . . . . . . . . . 73 74 74 C Abbrevations 75 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Chapter 1 Introduction Saab Space has been developing and producing systems for several differentiated space programs since its start in 1983. One of these systems is the S19 guidance system, which purpose is to guide the sounding rockets during the acceleration phase. A sounding rocket is an instrument-carrying research rocket designed to take measurements and perform scientific experiments during its sub-orbital flight [26]. This guidance system has been used with success in more than 200 NASA launches and is also used in the launching of the MAXUS1 rockets in Esrange, Kiruna, Sweden. The S19 family of guidance systems have proved to be reliable systems, which keep on evolving with new and exciting capabilities. An important part of these systems is to track the sounding rockets and calculate the flight course, thus make sure that the rockets will land in an isolated and restricted area. To track these rockets an Inertial Navigation System (INS), made up of several Inertial Measurement Units (IMU), has been used to measure the acceleration and calculate the velocity and position. This would be a perfectly good system if it was not for the fact that measurement errors introduce faults in the calculations. These faults make it hard to pinpoint the exact position of the rocket. The faults can be kept to a minimum with the help of Global Positioning System (GPS) measurement data and a sensor fusion solution that will correct the INS calculations if drifts occur. 1.1 Objective The objective of this Master’s thesis is to investigate and propose a design for an integrated GPS INS solution to be used in the positioning of a rocket. The thesis proposes two different Extended Kalman filters using so called Loose Integration. One is based on Euler angle linearization, while the other is based on quaternion linearization. A study on how to develop a GPS receiver capable of withstanding the high dynamics of a sounding rocket is also presented. 1 A sounding rocket developed by Swedish Space Corporation (SSC) and Astrium Space Transportation [7] 1 2 1.2 Introduction Problems and Methods This section presents the problems that exist when integrating GPS and INS to navigate in a high dynamic application such as a rocket flight, and what methods that could be used to manage these problems. Integration of GPS and INS GPS and INS are two different systems used in navigation. The GPS is quite slow, two to three updates per second, but produces a positioning error which will not grow with respect to time. The INS however is very fast, about 200 measurements per second, but when integrating acceleration to form velocity, small errors will be introduced in the system. The errors propagate with each calculation thus making the system more and more unreliable with respect to time. The different strength and weaknesses of the GPS and INS can be used to create a more robust and precise navigation system. The integration of the two systems is performed by a Kalman filter using INS and GPS data. Further information about the integration and an introduction to various integration techniques can be found in Chapters 5 and 7. The integration technique that is implemented in this thesis is the loose integration which is explained in Chapter 5. Using GPS in a High Dynamics Environment During a sounding rocket flight high acceleration and deceleration often appear. For example, in the case of the launch and flight of a MAXUS rocket the amount of stress caused by the acceleration can be measured to about 130 m/s 2 (13 g), while in the reentry-phase a deceleration of about 40 g will affect the rocket. With these quick changes in velocity a regular GPS receiver will not be able to adapt to the signals of the satellite, thus the receiver will loose its sense of positioning. The reason for a GPS receiver losing track is that when the velocity of the rocket suddenly changes, so does the introduced Doppler effect on the GPS signal of the satellites. A GPS receiver will always try to adapt to Doppler effects introduced to the signal and will normally cope with the changes, but in the case of a rocket flight these changes are just too large for a regular GPS receiver. More details on GPS receivers and their design can be found in Chapter 4. 1.3 Previous Research In this section earlier research of creating tracking loops for high dynamics and different integration techniques for GPS INS integration are introduced. Loop Filter based Carrier Tracking Loops A recent related research was done in 2006 [23], where a Frequency-Lock Loop (FLL) assisted Phase-Lock loop (PLL), see Section 4.2, was constructed and tested. The designed loops were implemented in a GPS receiver and tested with frequency modulated radio signals. Simulations and experimental results confirmed that with a careful design, the digital loops can expand their tracking capacity up to acceleration steps of 40 g. 1.3 Previous Research 3 The work was based on [24], where the performance of FLL assisted PLLs were investigated. The Space Flight Technology department of Deutches Zentrum für Luft und Raumfahrt (DLR), Germany, also constructed FLL assisted PLL algorithms to be used in their high dynamic GPS receiver Phoenix HD [20]. They use a wide-band third order PLL with FLL assist. Their algorithms were also previously tested in [21] with an older GPS receiver (GPS Orion) used in a MAXUS rocket with good results. A comparison between that receiver and other high dynamic GPS receivers was done in [4], where the Orion GPS excelled. GPS INS Integration Techniques The are many ways to integrate the GPS and INS systems, but most of them can be grouped into three levels of integration. These integration methods are loose, tight and deep integration. These levels can be further differentiated, where for example tight integration can be either classical or alternative. There has been quite an extensive research about integration methods combining INS and GPS since the launch of GNSS. Current research aims at deep (often called ultra-tight) integration, where the INS is deeply integrated with the carrier and code loop of the GPS to gain an even more robust signal tracking than tight integration. The different methods of integration are further described in Chapter 5. Chapter 2 Inertial Navigation The basic idea of inertial navigation is to measure the motion relative a given point in space in order to compute the actual position. It can also be described as a formula: Let R(t) be a vector that points to the actual position and v(τ ) be the actual velocity, then the position can be described as Zt R(t) = v(τ )dτ + R(0) (2.1) 0 The relation is based on Newton’s laws of motion, which are stated below. Isaac Newton’s first and second law of motion as found in [16] states 1. A body in motion tends to maintain its motion unless acted on by a force 2. a = F/m Newton’s laws of motion are valid in a reference frame that is neither rotating nor accelerating. The origin of the inertial coordinate system is arbitrary, and the axis may point in any three perpendicular directions. It is however convenient to consider the inertial frame of reference with its origin in the center of the earth, since we are only moving in close proximity to the Earth. By placing the inertial frame in the center of the Earth, it is no longer an inertial frame mainly due to the gravity field of the Earth. This new frame is called Earth centered inertial frame (ECI) and is an approximate inertial frame. In ECI a body is considered to be in rest if it is in free fall or equivalently accelerating with g toward the center of the Earth. 2.1 Coordinate Frames Position and movements can be presented in a number of ways each with its own coordinate system. In this section the different coordinate systems used in this master’s thesis are presented. These descriptions are a summary of the frames explained in [9]. 5 6 Inertial Navigation ECI (Earth Centered Inertial) The ECI is a frame with its origin in the center of the Earth. The z-axis is parallel with the rotation axis of the Earth. The y- and x-axis span the equator plane of the Earth and the x-axis points at the vernal equinox. Figure 2.1 shows the ECI system. Figure 2.1. The Earth Centered Inertial frame. The z-axis is parallel to the rotation axis of the earth, the x- and y-axis span the equatorial plane and the x-axis points at the vernal equinox. ECEF (Earth Centered Earth Fixed) The Earth Centered Earth Fixed frame (ECEF) is a frame with its origin in the center of the Earth. The z-axis is parallel with the rotation axis of the Earth. The y- and x-axis span the equator plane of the Earth and the x-axis points at the Greenwich meridian. This frame, unlike the ECI, rotates with the Earth which means that for example, Linköping will have a fixed position (x, y, z) in the ECEF regardless of the movement of the Earth. Figure 2.2 shows the ECEF frame. B-frame (The Body Fixed Frame) The Body Fixed Frame (B-frame) is a frame that is fixed to the host vehicle. Figure 2.3 shows the Body fixed frame. 2.2 Inertial Measurement In an inertial navigation unit there are typically two sensors, a gyro and an accelerometer. The accelerometer measures the accelerations applied to its acceleration sensitive axis relative the inertial frame. The gyro measures the angular rates around its three axis relative the inertial frame. When the accelerometer is in close proximity to the Earth, it measures the accelerations relative to the ECI. This means that if the accelerometer is placed on 2.2 Inertial Measurement 7 Figure 2.2. The Earth Centered Earth Fixed frame. The z-axis is parallel to the rotation axis of the earth, the x- and y-axis span the equatorial plane and and the x-axis points at the Greenwich meridian. This frame also rotates with the earth Figure 2.3. The Body fixed frame. This frame is fixed to the vehicle the surface of the Earth lying completely still, it will measure an acceleration of 1 g radially out from the Earth. If the accelerometer falls to the ground it will measure zero force on all axes before it hits the ground (in an ideal case). To compensate for the gravity effects on the accelerometer a gravity model is needed. The gravity model subtracts 1 g of acceleration toward the Earth to compensate for the gravity field effects on the accelerometer. The gyro however does not need to be compensated for gravity effects. 8 2.3 Inertial Navigation Choosing Reference Frame It is often desired to navigate relative the Earth. Choosing the ECEF frame as a reference frame results in an Earth relative navigation. A few more compensations must be done due to the rotation of the Earth. The centripetal force and the Coriolis force must be compensated for in the accelerometer. The gyro must also be compensated for the rotation of the Earth. The equations for the ECEF compensations as well as the corresponding gravity model are presented in Chapter 6. 2.4 Strap Down Navigation By measuring the accelerations acting on the accelerometer, then compensating for the gravity field and the forces due to the rotation of the Earth, we can now integrate the accelerations twice to determine the position in the ECEF frame. If the accelerometer is perfectly aligned to the ECEF frame at all times, the double integration of the accelerations are all the computations needed in order to determine the position. This method of having the accelerometer aligned with the reference frame and thus completely isolated from the attitude of the host vehicle is called a gimbaled implementation of inertial navigation. Bolting the accelerometer to the chassis of the host vehicle and adding a gyro is called strap down navigation. This means that the accelerometer now is aligned with the time dependent B-frame of the host vehicle. In order to determine the position in the reference frame the attitude of the B-frame relative the reference frame must be known at all times. The attitude can be computed by integrating the angular rates provided by the gyro. Knowing the attitude of the B-frame the accelerations measured by the B-frame aligned accelerometer can be transformed to the reference frame and can thus be used to compute the position. Chapter 3 Global Positioning System This chapter introduces the Global Positioning System (GPS), with a short historical description followed by a description of the technical properties or the system. 3.1 Brief History The concept of NAVSTAR GPS was formed in the wake of the Defence Navigation Satellite System (DNSS) program, which was established 1969 by the Office of the Secretary of Defense (OSD). This was in response to the need of a navigation system in the United States (US) military that could be used by high dynamics vehicles such as aircraft. The former US president Ronald Reagan announced in 1983 that the system would be declassified, which meant that the system would move from pure military applications to include civilian ones. This was made after a Korean civilian airplane carrying 269 people accidentally crossed the Soviet border twice and was shot down by Soviet jets. The GPS signal that the civilian GPS receivers got access to was intentionally degraded with an error called Selective Availability (SA). SA introduced a random fault in the GPS signal that lowers the resolution of the position if you do not possess a certain encryption key. This key was only available to the US military and its allies and it changes daily. In 2000, after an executive order by the US president Clinton, the system was deactivated and has remained so until present day. The GPS is a navigation instrument that provides users the ability to calculate their position and course worldwide. To be able to cover the whole world, the system originally consisted of 24 operational satellites (as shown in Figure 3.1) orbiting the Earth at an altitude of about 20200 km. Today the system consists of additionally eight satellites, increasing the operational satellites to 32. The increase provides the system with redundant hardware, thus making it more precise and more robust against degradation of the signals caused by malfunctioned satellites. The satellites are continuously sending out information that can be obtained if 9 10 Global Positioning System Figure 3.1. [10] The Global Navigation Satellite System (GNSS) constellation of satellites you are within range and in possession of a GPS receiver. The GPS receiver needs to obtain signals from at least four different satellites to be able to determine the position. If more than four signals from different satellites can be obtained, the positioning error decreases as the receiver can pick and choose the best signals. 3.2 Calculating the Position To calculate the receiver’s position, it must find four unknown values; position (x, y, z) and clock error. The receiver calculates its distance from every satellite that it has obtained a signal from. This is done by looking at when the satellite sent the signal and when it was obtained by the receiver. If one have the precise time, one could easily calculate the distance by multiplying the time it took for the signal to travel from the satellite to the receiver with the speed of light. The calculations needed to determine the position forms a 3-D trilateration algorithm. It is essentially a triangulation, but instead of circles, spheres are used. One will need at least three different satellites for the algorithm and thus knowing the distance to three satellites. If the receiver knows the distance to one satellite it knows that its position is within a sphere with a radius that is equal to the distance with the satellite being in the center (see Figure 3.2 sphere 1). If then a second satellite is added, creating two of these spheres, the receiver knows that its position is somewhere in the resulting ring (the resulting line in Figure 3.2 sphere 2). By introducing the distance of a third satellite, the receiver can narrow down its search area to two points (see Figure 3.2 sphere 3). One of the points will be located in space while the other on the surface, meaning that the receiver now knows its position. 3.3 The GPS Signal 11 Calculating the time it took for the signal to reach the position of the receiver would mean that the receiver needs a very precise clock. Those precise clocks are very expensive. For example an atomic clock cost from 50000$ to 100000$. This problem is solved in quite a clever way. Every satellite has a precise atomic clock, while the receivers have inexpensive quartz clocks. The quartz clock is reset constantly during the receivers operation. What the receiver does is calculating the unwanted time offset between the atomic clocks and the quartz clock. This is done by calculating the time value that the four satellites must have to align in a single point in space. The calculations for this is done in the same way as those used in determining the position of the receiver, but this time you will need a fourth satellite to determine which of the two points that is the right one. The receiver now knows the exact time offset and has in essence transformed the quartz clock to a cheap atomic one. 3.3 The GPS Signal The signal that the GPS satellites is continuously sending is created by combining three different signals, the Pseudo Random Code (PRN) code, the navigation message and the carrier signal. In civilian applications, that is not the US military, the PRN code is the Coarse Acquisition (C/A) code . Every satellite has its own predetermined C/A code which makes it possible for all satellites to use the same frequency without destroying each others message. First the C/A- code is combined with the navigation message and the resulting signal is then combined with the carrier signal creating the final GPS signal. One could, if using an analogy, see the carrier signal as a large box where the message (navigation data) should be put. Every sender (satellite) sends their message in the same large box with the only difference being where they put the message. The purpose of the C/A code is informing where every sender should place its message. To find a message the receiver searches through all the potential places in the box where the message might be. If the receiver then finds the message it also knows the identity of the sender. The navigation data is transmitted with a bit rate of 50 bps. The data is 1500 b long which means that to receive one complete frame of data 30 seconds is needed. There are five different sub frames that forms the complete navigation message, each one is 300 b long . The first sub frame contains information about the sending satellite’s GPS week number, its health and accuracy, and its the clock correction terms. The second and third sub frames contain all the ephemeris data. Sub frame number four contains almanac and health data for satellites number 25-32, special messages, satellite configuration flags, ionospheric and UTC data. The fifth sub frame contains almanac and health data for satellites number 1-24, almanac reference time and week number. The last two sub frames are compromised by 25 different pages. These pages are cycled with each navigation data transmission. So in order to get the full frame of the satellites you will need to intercept 25 12 Global Positioning System complete frames. It takes 12.5 minutes to attain a whole set of frames. The GPS satellites are broadcasting signals with two different carrier signals with different frequencies, L1 and L2. The L1 frequency is for everyone, while L2 is strictly for the military. The L2 frequency is only transmitted with the P-code, while the L1 frequency transmits both the C/A and P-code. For the task of decrypting the military transmissions specialized hardware is needed. There are two variants of the P-code. One is simply called P-code while the other is a special form of the P-code, called Y-code, which is used to protect against false transmissions. In this application the L1 frequency is used. A good overview of how the navigation data, the different codes and the carrier signals form the GPS signal is given by Figure 3.3. 3.3 The GPS Signal 13 Figure 3.2. The figure pictures the three steps needed for the GPS receiver to calculate its position. If one know the distance to one satellite, one knows that the possible position solutions of the receiver is within a sphere with the satellite in its center. Knowing the distance to two satellites and then calculating the intersection narrows the position solutions to a circle. If a third satellite is used one can calculate the intersection between its sphere and the earlier calculating circle to narrow the possible position solution down to two points. One of the points is located in space while the other near the surface. 14 Global Positioning System Figure 3.3. The figure explains how the navigation data is mixed to form the channels L1 and L2 Chapter 4 GPS Receiver To be able to obtain the signals that the GPS satellites broadcast a GPS receiver is needed. This chapter gives an introduction of the GPS receiver design and a more in-depth study of the parts that are needed in designing a GPS receiver for use in high dynamics applications. 4.1 The Design The function of the GPS receiver design can be divided into three groups each constituted of several functions and components. By working together the subgroups makes it possible to extract the important navigation data from the GPS signal. First you have the front-end of the receiver where the GPS signal is down converted to a digital intermediate frequency (IF) signal. Basically this means that the GPS signal is converted from an 1575.42 MHz analog signal to a digital signal with a much lower frequency, often in the range of about 4 MHz. After the conversion the data extraction begins and this is where the second group makes its entry. This group contains the functions that help the receiver tracking the signal and extract data. The mission of each channel is to lock onto one unique signal of a satellite with the help of the carrier tracking and then extracting the navigation data with the code tracking. The attained data from the tracking functions is then processed by the functions that constitute the third group. These functions process the data to calculate the correct position of the receiver. Figure 4.1 shows a simplified overview of the complete process. 15 16 GPS Receiver Figure 4.1. GPS overview of a simplified GPS receiver. The antenna receives the 1575.42 MHz GPS signal and sends it to the group of functions that constitute the front-end (Preamp, Down converter, A/D converter and AGC). The front-end samples the analog signal and converts it to a 4 MHz digital signal (the Digital IF signal in the picture above). The digital signal is then processed by the Receiver channels and Receiver processing to extract the data (clock error, pseudo range) needed by the Navigation processing functions to calculate a position estimate. 4.2 Carrier Tracking Before any extraction of navigation data can take place, the receiver must wipe off the carrier frequency from the signal. This is done by creating a replica of the incoming carrier signal, see Figure 4.2. In order to do this, a good tracking is necessary. The tracking of the carrier signal is where the problems of using the GPS receiver in a high dynamic environment will be the most prominent. The carrier tracking is managed by a carrier tracking loop, which is illustrated in Figure 4.2. A carrier tracking loop is made up of several different parts but the most important ones are the carrier predetection integrators, the carrier loop discriminator and the carrier loop filters. The discriminator determines what kind of tracking loop that is used. The tracking loop can be either a phase-lock loop (PLL) or a frequency-lock loop (FLL). The two different tracking loop discriminators can also be used in conjunction to solve their inherent flaws. Information about these flaws and the characteristics of the PLL and FLL can be found later in this section. The discriminator uses the I and Q signals, see Figure 4.2, together with the chosen discriminator algorithm when creating an output error. The I and Q signals are created by the carrier wipe-off functions by multiplying the digital IF signal with a cos, respectively a sin replica signal. For a FLL to create a frequency error it needs two pars of I and Q samples taken from two different adjacent moments in time. In Table 4.2 these signals are called I1 , I2 , Q1 and Q2 . In high dynamic applications used in the launch and flight of a sounding rocket, a short predetection integration time and a wide bandwidth are needed to handle the stress. With a short integration time the measurements will be faster which can be crucial when the rocket is speeding with several kilometers per second. A wide bandwidth will provide a better signal. 4.2 Carrier Tracking 17 On the other hand a narrow bandwidth and a longer integration time would mean better Doppler phase measurements as the noise in the signal is lower. Numerically Controlled Oscillator Figure 4.2. The different parts and layout that makes up a tracking loop Phase-Lock Loop The Phase-Lock Loop (PLL) is one of the most commonly used carrier tracking techniques. The PLLs used in GPS carrier tracking are usually of the Costas type PLLs [11]. The Costas PLL can have several different discriminator algorithms, as shown in Table 4.1. The phase error will affect the numerically controlled oscillator (NCO) period after being filtered in the carrier loop filter. This will result in the decreasing of the phase error thus getting a better replicate wave to represent the incoming sinus wave. A PLL produces accurate phase measurements, which are important in the aiding of the code loop, but it is sensitive to dynamic stress such as acceleration and jerk. A PLL could be made more robust against dynamic stress with a good loop filter. Although this is not enough to handle the amount of stress present in the launch and flight of a sounding rocket. PLLs are sensitive to dynamic stress, but produces the most accurate velocity measurements thanks to its good phase measurements. Because of the inherent sensitivity to high dynamics stress of the PLLs, caused by the narrow bandwidth, this disqualifies it for tracking a launched rocket. Frequency-Lock Loop The ultimate goal of the Frequency-Lock Loops (FLLs) used in GPS receivers is the same as for the PLLs. FLL discriminators have short predetection integration time, thus it is well adapted when it comes to handling dynamic stress. Even though it is faster in predicting the carrier frequency it has more difficulties measuring the carrier Doppler phase than the PLL because of the inherent trouble in detecting a change of sign of the phase. This difficulty means that the FLL produces noisier error output values than the PLL. Noisier values translate to degradation of accuracy in comparison. The inherent problem with the accuracy of FLL discriminators could create quite large position discrepancies when used on a very fast vehicle and is therefore a poor choice, if used alone. 18 GPS Receiver Table 4.1. A table presenting the most commonly used PLL discriminator algorithms Discriminator algorithm Q·I Output phase error sin 2φ Q · sign(I) sin φ Q/I tan φ arctan(Q/I) φ Characteristics Classic Costas analog discriminator. Near optimal at low signal to noise ratio (SNR). Slope proportional to signal amplitude squared. Moderate computational burden. Decision directed Costas. Near optimal at high SNR. Slope proportional to signal amplitude. Least computational burden. Suboptimal but good at high and low SNR. Slope not signal amplitude dependent. Higher computational burden. Divide by zero error at ±90o . Two-quadrant arctangent. Optimal (maximum likelihood estimator) at high and low SNR. Slope not signal amplitude dependent. Highest computational burden. Examples of common FLL discriminators can be found in Table 4.2. Combining FLL and PLL When comparing the FLL and PLL characteristics one can see that by combining their strengths, one would have a lock loop that could lock onto a signal with good phase accuracy and still work under high dynamic stress. This is the goal of the FLL assisted PLL, see Figure 4.3. The combined FLL and PLL works in a coupled mode where the FLL performs the signal acquisition in high dynamic situations and then helps the PLL to achieve a phase lock. The acquisition could also be improved by using a carefully designed FLL and then revert to the combined FLL and PLL when you get a good lock of the signal. When phase lock have occurred several options exist. The tracking design could either convert the FLL assisted PLL into a pure PLL until the phase lock is lost and sub sequentially revert back into a pure FLL or one could proceed with using the coupled FLL and PLL as suggested in [24]. If the two are used in a coupled mode with the addition of a good loop design, it could very well be used in situations of high dynamics without the need for assistance of an INS system. Externally Aided Carrier Tracking Loops One way to solve the problem of using GPS receivers in a high dynamics environment is by estimating the change of 4.3 Code Tracking 19 Table 4.2. A table presenting the most commonly used FLL discriminator algorithms Discriminator Algorithm A t2 −t1 Output frequency error sin(φ2 − φ1 ) Characteristics sin 2(φ2 − φ1 ) Decision directed. Near optimal at high SNR. Slope proportional to signal amplitude. Moderate computational burden A = I1 · Q2 − I2 · Q1 A·Q1 ·sign(B) t2 −t1 B = I1 · I2 + Q1 · Q2 arctan 2(B,A) t2 −t1 φ2 −φ1 t2 −t1 Near optimal at low SNR. Slope proportional to signal amplitude squared. Least computational burden. Four-quadrant arctangent. Maximum likelihood estimator. Optimal at high and low SNR. Slope not signal amplitude dependent. Highest computational burden. Usually table lookup implementation. the Doppler effect. If a good estimation of the Doppler effect can be had, then the change of frequency of the GPS signal could be calculated. One could then feed this information into the carrier tracking to get a better lock of the signal. Aiding the carrier tracking in this way can reduce the acquisition time, help keep lock on the signal and also in the reacquisition of a lost signal. The Doppler estimation can be calculated in various ways. The easiest would be to take the acceleration data from the INS and just integrate it to a velocity and then calculating the Doppler effect using fv ∆f = c where f is frequency of the GPS signal (without Doppler), ∆f is the change of frequency, v is the relative velocity between the satellite and the rocket and c is the speed of wave, that is for electromagnetic signals, the speed of light. What one would prefer is to use the filtered velocity from the Kalman filter and thus using both the GPS and INS data to form a better estimation of the Doppler effect. The relative velocity is calculated from the knowledge of the movement of the rocket and the empheris data from the satellite. The use of aiding could be included in for example the FLL assisted PLL to make the solution more robust and maybe also enable it to handle even more stress without making large changes to the design of the discriminators and the loop filters. 4.3 Code Tracking With the help of a shift register the receiver uses the I and Q signals to create new signals called early, prompt and late, where the only difference is the time offset. The signals are then correlated with the incoming digital IF signal to measure if the 20 GPS Receiver Figure 4.3. A FLL assisted PLL loop filter. Note that the insertion point of the FLL is one integrator back from the PLL. This is because the FLL error is in units of Hertz, whereas the PLL errors are in units of phase. replica code is properly aligned. There are several different algorithms to calculate this correlation. The method of code tracking is called Delay-Lock Loop (DLL) and the algorithms used in code tracking are called delay lock loop discriminators. The DLL is often aided by the carrier tracking like the simple GPS receiver shown in Figure 4.1. 4.4 Loop Filters The purpose of the loop filters is to filter the error signal produced by the discriminator in order to provide a better signal to the numerically controlled oscillator (NCO). The order of the loop filter and its noise bandwidth also determines the response of the loop filter to signal dynamics. There are two loop filters present in GPS receivers, one is used for the carrier tracking loop and the other is used for the code tracking loop. Most common GPS receiver uses a second order loop filter for the carrier tracking whereas the code tracking loop normally uses a filter of order one. A analog loop filter with a first or second order is unconditionally stable at all noise bandwidths whereas a loop filter of order three remains stable if the noise bandwidth is within 18Hz [24]. The reason why one often would like to use a filter with a higher order than two, despite being less stable, is because of the useful characteristics of being insensitive to both velocity and acceleration stress [24]. Both these are present in the launch and flight of a sounding rocket. 4.5 GPS Receiver Hardware This section introduces and discusses the different hardware options when creating a low-cost GPS solution capable of handling high dynamics. The chosen hardware must be flexible enough so it can be able to run custom firmware. 4.5 GPS Receiver Hardware 21 Previous Development There have been several projects building GPS receivers and implementing custom software. Several of these projects, like DLR’s Phoenix HD receiver [20] can be traced back to Mitel’s (now Zarlink) GPS receiver. Mitel created a low-cost receiver prototype aimed for mass market applications. This prototype never entered production but, thanks to the open design documentation and the fact that the components used in the prototype could be purchased independently, the design idea was further developed later on in various projects. There is also another reason why Mitel’s creation inspired further development. In order to sell more hardware, Mitel developed a complete software development kit (SDK) called Mitel Architect code (sometimes referred to as Orion). This SDK could be licensed from Mitel and allowed users to modify and expand an already working code base. Unfortunately the SDK was discontinued and one can no longer be obtain a license from Zarlink. The Orion receiver is compromised by a GP2015 front-end, a DW9255 saw filter, a GP2021 and an ARM60B microprocessor [19]. Zarlink nowadays offers the GP4020 which is a unified platform including correlators and an ARM7TDMI microprocessor with built-in functions such as a real-time clock and watchdog. The GP4020 has been used to create the Novatel Superstar II receiver and Signav’s MG5001 receiver. The Novatel Superstar II offers a development kit but does not provide the source code. This could make it hard to expand the functions and still be able to use existing functions of the firmware. The MG5001 would be quite attractive if it was not for the fact that it has been discontinued. Signav has moved on making several solutions in the navigation field and consequentially stopped the development of the MG5001 receiver. This is unfortunate as Signav created a MG5001 receiver compatible software development kit (SDK) which allowed developers to modify parts of the software. Signav’s SDK was called MG5021 development kit and it was a kit based on the GP4020 platform. Unfortunately this SDK also has been discontinued. The earlier mentioned Architect SDK has been modified to work on the MG5001 [22]. The reason why a port of the Architect code was done was that while the MG5021 is a good SDK, it did not offer full access to the source code [18]. German Space Operations Center (GSOC) and Deutches Zentrum für Luft- und Raumfahrt (DLR) used the MG5001 receiver with their own software to create the Phoenix Miniature GPS receiver which has been proven to work in high dynamics situations. The software used was most likely based on their previous work on an Orion receiver, which was a modified version of the Architect code. There have been some recent development on open source receivers like the Namuru platform from the University of New South Wales, Sydney Australia [22], and GPS1001 from GPS Creations [8]. The Namuru platform uses Field-programmable gate arrays (FPGAs) in conjunction with a ported version of the Architect code while the GPS1001 is implemented on a computer ISA-card. The GPS1001 comes with an open source GPS code. 22 GPS Receiver The question about what software to use is difficult as the earlier mentioned SDKs can no longer be purchased or licensed. There might be a possibility to reach a deal with third-parties who already own a license, but it could prove difficult. There have been efforts made in developing an open source software such as GPLGPS [1] and OSGPS [5]. The GPL-GPS code is compatible with the GP4020 platform and could potentially be used as a starting point. This option however requires quite some effort as it does not support for example functions for atmospheric corrections among other things. But it might be the best thing if you really want to be able to set up your own GPS receiver development system. The OSGPS code has a lot of functions that the GPL-GPS code lacks, but as it is designed to run on GPS1001 hardware it is not suited for use in a rocket. There is a possibility that one could port some of the functions to the GPL-GPS code in order to create an attractive custom open source development kit. This approach will however require some additional effort than just replacing the tracking loops with help of an existing SDK. Hardware summary In order to choose the best hardware several factors have to be taken into consideration; hardware price, software options and availability. The Orion design is quite dated and some of the components used have been discontinued or replaced by newer versions like Zarlink’s own GP4020 integrated chipset. The GP4020 platform together with the front-end GP2015 is an attractive low-cost solution for a GPS receiver and has been successfully used in several different products like the MG5001 receiver. The GP2015 chipset and the GP4020 platform are still being produced and should not be difficult to obtain. There is still a large question mark about what code to use in development of new software for the receiver. The MG5001 receiver, which uses the GP4020 platform, is not really an option as the product is discontinued and it has been for some time. There is also the issue that if the MG5021 SK can not be obtained, one is faced with the problem of developing the firmware code. The Phoenix HD receiver is an already competent GPS receiver and could be used as it is for future MAXUS missions. If one only wants a working product and not the know-how and option of developing a specialized GPS receiver, then this might be the way to go. The disadvantages with this approach is that it can be expensive and there will probably arise problems later on if one would like to expand its features. The fact that as it belongs to the GSOC and DLR, and thus the German state could also pose some problems regarding, for example, exportation of the product. Both the Namuru receiver and the GPS1001 receiver should both be quite good if one wanted to learn and expand the functions in an GPS, but they are ill suited for practical use with rockets. First of all they are more complex than needed. The Namuru receiveruses FPGAs, which are nice when developing and testing hardware but are not as efficient as specialized hardware. Moving from a GPS correlator chipset to a general purpose processor or FPGA currently incurs unacceptable power and size penalties for the embedded applications. The GPS1001 receiver is an ISA-card and also suffers from the same problems as Namuru regarding power efficiency and size of the system. Chapter 5 GPS INS Integration The GPS and the INS have complementary characteristics. In an INS small errors will propagate with each position estimation, that is the system has an unbounded error. The GPS solution, however, has a bounded error typically for a civilian receiver in the range of ± 10 m. The GPS output rate is smaller than 20 Hz while the INS have an output rate of more than 100 Hz. The GPS user may also experience short term losses of the signal because of signal jamming, blockage or interference. The INS however does not rely on external signals hence its insensitivity to jamming, blocking and interference. Combining these sensors gives the advantages of both and removes most of their individual weaknesses. If the GPS has lost the GPS satellite signal, the INS can still compute a position and help the GPS to re-acquire the lost signal. An integration, with the help of a Kalman filter (see Chapter 7) of the two systems makes it possible to estimate the error of the INS. The estimated error can then be feed back to the INS system thus making the error bounded. The result is a navigation system which enjoys the measurement speed of a INS but with a bounded error thanks to the GPS. There are many possible options and techniques when integrating the two different systems, and one can essentially divide them into the three groups; loose integration, tight integration and deep integration. The names and definitions of the different techniques can differ much from one article to another. In this thesis the definitions according to [13] are used. 5.1 Loose Integration Loose integration is the most commonly used integration technique when integrating GPS and INS, due to its simplicity and the low computational burden relative to other integration techniques. The idea of the loose integration is to see the GPS as a black box which gives a calculated position with a certain bounded error as the output. The position and velocity data from the GPS is then blended with the output from the INS, typically in a Kalman filter or in an extended Kalman filter (EKF). The unbounded error of the INS can then be estimated and fed back to create a bounded INS solution. 23 24 GPS INS Integration Figure 5.1. An example of loose integration. The EKF blends the GPS and INS solutions and estimates a position. Estimated acceleration and angular bias are fed back from the EKF to the INS. 5.2 Classic Tight Integration Classic tight integration can essentially be explained as when the GPS navigational computations are performed in an EKF. This integration technique sees the GPS receiver as a measurement unit that measures the distance to visible satellites, that is pseudo range. The measured pseudo ranges are taken in as observations in the EKF. The EKF in this integration differs from the loose integration in two aspects; one, it has an additional state for the receiver’s clock error and two it uses pseudo ranges as a measurement of distance. The advantages of this integration is that when less than four satellites are visible to the receiver, the EKF will still be able to use satellite data to estimate a position. The disadvantage with this approach is the higher computational burden and an increase in complexity. 5.3 Tight Alternative Integration The Alternative Tight Integration is similar to the classic tight, with the difference that it aids the GPS carrier tracking loop with Doppler estimates computed from the EKF. More about Doppler aiding frameworks can be found in Section 4.2. 5.4 Deep Integration The basic idea with deep integration is that the code- and carrier-tracking loops are integrated into the EKF. This technique has a very high computational burden, since the EKF must be updated very often. The update rate must atleast be in the range of 100 to 200 Hz becauset that is the typical rate of tracking and code 5.4 Deep Integration 25 Figure 5.2. An example of classic tight integration. The EKF uses the INS solution, and the measured pseudo ranges and clock error from the GPS receiver to calculate a position. The estimated accelerator and angular bias are fed back from the EKF to the INS. loops. The EKF computes the pseudo ranges and blends them with IMU data as in the classic tight integration. 26 GPS INS Integration Figure 5.3. An example of alternative tight integration. The EKF uses the INS solution and the measured pseudo ranges and clock error from the GPS receiver to calculate a position. The estimated accelerator and angular bias are fed back from the EKF to the INS. In this integration, unlike the classic tight integration, the Doppler effect is also estimated by the EKF and fed back to the receiver to aid the carrier tracking. Chapter 6 Navigation Equations This chapter introduces the navigation equations used in calculating the position, velocity and attitude of the rocket. Their respective linearization are also described. 6.1 Notation Before the navigation equations are explained some notations must first be considered which are explained in table 6.1 and 6.2. Table 6.1. Notation of the navigation equations, a and b can be any frame Notation xa Cab qab [v × ] fa a ωab Description A position in the coordinate frame a The transformation matrix from frame a to frame b The quaternion rotation from frame a to frame b The cross-product of v in matrix form. (v × w = [v×]w) A specific force with unit m/s2 in the a-frame The angular velocity between frame a and b with the unit rad/s in the a-frame Table 6.2. Notation of the different frames, each frame is described in chapter 3.1 Notation i e b Description The I-frame (Earth Centered Inertial frame) The E-frame (Earth Centered Earth Fixed frame) The B-frame (Body Fixed frame) 27 28 6.2 Navigation Equations Deriving the Navigation Equations The navigation equations purpose is to describe the movements of the rocket in the E-frame. These navigation equations are based upon [3]. The starting point for the derivation is in the I-frame where accelerations will be integrated in order to obtain position and velocity. The equations describing this are ai = d i x˙ = x ¨i = f i + g i (xi ) dt (6.1) where xi is the position of the rocket, f i is the measured acceleration from the IMU in the I-frame and g i (xi ) is the gravitational force in position xi . If the application had been ground based, g i could have been approximated as a constant. But as the rocket accelerates from the Earth the gravitational force affecting the rocket will drastically change. While the navigation equations are now fully described in the I-frame, the E-frame equations remain. The transformation of a position from one coordinate system to another is performed with the help of a transformation matrix. xi = Cei xe (6.2) The relation describes the transformation from a position in the E-frame to the I-frame. Differentiating (6.2) gives x˙ i = C˙ ei xe + Cei x˙ e (6.3) Differentiating (6.3) then results in x ¨i = C¨ei xe + C˙ ei x˙ e + C˙ ei x˙ e + Cei x ¨e = C¨ei xe + 2C˙ ei x˙ e + Cei x ¨e (6.4) which will be used in relation (6.1) when solving for xe . But first multiple expressions for the derivate of the transformation matrices are created. The derivative of Cei is in [9] described as C˙ ei = Cei Ωeie (6.5) e where Ωeie is a skew-symmetric matrix. Ωeie and ωie Earth and are given by   e ωie = 0 0 ωe  0 −ωe  e  ωie × = Ωeie = ωe 0 0 0 describes the rotation of the (6.6)  0 0 0 (6.7) C¨ei is then calculated by taking the derivative of C˙ ei as follows C¨ei = C˙ ei Ωeie + Cei Ω˙ eie = Cei Ωeie Ωeie + Cei Ω˙ eie (6.8) Combining (6.3), (6.4), (6.5) and (6.8) results in x ¨i = C¨ei xe + 2C˙ ei x˙ e + Cei x ¨e ˙ eie )xe + 2Cei Ωeie x˙ e + Cei x = Cei (Ωeie Ωeie + Ω ¨e (6.9) 6.2 Deriving the Navigation Equations 29 An expression for x ¨e can now be derived. Setting (6.1) equal to (6.9) gives g i (xi ) + f i = Cei (Ωeie Ωeie + Ω˙ eie )xe + 2Cei Ωeie x˙ e + Cei x ¨e (6.10) Solving (6.10) for x ¨e is done in the following way −1 (g i (xi ) + f i ) − (Ωeie Ωeie + Ω˙ eie )xe − 2Ωeie x˙ e = Cie (g i (xi ) + f i ) − (Ωeie Ωeie + Ω˙ eie )xe − 2Ωeie x˙ e x ¨e = Cei (6.11) ˙ eie )xe − 2Ωeie x˙ e = g e (xe ) + f e − (Ωeie Ωeie + Ω Assume that the angular velocity of the Earth relative to inertial space Ω˙ eie , is constant. This assumption together with equation (6.11) gives Ωeie = 0 and the resulting equation x ¨e = g e (xe ) + f e − Ωeie Ωeie xe − 2Ωeie x˙ e (6.12) where the gravitational force g e with a good approximation can be written as in [9] kM e (6.13) ge ≈ − 3x | xe | kM is the standard gravitational parameter, µ, which is the product of the gravitational constant and mass of the Earth as in µ = kM = 398600.4418 km3 /s2 The measured gravitational force f b can be transformed to the E-frame using the relation f e = Cbe f b (6.14) Cbe is obtained by calculating Ceb with the equation for the transformation matrix from a quaternion described in [9], given by   2 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) q0 + q12 − q22 − q32 2(q2 q3 + q0 q1 )  q02 + q22 − q12 − q32 (6.15) Ceb (qeb ) =  2(q1 q2 − q0 q3 ) 2 2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) q0 + q32 − q12 − q22 where the quaternion is denoted   q0 q1  b  qe =  q2  q3 (6.16) Cbe is then obtained by taking the transpose of Ceb because of the transformation matrix properties. Cbe = (Ceb )−1 = (Ceb )T (6.17) 30 Navigation Equations Last but not least the time derivative of the quaternion’s can be expressed as described in [9]     q˙0 −q1 −q2 −q3   q˙1  1  q0 −q3 q2  r  =   s (6.18) q˙2  2  q2 q0 −q1  t q˙3 −q2 q1 q0 | {z } Ξ(q)  where r 6.3 s  b t = ωeb . Linearization with Quaternion Angle Representation In this section the navigation equations are linearized with quaternion’s as the angle representation. Consider the nonlinear differential navigation equations with quaternion angle representation. b e q˙eb = Υ(ωib − Ceb (qeb )ωie )qeb x ¨e = −2Ωeie x˙ e − Ωeie Ωeie xe + Cbe (qeb )f b + g e (xe ) (6.19) where the time derivative of the quaternion is written as in [16].The matrix function Υ(ω) is   0 −ωx −ωy −ωz 1 ωx 0 ωz −ωy   (6.20) Υ(ω) =   ω −ω 0 ωx  2 y z ωz ωy −ωx 0 This can also be written according to equation (6.18), giving q˙ = Ξ(q)ω  −q1 −q2 1 q0 −q3 Ξ(q) =  q0 2  q3 q2 q1 (6.21)  −q3 q2   −q1  q0 (6.22) Since the matrix function Υ(ω) is linear one can write the first row of equation (6.19) as d b b e q = Υ(ωib )qeb − Υ(Ceb (qeb )ωie )qeb (6.23) dt e The derivative with respect to the state vector gives   h

∂ d b ∂ ∂ b b b Ξ(qeb )ωib qe = ∂qeb Υ(ωib )qe ... b ∂ωib ∂z dt  (6.24) ∂ b b e b − b (Υ(Ce (qe )ωie )qe ) ∂qe 6.3 Linearization with Quaternion Angle Representation 31 Consider the transformation matrix Ceb that consists of nine elements   c11 c12 c13 Ceb = c21 c22 c23  c31 c32 c33 Expanding the last term in equation (6.24), using (6.20) gives  !   ! 0 c13 e Υ(Ceb (qeb )ωie )qeb = Υ Ceb (qeb )  0  qeb = Υ c23  ωe qeb ωe c33    0 −c13 −c23 −c33 q0 c13  q1  0 c −c 33 23  ω = c23 −c33 0 c13  q2  e c33 c23 −c13 0 q3 Using (6.26), (6.15) and the relation  −2q0 q3  −2q0 q2 ∂ b b e b (Υ(Ce (qe )ωie )qe ) =   2q0 q1 ∂qeb 1 + 2q02 | = (6.25) (6.26) q02 + q12 + q22 + q32 = 1 gives −2q1 q3 −2q1 q2 1 + 2q12 2q1 q0 −2q2 q3 −1 − 2q22 2q2 q1 2q2 q0 {z Θ(qeb )  −1 − 2q32 −2q3 q2  ω 2q3 q1  e 2q3 q0 } (6.27) Θ(qeb )ωe The matrix in (6.27) is replaced by Θ for a more compact notation. The linear quaternion equation can now be derived as
b b ∆q˙eb = Υ(ωib ) − Θ(qeb )ωe ∆qeb + Ξ(qeb )∆ωib (6.28) Linearization of the Velocity Equation The velocity equation can be expressed as: x ¨e = −2Ωeie x˙ e − Ωeie Ωeie xe + Cbe (qeb )f b + g e (xe ) (6.29) The derivative with respect to the state vector gives  ∂ (¨ xe ) = ∂z ∂ e e ˙ ) ∂ x˙ e (−2Ωie x ∂ (Cbe (qeb )f b ) ∂qeb ∂ e e e − ∂x e (Ωie Ωie x ) i ∂ (Cbe (qeb )f b ) ∂f b ∂ e e ∂xe (g (x )) ... (6.30) It is assumed that Ω˙ eie = 0 because of the rotation of the Earth is nearly constant. Remember that Cbe is the transpose of equation (6.15), it gives Cbe (qeb )f b =  2 (q0 + q12 − q22 − q32 )  2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) 2(q1 q2 − q0 q3 ) (q02 − q12 + q22 − q32 ) (q2 q3 + q0 q1 )  2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 )  f b 2 (q0 − q12 − q22 + q32 ) (6.31) 32 Navigation Equations Equation (6.31) is differentiated with respect to the quaternion components giving ∂ (C e (q b )f b ) = ∂qeb b e     q0 −q3 q2 q1 q2 q3 q0 −q1  f b  q2 −q1 −q0  f b . . . 2  q3 −q2 q1 q0 q3 q0 −q1      −q2 q1 q0 −q3 −q0 q1 . . .  q1 q2 q3  f b  q0 −q3 q2  f b  = Λ(qeb , f b ) −q0 q3 −q2 q1 q2 q3 (6.32) The matrix in (6.32) is replaced by Λ(qeb , f b ) for a more compact notation. The gravity model (6.13) is then differentiated with respect to the position xe resulting in   I3 xe (xe )T ∂g e (xe ) = −kM − 3 ∂xe r3 r5    2  ! x1 x1 x2 x1 x3 1 0 0 (6.33) x22 x2 x3  r−5 = −kM 0 1 0 r−3 − 3 x2 x1 x3 x1 x3 x2 x23 0 0 1 | {z } Γ The derivative of the gravity function is replaced by Γ for a more compact notation. With (6.33), (6.32) and (6.30) the linear velocity equation can be written as   ∆¨ xe = − 2Ωeie ∆x˙ e − Ωeie Ωeie − Γ(xe ) ∆xe (6.34) + Λ(qeb , f b )∆qeb − Cbe (qeb )∆f b The Complete Linear Model pressed using (6.28) and (6.34) as The complete linear model can now be ex-   ∆¨ xe = − 2Ωeie ∆x˙ e − Ωeie Ωeie − Γ(xe ) ∆xe + Λ(qeb , f b )∆qeb + Cbe (qeb )∆f b
b b ∆q˙eb = Υ(ωib ) − Θ(qeb )ωe ∆qeb + Ξ(qeb )∆ωib (6.35) b where xe , qeb , ωib is the linearization point. 6.4 Linearization with Euler Angles A different approach is to represent the small deviation angles with Euler angles, it gives a more intuitive angle representation. If the angles are small, the difference between Euler angle representation and quaternion angle representation is minimal, as described in Appendix B.2. This linearization is based on the work of [3]. Consider the equation (6.36) derived from (6.12) and (6.33) x ¨e = −2Ωeie x˙ e − (Ωeie Ωeie − Γe )xe + Cbe f b + g e (6.36) 6.4 Linearization with Euler Angles 33 The transformation matrix Cbe can be written as Cbe = Cbe 0 + ∆Cbe (6.37) where Cbe 0 is the linearization point and ∆Cbe is the deviation from the linearization point. Assume that the deviation from the linearization point can be described by small Euler angles Ψ = (ψ, θ, φ)T . According to [9] the direction cosine matrix A can be written as A = (I + [Ψ×]) (6.38) Where I is an 3 × 3 identity matrix and [Ψ×] is by  0 −ψ 0 [Ψ×] =  ψ −θ φ the skew-symmetric matrix given  θ −φ 0 (6.39) The transformation matrix Cbe can now be expressed as Cbe = ACbe 0 (6.40) Combining (6.40) and (6.37) gives an expression for ∆Cbe (A − I)Cbe 0 = ∆Cbe (6.41) [Ψ×]Cbe 0 = ∆Cbe (6.42) Replacing A with (6.38) gives The acceleration now becomes Cbe f b = ∆Cbe f b 0 + Cbe 0 ∆f b = [Ψ×]Cbe 0 f b 0 + Cbe 0 ∆f b = [Ψ × Cbe 0 f b 0 ] + Cbe 0 ∆f b (6.43) = −[Cbe 0 f b 0 × Ψ] + Cbe 0 ∆f b where ∆f b is the deviation from the linearization point f b 0 . In order to have Ψ as a state in the state space model, its derivative must be derived. Differentiating the relation C˙ be = Cbe Ωeeb gives ∆C˙ be = ∆Cbe Ωeeb + Cbe 0 ∆Ωeeb (6.44) d e ˙e ˙ ([Ψ×]Cbe 0 ) = [Ψ×]C b 0 + [Ψ×]Cb 0 dt (6.45) Differentiating (6.42) gives ∆C˙ be = Setting (6.45) equal to (6.44) and using ∆Cbe = [Ψ×]Cbe 0 and C˙ be 0 = Cbe 0 Ωeeb leads to e e e e e e e ˙ [Ψ×]C b 0 + [Ψ×]Cb 0 Ωeb = [Ψ×]Cb 0 Ωeb + Cb 0 ∆Ωeb 34 Navigation Equations ˙ Solving for [Ψ×] gives ˙ [Ψ×] = Cbe 0 ∆Ωeeb Ceb 0 (6.46) and the corresponding vector equation b ˙ = Cbe ∆ωeb Ψ 0 (6.47) Assume that the output from the gyros can be described by the following relation b b b a b ωib = ωie + ωeb = Cab ωie + ωeb (6.48) b b b b b b Writing (6.48) with ωib = ωib0 + ∆ωib , ωeb = ωeb0 + ∆ωeb and Ceb = Ceb 0 + ∆Ceb gives
e b b e b b (6.49) ∆ωib + ωib0 = Ceb 0 + ∆Ceb (ωie0 + ∆ωie ) + ωeb0 + ∆ωeb b Neglecting second order terms and solving for ∆ωib gives b e e b ∆ωib = ∆Ceb ωie0 + Ceb 0 ∆ωie + ∆ωeb (6.50) Using ([Ψ×]Cbe 0 )T = −∆Ceb with (6.49) results in b e e b ∆ωib = −Ceb 0 [Ψ×]ωie0 + Ceb 0 ∆ωie + ∆ωeb (6.51) One can now derive b ˙ = Cbe ∆ωeb Ψ 0 b e e = Cbe 0 (∆ωib + Ceb 0 [Ψe ×]ωie0 − Ceb 0 ∆ωie ) b e e = Cbe 0 ∆ωib + [Ψ×]ωie0 − ∆ωie (6.52) b e e = Cbe 0 ∆ωib − ωie0 × Ψ − ∆ωie The Complete Linear Model Using (6.52), (6.36) and (6.43), one get b e ˙ = Cbe ∆ωib Ψ − ωie0 ×Ψ 0 ∆¨ xe = −2Ωeie ∆x˙ e − (Ωeie Ωeie − Γe )∆xe − [f e 0 ×]Ψ + Cbe 0 ∆f b (6.53) e Note that ∆ωie = 0 because the rotational rate of the earth is constant. 6.5 Measurement Equation Since the accelerations and angular velocities are measured with bias the estimated position will drift away from the real position as stated in Chapter 5. To estimate how large the bias is, the error they inflict on the estimate must be measured. By doing this one can derive how they propagate relative the estimates with the linear model. In these measurement equations the deviation is considered to be the error introduced by the bias. Consider the state vector to be written as z˜ = z + zerr (6.54) 6.5 Measurement Equation 35 where z˜ is the INS solution with errors inflicted by biases, zerr are the errors and z are the real states. Analogously one can write the state vector as in the linearization z = z0 + ∆z (6.55) The measurement equation can be considered to measure the errors. How to correct them are described in Section 7.4 Measurement Using Position Error Consider the GPS receiver to output the position in the E-frame. Assume that the position can be described as a real value plus an error as described in (6.56) xeGPS = xe + ∆xeGPS (6.56) where ∆xeGP S are the GPS errors. Analogous the position derived by the INS can be written as xeINS = xe + ∆xeINS (6.57) where ∆xeIN S are the errors inflicted by the sensor bias. The measurement equation can then be written as ∆y =xeIN S − xeGPS = xe + ∆xeINS − xe − ∆xeGPS =∆xeINS − ∆xeGPS (6.58) Now the measurement equation is written as if it measures the INS errors. Measurement Using Pseudo Range Error The INS errors can also be measured from the pseudo ranges (satellite distance). Pseudo range ρi to satellite i can be expressed as p ρi = (Xi − xr )2 + (Yi − yr )2 + (Zi − zr )2 + c∆ξ + ρi (6.59)  T Here Xi Yi Zi are the position of satellite i, p (Xi − xr )2 + (Yi − yr )2 + (Zi − zr )2 is the distance to satellite i from the receivers position (xr , yr , zr )T . Linearization around (xr , yr , zr )T gives the unit vector to satellite i: T  ∂ρi ∂ρi ∂ρi ˆ (6.60) , , Si = ∂xr ∂yr ∂zr ∂ρi Xi − xr =p 2 ∂xr (Xi − xr ) + (Yi − y r )2 + (Zi − zr )2 ∂ρi Yi − yr =p 2 ∂yr (Xi − xr ) + (Yi − y r )2 + (Zi − zr )2 ∂ρi Zi − zr =p ∂zr (Xi − xr )2 + (Yi − y r )2 + (Zi − zr )2 (6.61) 36 Navigation Equations The measurement equation can be expressed as ∆ρ = h(Xi , xr )∆z + v       ∂ρ1 ∂ρ1 ∂ρ1    1  ρˆ1 ρ1 ∂xr ∂yr ∂Zi ρ1 δx r  ∂ρ2 ∂ρ2 ∂ρ2 ρ2  ρ2  ρˆ2   1          ∂xr ∂yr ∂Zi  δyr   .    ..  −  ..  =  . .. .. ..    δzr  +   ..   .   .   .. . . . c∆ξ ∂ρi ∂ρi ∂ρi ρi ρˆi ρi 1 ∂xr ∂yr ∂Zi (6.62) (6.63) where ρˆi are the pseudo ranges estimated by the INS, ρi are the pseudo ranges measured from the GPS receiver, ρi is the measurement noise and c∆ξ is the clock error multiplied by the speed of light. Chapter 7 The Extended Kalman Filter The Kalman filter has been extensively used in several signal processing areas since its creation in the sixties. One interesting characteristic of the filter is the fact that it can use several signals and combine them in an optimal fashion. This is what sensor fusion is based upon. The Kalman filter theory requires linear systems, but the filter can also however be used with nonlinear systems. There are two choices when using a Kalman filter with a nonlinear system. One is by using the Linearized Kalman filter (LKF) where the linearization point is an input signal to the LKF. If the real point and the linearization point are too far a part then there is a risk that the linearization will not be valid. The other choice, which is used in this thesis, is the Extended Kalman filter (EKF). The largest difference between LKF and EKF is that the linearization in the latter method is around a state vector estimated by the filter. This means that the linearization takes place around the filter estimate zˆk|k−1 and that linearization is always valid. 7.1 Discretization of a Linear System Since the model is going to be used in a Kalman filter it needs to be discretizised. This discretization is based upon the example in [25]. Consider the linear continuous time system z(t) ˙ = F (t)z(t) + G(t)w(t) y(t) = H(t)z(t) + v(t) (7.1) The solution to the state equation can be written as z(t) = e F (t0 )(t−t0 ) Zt z(t0 ) + t0 37 eF (s)(t−s) G(s)w(s)ds (7.2) 38 The Extended Kalman Filter To discretizise (7.2) the solution for each sample must be calculated. Consider the two discrete time instants tk and tk+1 where tk = kT and  tk+1 = (k + 1)T . T is the sampling period and k is a discrete time series k = 1 2 3 · · · N . The solution for z(tk+1 ), tk ≤ t < tk+1 can then be written as z(t) = e F (tk )(t−tk ) Zt z(tk ) + eF (s)(t−s) G(s)w(s)ds (7.3) tk t0 is chosen to the start of the sampling period (tk ) and t is chosen to end (tk+1 ). eF (tk )(tk+1 −tk ) is Taylor-expanded and truncated to the first order in order to make the first term linear as in eF (tk )(tk+1 −tk ) ≈ I + F (tk )(tk+1 − tk ) (7.4) (7.4) is replaced by A(tk+1 , tk ) for a more compact notation. The equation can now be written as tZk+1 z(tk+1 ) = A(tk+1 , tk )z(tk ) + A(tk+1 , s)G(s)w(s)ds tk | {z e(tk ) (7.5) } The measurement equation is discretizised by replacing t with tk , tk = kT , which gives the measurement in (7.1) as y(tk ) = H(tk )z(tk ) + v(tk ) 7.2 (7.6) Discretization of a Nonlinear System Consider the nonlinear continuous time system z(t) ˙ = f (z(t), u(t), w(t)) y(t) = h(z(t), u(t), v(t)) (7.7) The solution to the state equation can be written as Zt z(t) = z(t0 ) + f (z(t), u(t), w(t))dt (7.8) t0 t0 is the start of the sampling period (tk ) and t is chosen as the end point of the sampling period (tk+1 ) in equation (7.8) tZk+1 z(tk+1 ) = z(t0 ) + f (z(t), u(t), w(t))dt (7.9) tk The measurement equation is discretizised by replacing t with tk y(tk ) = h(z(tk ), u(tk ), v(tk )) (7.10) 7.3 Extended Kalman Equations 7.3 39 Extended Kalman Equations Consider the nonlinear discrete-time state-space model zk+1 = f (zk , uk , ek ) (7.11) yk = h(zk , vk ) (7.12) Here zk are the states, uk are known input signals to the system, ek is the process noise, yk are the measurement signals and vk is the measurement noise. The EKF can as found in [12] be expressed as zˆk+1|k = f (ˆ zk|k , uk ) Pk+1|k = Sk = Kk = Ak Pk|k ATk (7.13) + Qk Hk Pk+1|k HkT + Pk+1|k HkT Sk−1 (7.14) Rk (7.15) (7.16) εk = yk − Hk zˆk+1|k (7.17) zˆk+1|k+1 = zˆk+1|k + Kk εk (7.18) Pk+1|k+1 = Pk+1|k − Pk+1|k HkT Sk−1 Hk Pk+1|k (7.19) where   ∂f (ˆ zk|k , uk ) ∂z   ∂h (ˆ zk+1|k ) Hk = ∂z Ak = (7.20) (7.21) Qk = E[ek eTk ] (7.22) E[vk vkT ] (7.23) Rk = One can see that every matrix is computed from states estimated by the filter. Ak is the system matrix which describes the dynamics of the system, Gk is the matrix which describes how the process noise affects the system, Qk is the covariance matrix for the process noise, Rk is the covariance matrix for the measurement noise and P is the covariance matrix for the state vector z. Calculating Ak As in equation (7.4) A(tk+1 , tk ) = I + F (tk )(tk+1 − tk ) where  F (t) = ∂f (ˆ z (t), u(t)) ∂z (7.24)  (7.25) Calculating Qk By the nature of the EKF, Qk Rmust be calculated in every dist crete time instant. Using (7.22) and the relation tkk+1 A(tk+1 , s)G(s)w(s)ds = ek 40 The Extended Kalman Filter one get E[ek eTk ] tZk+1 tZk+1 A(tk+1 , s)G(s)E[w(s)w(r)T ]G(r)T A(tk+1 , r)T dsdr = tk (7.26) tk First the expected value E[w(s)w(r)T ] is simplified. The covariance of a scalar time-continuous white noise process n(t) with zero mean is E[n(r)n(r0 )T ] = σn2 δ(r − r0 ) (7.27) where σn2 is constant power spectral density of the process, r and r0 are two different moments in time where r0 < r and δ(t) is the Dirac delta function. Because the vector w(t) is independent white noise, this implies that E[w(r)w(r0 )T ] = Q0 δ(r − r0 ) (7.28) where Q0 is a diagonal matrix with power spectral densities of the processes w(t) The simplified equation can now be written as E[ek eTk ] tZk+1 tZk+1 A(tk+1 , s)G(s)Q0 G(r)T A(tk+1 , r)T dsdr = tk (7.29) tk A and G is now assumed to be constant between tk and tk+1 . This implies E[ek eTk ] = A(tk+1 , tk )G(tk )Q0 G(tk )T A(tk+1 , tk )T (tk+1 − tk ) (7.30) Calculating Rk Rk is, as stated before, the covariance matrix for the measurement noise. Rk is usually modeled by a diagonal matrix with the power spectral densities of the process v(t) as in   2 v(t)1 E[v(t)1 ]    v(t) =  ...  , Rk =   v(t)n 7.4 0 .. 0 .    (7.31) 2 E[v(t)n ] The Measurement Update The measurement update is based upon the work in [17]. The deviation from the linearization states are the linearization states with the exact state subtracted ∆zk = z0k − zk (7.32) The state estimate zˆ can then be computed as zˆk = z0k − ∆ˆ zk (7.33) 7.5 Implementation 41 The prediction of z can then be written as zˆk+1|k = z0k+1 − ∆ˆ zk+1|k (7.34) The measurement equation for the deviation states is ∆ˆ zk+1|k+1 = ∆ˆ zk+1|k + Kk ∆yk+1 − Hk ∆ˆ zk+1|k
(7.35) As in Section 7.1 we know that ∆y = xeIN S − xeGP S . Where xeIN S = h(z0 ). One can then write ∆yk = h(z0k ) − yk (7.36) Putting (7.36) in (7.35) gives
∆ˆ zk+1|k+1 = ∆ˆ zk+1|k + Kk h(z0k+1 ) − yk+1 − Hk ∆ˆ zk+1|k
= ∆ˆ zk+1|k + Kk h(z0k+1 ) − Hk ∆ˆ zk+1|k −yk+1 | {z } ≈h(z0 k+1 −∆ˆ zk+1|k ) ≈ ∆ˆ zk+1|k + Kk h(z0k+1 − ∆ˆ zk+1|k ) − yk+1
= ∆ˆ zk+1|k + Kk h(ˆ zk+1|k ) − yk+1 (7.37)
If z˜ is subtracted from both sides and using (7.34) results in zˆk+1|k+1 = zˆk+1|k + Kk yk+1 − h(ˆ zk+1|k ) 7.5
(7.38) Implementation This section describes how to implement the Extended Kalman filter with the two linearization methods described in Chapter 6. Conversion between WGS84 and ECEF Since the sensor fusion is done in the ECEF frame the geodetic coordinates from the GPS receiver must be converted to ECEF coordinates as described with the following relations q χ = 1 − e2 sin2 φ   a X= + h cos φ cos λ χ   (7.39) a Y = + h cos φ sin λ χ   a(1 − e2 ) Z= + h sin φ χ where φ is latitude, λ is longitude, a is earth radius, e is earth eccentricity and h is height over the mean sea level in meters. 42 The Extended Kalman Filter Nonlinear State Vector In both implementations the same state vector is used, described by  b qe x˙ e   e  (7.40) z= x  dg  da qeb is the quaternion rotation from ECEF to the Body-fixed frame, xe and x˙ e is the position and velocity in the ECEF frame, dg are the gyro bias and da are the acceleration bias. EKF with Quaternion Linearization Implementing the EKF with the quaternion angle representation works as a normal Extended Kalman filter   b b b Υ(ωib ) − Θ(qe )ωe Λ(qeb , f b ) 0 0 0   Ak = I(16 × 16) +    Hk = 0 0 I 0 0 0 −2Ωeie I 0 0 0 −Ωeie Ωeie + Γ(xe ) 0 0 0 Ξ(qe ) 0 0 0 0 0 Cbe (qeb )  0  ∆t 0  0  (7.41) The gyro noise is considered to be added directly to the quaternion components. This approach is more insensitive to model errors. The acceleration noise however is transformed with Cbe and added to the velocity state.   I(4 × 4) 0 0 Cbe  Gk =  0 0   (7.42) nq I(4 × 4) 0 0 Q = 0 na I(3 × 3) Qk = Ak Gk Q0 GTk ATk ∆t The quaternion process noise spectral densities is denoted by nq and na are the accelerometer noise spectral densities. The covariance matrix Rk is approximated with a diagonal matrix with the spectral densities of v(t). EKF with Euler Linearization A different approach must be taken here since the linearization has the angle deviation expressed in Euler angles as states instead of quaternion components, thus giving it 15 linear states. Note that the EKF assumes that the linear state vector is the same as the nonlinear state vector, this makes this implementation different from any standard EKF. In order to do this implementation a few changes must be made to the standard EKF procedure. The implementation of the EKF considers the nonlinear state vector to have Euler angles instead of quaternion components, to make the EKF compatible with the 7.5 Implementation 43 nonlinear state vector as described in (7.40) the measurement equation must be changed as explained later in this section. Meanwhile the EKF is implemented with the Euler angle state vector in mind.   −Ωeie 0 0 Cbe 0 0 −[f e 0 ×] −2Ωe −Ωe Ωe + Γ(xe ) 0 Cbe 0  ie ie ie    0 I 0 0 0  Ak = I(15 × 15) +   ∆t (7.43)  0 0 0 0 0  0 0 0 0 0   Hk = 0 0 I 0 0 G is modeled by regarding the gyro and acceleration sensors to be the source of the process noise.  e  Cb 0 Gk =  0 Cbe  0 0   (7.44) n I(3 × 3) 0 Q0 = g 0 na I(3 × 3) Qk = Ak Gk Q0 GTk ATk ∆t where ng are the gyro noise spectral densities and na are the accelerometer noise spectral densities. According to [17] Ak can be approximated by an identity matrix for small ∆t in this implementation. The main difference with this approach is the measurement update. Because the angle deviation is represented in Euler angles the quaternion must be updated with the Euler angle deviation. If the angle deviations are small one can write the conversion between the quaternion and Euler angles as     q0 1 q1   θ/2   ≈  (7.45) q2   φ/2  q3 ψ/2 The Euler angles are written as       θ θ0 ∆θ  φ  = φ0  +  ∆φ  ψ ψ0 ∆ψ The quaternion can now be written as       q0 1 q0 0 q1   ∆θ/2  q1 0   =    q2   ∆φ/2  ~ q2 0  q3 ∆ψ/2 q3 0 (7.46) (7.47) 44 The Extended Kalman Filter The deviations are multiplied from the left because of the quaternion behavior with multiple rotations. q1 ~ q2 means that the first rotation is done by q2 , the second by q1 . The measurement update for the full state vector is written as
zˆk+1|k+1 = zˆk+1|k + Kk yk+1 − h(ˆ zk+1|k ) (7.48) Consider that the state vector can be split into   qˆ zˆk = k0 zˆk (7.49) where qˆk is the quaternion part of the state vector and zˆk0 are the remaining states. Kk can then be split as   KΨk Kk = (7.50) Kz0 k where KΨk is the three first rows of Kk and Kz0 k are the remaining rows. The measurement equation can now be written as # "
T zk+1|k ) 1 12 KΨk yk+1 − h(ˆ ~ qˆ
k+1|k (7.51) zˆk+1|k+1 = 0 zˆk+1|k + Kz0 k yk+1 − h(ˆ zk+1|k ) where ~ is the quaternion multiplication. See Appendix A for quaternion characteristics. Time Updates and Measurement Updates In this implementation the EKF is updated when new GPS measurements are availible typically; 4 Hz. Between the GPS measurements the state prediction is computed by integrating the navigation equation at a sample rate of 100 Hz. During the prediction the bias estimates are used to correct the IMU. The quaternion is normalized in every update by dividing it with the quaternion norm. The prediction of P is done when new GPS data is available. First the model is linearized by calculating A(tk+1 , tk ) where tk is the last time GPS data was availible and tk+1 is the current time, second the P matrix is predicted with the same matrix A(tk+1 , tk ). One could also do the prediction of P at the same rate as the integration of the navigation equations, however this results in more computational burden. Chapter 8 Hardware Used for Testing and Verification of the System To develop and validate an GPS INS system one would need certain tools and data. First of all one need signals from several satellites and corresponding accelerations, which portraits the launch of a rocket in a correct manner. This can be achieved in two ways. You could either place a GPS antenna and IMUs on a rocket and retrieve and store the data for post-processing. This might be the best solution but is not really feasible if one considers the cost. An alternative is to use a GPS simulator which simulates the entire GNSS constellation with its 31 satellites and then simulate a rocket launch to retrieve corresponding satellite signals. With a simulator you could also create difficult scenarios and test the receiver’s limits. To simulate a true rocket launch logged data such as acceleration, velocity and position are needed from an earlier launch of a rocket. Saab Space has logged data from earlier launches of a MAXUS rockets that could be used for this purpose. The acceleration data can bee seen in Figure 8.3. 8.1 Proposed Hardware This section introduces hardware that is used in the development of the EKF, but also hardware that could be used to analyze a complete solution to the navigation issue. This includes developing a GPS receiver capable of withstanding high accelerations. Xsens MTi-G When developing the EKF used in the system proposed in this thesis one needs GPS positions and the corresponding IMU data. To attain this data a MTi-G from Xsens, as can bee seen in Figure 8.1, is used. The MTi-G is a combined IMU and GPS sensor with a USB interface that makes it easy to log data for post-processing. The sensor has a Kalman Filter used for sensor fusion 45 46 Hardware Used for Testing and Verification of the System but because of its poor performance in off-line mode, it is not used as a reference when evaluating the sensor fusion solution prposed in this thesis. Figure 8.1. [15] The MTi-G sensor from Xsens. A combined IMU/GPS with integrated sensor fusion used in this thesis to attain GPS and corresponding IMU signals. These signals are used for developing the proposed navigation solutions found in this thesis. Nordnav R30 Nordnav R30 is a complete GNSS software receiver package from Nordnav [2]. The receiver emulates hardware, such as correlators, to create a complete unified software solution. This means that one can modify the receiver in many ways which would not be possible if it would have been regular hardware. With the help of several development APIs included in the package and the open architecture, one can modify certain parts of the receiver without the need of redoing all other parts of the software. Without some sort of SDK, one must write a complete software solution for the receiver because one part needs modification. These characteristics of the receiver makes it ideal for development and testing of new solutions for GNSS receivers. Spirent GSS 7700 Spirent GNSS simulators recreate GPS RF signal environment based on test scenarios defining receiver position, environment, motion and other effects at a given time. The newest model of Spirent’s GNSS simulator series is called GSS 7700 (see Figure 8.2). This simulator allows one to, with the use of real positioning data from earlier launches, to recreate GPS signals. The recreated GPS signals are the same signals as the rocket would have received during the flight if a GPS receiver had been used. One can then analyze and evaluate how the GPS behaves when introduced to a fast changing GPS signal due to the high dynamics. This will also help when analyzing how the produced solution should behave in a real test. The Proposed High Dynamics Environment The proposed environment for the system and the trajectory data is based on launched MAXUS rockets. The rocket program MAXUS was initiated in 1989 with its first successful launch from Esrange in November 1992 [6]. The purpose of the MAXUS program was to increase the mission time for experiments in micro gravity. The mission related tasks are handled by the joint venture formed by EADS-ST and the Swedish Space Corporation (SCC). The MAXUS is a one stage rocket and allows 12-13 minutes 8.2 A Complete Integration Test of the System 47 Figure 8.2. [14] Spirent GSS 7700 signal generator. It is used for simulating signals from the entire GNSS constellation. of micro gravity time for experiments. Since the start seven successful MAXUS launches have been made. More information about the MAXUS rocket can be found below in Table 8.1. Table 8.1. The table presents some important characteristics of the Maxus rocket used in micro-gravity experiments. Parameter Overall length Max. diameter Micro gravity time Apogee Peak acceleration Burn time Spin rate 8.2 Value 16.2 m 1m 12 minutes 30 s 705 km 13 g 64 s ≤ 0.5 Hz A Complete Integration Test of the System In Chapter 9 the results from the test of the proposed EKF are presented. In order to verify the complete system, one would need further testing. The sensor fusion solution (in this thesis the EKF) is tested in a route which includes different scenarios such as, increase/decrease of speed and sharp turns. This test is a good starting point for the final integration test, but before that test, the GPS needs to be verified. In order to verify the GPS receiver one needs logged or simulated data from earlier launches of sounding rockets. The best case scenario is to possess acceleration and GPS data from a real launch. If this is not possible, one at least needs acceleration data from earlier launches in order to create corresponding GPS data with the help of a GPS signal generator (for example the GSS 7700). With acceleration data and corresponding GPS data one can perform the final integration test which will essentially test if the complete solution is suitable for use in sounding rocket environments. 48 Hardware Used for Testing and Verification of the System 250 Acceleration (m/s2) 200 150 100 50 0 −50 −400 −200 0 200 400 Seconds 600 800 1000 1200 Figure 8.3. Acceleration data from the flight of Maxus 7. The acceleration is about 130 m/s2 at the end of the burn time. At 850 seconds one can see an even bigger acceleration step. This is due to the deceleration when re-entring the atmosphere. Between the burn time and the re-entry the acceleration is nearly zero. This phase of the sounding rocket is called micro-gravity. Chapter 9 Results This chapter presents the results from the evaluation of the EKF using linearization of quaternions and proposes the work that in continuation can be done with this thesis. 9.1 Test Procedure The test data used in evaluating the EKF is collected with the help of a MTi-G sensor from Xsens (see Section 8.1). The sensor is attached to a car and subject to several different scenarios. The scenarios include sharp turns (roundabouts), rapid speed changes (getting on and off an expressway) and normal driving through population centers (less than 50 km/h). The data is recorded in an area called Ryd in Linköping, Sweden and the route used in this thesis is shown in Figure 9.1. In the evaluation the proposed EKF using linearization of quaternions is used. An EKF using Euler angles has also been developed and implemented (as described in Section 7.5). The results of the two EKFs are very similar although their implementation is very different, presenting one of the filters is enough. One explaniation of the similar results is that the data recorded is very low dynamic compared to a rocket launch. The EKF is evaluated by the following procedure; comparing the estimated quaternions with the driftless quaternions from the sensor, examining how much the EKF differs from the raw GPS data in position, calculating the confidence interval in parts of the test route, comparing the velocity of the sensor to the estimated velocity from the EKF, and finally, seeing how the EKF performs if a short GPS outage occurs. The data used as reference in the different comparisons are not completely true as they contain errors but they are good enough to show the characteristics of the proposed EKF. 49 50 Results Figure 9.1. A bird’s view of the route used in evaluating the EKF proposed in this thesis. The plotted path is the output from the EKF. The length of the entire sequence is 500 s. It starts by standing still 50 s, then driving trough two roundabouts 50 s to 100 s. At 100 s the vehicle enters the expressway seen as the long straightaway. Between 200 s and 300 s the vehicle drives through the big roundabout. By the time of 300 s the vehicle enters the expressway in the other direction, at 400 s the vehicle exits. After exiting the expressway the vehicle drives through the two roundabouts and turn in to the starting point. 9.2 Filter Tuning The Kalman filter is tuned according to sensor specifications supplied by the factory and the results of multiple trial and error simulations. The final parameters are presented in table 9.1 and 9.2. The number in [ ] corresponds to how many elements there are in each vector which also have the same value. The qeb has the initial standard deviation of 0.3 because of the initial state is attitude is taken from the sensors estimate. Because the initial position is taken from the GPS receiver the initial standard deviation is the GPS resolution of 10 m. The initial velocity standard deviation is set to 0.1 since the velocity is estimated directly by the MTI-G. The gyro and accelerometer biases is set to a typically order of size of the biases. The filter has proved to be very sensitive of initial bias standard deviation, setting the starting value to the same order of size as the bias should however be 9.3 Estimating Position 51 enough to ensure a good estimate. The spectral density of the process noise is set to specifications of the MTi-G. The noise of the quaternions is approximated to be the same order of size as the gyro noise which according to specification is 0.006. The accelerometer noise is set to specification 0.01. The R matrix is set to a diagonal matrix with the magnitude of 16 m, which means that the standard deviation of the GPS measurement is 4 m. Table 9.1. Square root of diagonal of P0 p E[(qeb )2 ][4] 0.3 p E[(x˙ e )2 ][3] p E[(xe )2 ][3] 0.1 10 p E[(dg )2 ][3] 0.01 p E[(da )2 ][3] 0.1 Table 9.2. Square root of diagonal of Q0 √ nq [4] 0.007 9.3 √ na [3] 0.01 Estimating Position One can see in the following figures that the EKF performs well when estimating the position. The route used for both scenarios is part of the large roundabout shown in Figure 9.1. Figures 9.2 and 9.3 shows the confidence interval (circles) for two parts of the driven route and the output of the EKF (the line); one for the start (and end) position and one for the roundabout. At first, as seen in Figure 9.2, the confidence interval is large at the start but rapidly decreases to a level that is almost constant for the remainder of the course. If one look at Figure 9.3 there is a circle with a radius slightly larger than the rest of the circles. This is due to a loss of GPS position thus making the EKF more uncertain about its position. Figures 9.4,9.5 and 9.6 show the difference in position between the EKF output and the GPS data the E-frame. Figure 9.7 shows the total difference between EKF output and GPS data. One can see that the difference in position is larger throughout all three figures when driving on the expressway due to higher velocity typically 110 km/h on the expressway. 9.4 Attitude Estimation Performance The attitude estimation performance was validated against the Xsens MTi-G which is described in Section 8.1. The attitude precision of the MTi-G is 0.50 52 Results in pitch and roll and 10 in yaw referring to Euler angles. The error plots in Figure 9.8 are done by subtracting the attitude calculated by the MTI-G from the estimated attitude by the EKF. The 3σ limit shows that under the assumption that the errors are normally distributed one can tell that there is a 99.7 % chance that the real value is within the 3σ limit. The size of the 3σ limit of quaternion component q0 and q3 are larger than the other two components. q0 and q3 have proven to be the hardest to estimate through the simulations. These shaky estimates is probably due to the mems gyro performance of the MTi-G. Using the tactical grade Northrop Grumman LN-200 IMU as in the supposed application for this filter should yield better results due to lower noise and better precision. 9.5 Velocity Estimation Performance The velocity estimation performance is validated against the Xsens MTi-G which is described in Section 8.1. The error plots in Figure 9.9 are computed by subtracting the velocities estimated by the MTI-G from the estimated velocities by the EKF. One can draw the conclusion that the velocity estimation accuracy is approximately 5 m/s. Using the LN-200 should yield much better results here as well. 9.6 Bias Estimation Performance Since no reference signal is available in the bias estimation, one can only look at the 3σ limit and convergence time in Figure 9.10. One can see that it takes approximately 250 s before the gyro bias estimation converges to a stable value. Here one can also see the typical order of size of the bias. The yaw bias seems to be the hardest bias to find, but when the vehicle starts to move at 50 s the 3σ limit decreases and eventually converge at 250 s. The accelerometer bias standard deviation does however only slightly change over time as seen in figure 9.11, especially not in Z-direction. One could relate this to the fact that the Z-accelerometer is not exposed to so large or any acceleration dynamics during the test run. The EKF may then have some trouble to find the correct bias. One can also relate this phenomenon to the yaw bias estimation in Figure 9.10 where the 3σ limit does not change until the vehicle starts to turn at 50 s. One should note that there are some facts that are not taken into account such as temperature compensation and scale factors when estimating the bias. 9.7 GPS interruption Because the GPS is sensitive to signal blockage and interference, it sometimes loses lock to satellites. This can lead to GPS navigation failure, which means that the EKF will have to manage without the GPS updates under the interruption. Because the bias estimates are always fed back to the INS this helps to decrease the 9.7 GPS interruption 53 error when the GPS fails. Figure 9.12 show a short GPS interruption of 10 s. The velocity during this outage is approximately 70 km/h. The diamond marks the end and start of the interruption on the estimated path, the asterisk marks the end and start of the interruption on the GPS path. The distance between the diamond and the asterisk in the end of the interruption is approximately 27 m. The circles represent the confidence area of 1σ which means under the assumption that the errors are normally distributed that there is a 68 % chance that the exact position is within the circle. As one can see the circles grow just after the interruption but decreases in size when GPS data is availible again. The performance under the GPS interruptions are mainly dependent of the bias estimates and the IMU itself. 54 Results 4 x 10 −1.9 −1.905 Meters −1.91 −1.915 −1.92 −1.925 −100 −50 0 50 Meters 100 150 Figure 9.2. The confidence interval at the start and end of the driven route. Here a portion of the starting point is shown from figure 9.1. The start- and the end-point are the same in this driving scenario which can be seen in the middle of the largest circle. The large circle represents the 1σ starting confidence area. The circles then rapidly decreases in size to a radius of 4 m 9.7 GPS interruption 55 4 x 10 −1.57 Meters −1.58 −1.59 −1.6 −1.61 −1.62 1500 1550 1600 1650 1700 1750 1800 Meters 1850 1900 1950 2000 Figure 9.3. The confidence areas at the large roundabout in the end of the expressway The driving direction is anti-clockwise in this figure. If one look closely two of the circles appear to be bigger than the rest, this is due to outage while driving under the crossing road. 56 Results 7 6 5 Meters 4 3 2 1 0 0 0.5 1 1.5 2 2.5 Sample No. 3 3.5 4 4.5 5 4 x 10 Figure 9.4. The figure shows the difference in meters for the x-axis (in the ECEF system) between GPS data and the output of the EKF. Here one can see that the EKF solution approximately stays within 6 m of the GPS position in the x-axis. The difference is larger during the driving on the expressway (Sample No. 1 · 104 to 2 · 104 and 3 · 104 to 4 · 104 ) due to the higher velocity. 7 6 5 Meters 4 3 2 1 0 0 0.5 1 1.5 2 2.5 Sample No. 3 3.5 4 4.5 5 4 x 10 Figure 9.5. The figure shows the difference in meters for the y-axis (in the ECEF system) between GPS data and the output of the EKF.Here one can see that the EKF solution approximately stays within 6 m of the GPS position in the y-axis. The difference is larger during the driving on the expressway (Sample No. 1 · 104 to 2 · 104 and 3 · 104 to 4 · 104 ) due to the higher velocity. 9.7 GPS interruption 57 4.5 4 3.5 3 Meters 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 Sample No. 3 3.5 4 4.5 5 4 x 10 Figure 9.6. The figure shows the difference in in meters for the z-axis (in the ECEF system) between GPS data and the output of the EKF.Here one can see that the EKF solution approximately stays within 4 m of the GPS position in the z-axis.The difference is larger during the driving on the expressway (Sample No. 1 · 104 to 2 · 104 and 3 · 104 to 4 · 104 ) due to the higher velocity. 9 8 7 6 Meters 5 4 3 2 1 0 0 0.5 1 1.5 2 2.5 Sample No. 3 3.5 4 4.5 5 4 x 10 Figure 9.7. The figure shows the total difference in meters (in the ECEF system) between GPS data and the output of the EKF. Here one can see that the EKF solution approximately stays within 8 m of the GPS position in total distance. The difference is larger during the driving on the expressway (Sample No. 1 · 104 to 2 · 104 and 3 · 104 to 4 · 104 ) due to the higher velocity. If one look closely between Sample No. 2 · 104 and 3 · 104 two spikes is visible which also can be seen in Figure 9.4, 9.5 and 9.6 they are due to the GPS outage which occur when driving under the crossing road. 58 Results Quaternion component q0 estimation error 1 Estimation error 3 σ limit 0.8 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 0 50 100 150 200 250 time (s) 300 350 400 450 500 Quaternion component q1 estimation error 1 Estimation error 3 σ limit 0.8 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 0 50 100 150 200 250 time (s) 300 350 400 450 500 Quaternion component q2 estimation error 1 Estimation error 3 σ limit 0.8 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 0 50 100 150 200 250 time (s) 300 350 400 450 500 Quaternion component q3 estimation error 1 Estimation error 3 σ limit 0.8 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 0 50 100 150 200 250 time (s) 300 350 400 450 500 Figure 9.8. Quaternion error estimates. One can see that q0 and q3 have higher 3σ limit than the rest of the quaternion components. These two components have proven to be the hardest to estimate through the simulations. Other than that the precision is approximately ± 0.1 to ± 0.2 referring to the 3σ limit. 9.7 GPS interruption 59 ECEF X velocity estimation error (m/s) 20 ECEF X velocity estimation error 3 σ limit 15 10 5 0 −5 −10 −15 −20 0 50 100 150 200 250 time (s) 300 350 400 450 500 ECEF Y velocity estimation error (m/s) 20 ECEF Y velocity estimation error 3 σ limit 15 10 5 0 −5 −10 −15 −20 0 50 100 150 200 250 time (s) 300 350 400 450 500 ECEF Z velocity estimation error (m/s) 20 ECEF Z velocity estimation error 3 σ limit 15 10 5 0 −5 −10 −15 −20 0 50 100 150 200 250 time (s) 300 350 400 450 500 Figure 9.9. Velocity error estimates. One can see that the velocity errors are very similar in every direction. One can draw the conclusion that the velocity precision is approximately 4 m/s referring to the 3σ limit. 60 Results Roll gyro bias estimation (rad/s) 0.04 Roll gyro bias estimation 3 σ limit 0.03 0.02 0.01 0 −0.01 −0.02 −0.03 0 50 100 150 200 250 time (s) 300 350 400 450 500 Pitch gyro bias estimation (rad/s) 0.03 Pitch gyro bias estimation 3 σ limit 0.02 0.01 0 −0.01 −0.02 −0.03 −0.04 0 50 100 150 200 250 time (s) 300 350 400 450 500 0.04 Yaw gyro bias estimation 3 σ limit Yaw gyro bias estimation (rad/s) 0.03 0.02 0.01 0 −0.01 −0.02 −0.03 −0.04 0 50 100 150 200 250 time (s) 300 350 400 450 500 Figure 9.10. Gyro bias estimation. Here one can see that the time it takes for the bias values to converge is about 250 s. The hardest bias to find is the yaw bias, which seems to have the same 3σ limit until the vehicle starts to move at 50 s. 9.7 GPS interruption 61 X accelerometer bias estimation (m/s2) 0.8 X accelerometer bias estimation 3 σ limit 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 0 50 100 150 200 250 time (s) 300 350 400 450 500 Y accelerometer bias estimation (m/s2) 0.8 Y accelerometer bias estimation 3 σ limit 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 0 50 100 150 200 250 time (s) 300 350 400 450 500 Z accelerometer bias estimation (m/s2) 1 Z accelerometer bias estimation 3 σ limit 0.8 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 0 50 100 150 200 250 time (s) 300 350 400 450 500 Figure 9.11. Accelerometer bias estimation. Here one can see that the accelerometer bias has the same 3σ limit value until the vehicle starts to move at 50 s. The zaccelerometers 3σ limit does only change slightly during the simulation. This behaviour is also seen in Figure 9.10 where the yaw bias is not found until the vehicle starts to turn. The z-accelerometer however is exposed to very little acceleration during the simulation. 62 Results 2000 NWU Y 1950 1900 1850 1800 3380 3400 3420 3440 3460 NWU X 3480 3500 3520 Figure 9.12. GPS interruption. Here a portion of the roundabout in Figure 9.3 is shown but with a 10 s GPS outage. The driving direction in this figure is anti-clockwise. The GPS outage starts where the asterisk and the diamond intersects each other in the lower part of the figure. The EKF then only relies on the IMU, and one can see that the position starts to drift away from the GPS (dashed line). When the GPS outage ends, the EKF estimates the position to be the diamond in the upper part of the figure, when the GPS position is the asterisk in the upper part of the figure. The velocity during this outage is approximately 70 km/h and the distance between the GPS position and the estimated position by the EKF in the end of the outage is 27 m. Chapter 10 Conclusions and Future Work This chapter presents the conclusions found when working with the implemented EKFs and tracking loops and the future work needed in order to create a complete navigation solution for use in high dynamics environment. 10.1 Conclusions In this section the conclusions of creating an implementing two different extended Kalman filters and of using a Nordnav R30 software GPS receiver with a FrequencyLock Loop (FLL) assisted Phase-Lock Loop (PLL) are presented. EKF Aiding the intertial navigation system with global positioning system (GPS) measurements increases navigation performance. Using just inertial navigation with the MTi-G sensor from Xsens, described in section 8.1, results in immediate attitude loss and position drift. With GPS measurements a good attitude and position estimate is maintained. The loose integration method with quaternion and Euler angle linearization has been implemented. The reference frame is chosen to the Earth Centered Earth Fixed frame in order to simplify a tight integration with the GPS. During the simulations and verification of the quaternion implementation it was observed that independent noise on each quaternion component proved to be better than to consider the noise to be coming from the gyro. The EKF with the quaternion linearization has been compared to the driftless attitude of the sensor, the velocity estimate of the sensor and a comparison with the GPS trajectory has also been done. The results show that the attitude estimated by the EKF is consistent with the attitude estimated by the Xsens MTI-G. The velocity estimated was also very similar to the velocity estimated by the MTI-G. One could also see in the results that the EKF need a long time to converge (250 s in the case study). The EKF can also handle GPS interruption and dead reckons under those GPS outages. Using bias feedback, the performance increased dur63 64 Conclusions and Future Work ing the interruptions. Many estimated states of the EKF have a high estimated standard deviation, this can probably be solved by using a better inertial measure unit(IMU) such as the LN-200 sensor which is the IMU used by the S19 guidance system. GPS Receiver A FLL assisted PLL tracking loop is implemented in a Nordnav R30 software receiver. The filter of the tracking loop is designed with the help of the tracking loop design presented in [11]. The implemented tracking loop is able to maintain lock with GPS data that is recorded in a low dynamic situation. The receiver has not yet been tested with high dynamic GPS data and thus, has yet to be proven to work in a sounding rocket scenario. To verify such a scenario, one would need to use a GPS signal generator and create GPS data featuring acceleration steps with differentiating jerks. 10.2 Future Work Extended Kalman Every time the quaternion is updated in the EKF it is normalized. As of now, no normalization is done to the P matrix. A possible solution would be to measure the quaternion norm in order to normalize the P matrix. The EKF is not validated against a reference trajectory other than the interpolated GPS trajectory. The EKF should be validated against a reference INS with good precision. The GPS does output the dilution of precision (DOP) depending on satellite positions. These DOP-readings could be used to calculate the covariance matrix of the measurement noise. The EKF was constructed with the ECEF frame as reference frame in order to make use of the pseudo range readings from the GPS. This however could not be tested since the GPS available could only output geodetic coordinates. Tracking Loops Tracking loops capable of high dynamics has to be constructed. The presented design for tracking loops found in [11] could be used a reference design, but they need thorough testing to see exactly where the limits are for acceleration and jerk stress. If these test turn out to be successful one could investigate how to create an even better loop design with the help of a pure digital loop design such as the one used in [23]. In order to develop and evaluate a GPS tracking loop, one would need a proper test environment. Developing and verifying an extensive Simulink model of the tracking loop would greatly help in developing a GPS solution. If this can be done, one could easily test different tracking loops before implementing them in a software receiver. 10.2 Future Work 65 Testing a Complete Solution In order to test a complete solution one would need to create tests that integrates the high dynamics data of a rocket, a GPS receiver and a sensor fusion solution. In this thesis we suggest that this is done by using recorded acceleration data, a GPS signal generator and a easily modified GPS receiver (like the Nordnav R30). A Simulink model of a tracking loop could also be used here, instead of the Nordnav R30, in order to simplify the integration of GPS and INS data, the sensor fusion and tracking loops. Bibliography [1] Greenberg A. Gpl-gps. URL:http://gps.psas.pdx.edu/, April 2008. [2] Nordnav Technologies AB. Nordnav-r30 package. navtechgps.com/pdf/r30.pdf, Jan 2004. URL:http://www. [3] Boberg B. and Wirkander S. Robust navigation using gps and ins comparing the kalman estimator and the particle estimator. Technical report, FOI, 2002. [4] Bull B., Diehl J., Montenbruck O., and Markgraf M. Flight performance valuation of three gps receivers for sounding rocket tracking. In 2002 National Technical Meeting - Integrating Technology. ION, 2002. [5] Kelley C. Opensourcegps. URL:http://home.earthlink.net/~cwkelley, April 2008. [6] Swedish Space Corporation. New maxus campaign carried out at esrange. URL:http://www.nordicspace.net/PDF/NSA030.pdf, May 2006. [7] Swedish Space Corporation. Maxus. URL:http://www.ssc.se/?id=6277, May 2008. [8] GPS Creations. Gps1001: Gpsbldr2, opensource gps receiver. URL:http: //www.gpscreations.com/NewFiles/GPS1001%20Brochure.pdf, May 2008. [9] Titterton D. and Weston J. Strapdown Inertial Navigation Technology. IEE, 2004. ISBN 0-863-41358-7. [10] U.S. State Department. U.s. global positioning system (gps). URL:http: //www.state.gov/g/oes/rls/fs/2006/71631.htm, Jun 2008. [11] Kaplan E. and C. Hegarty. Understanding GPS Principles and Applications. Artech House, 2006. ISBN 978-1-58053-894-7. [12] Gustafsson F., Ljung L., and Milnert M. Signalbehandling. Studentlitteratur, 2001. ISBN 978-9-14401-709-9. [13] Lachapelle G. and Petovello M. Gnss solutions: Wheighting gnss observations and variations of gnss ins integration. URL: http://www.insidegnss.com/ auto/JanFeb07GNSSSolutions.pdf, Jan 2007. 67 68 Bibliography [14] GPSWorld. Spirent tapped for gps wing. URL:http://www.gpsworld.com/ gpsworld/article/articleDetail.jsp?id=444845, Jun 2008. [15] InsideGNSS. Xsens integrated gps-imu unit. URL:http://www.insidegnss. com/products, Jun 2008. [16] Farrel J. The Global Positioning System and Inertial Navigation. McGrawHill Professional Publishing, 1998. ISBN 978-0-07022-045-4. [17] Malmström J. Robust navigation with gps/ins and adaptive beamforming. Master’s thesis, Royal Institute of Technology (KTH), 2003. [18] SNAP lab. Software gps receiver development. URL:http://www.gmat.unsw. edu.au/snap/work/sdk_projects.htm, April 2008. [19] Markgraf M. and Montenbruck O. A gps tracking system with onboard iip prediction for sounding rockets. In Journal of Spacecraft and Rockets. AIAA, 2004. [20] Markgraf M. and Montenbruck O. Phoenix hd a miniature gps tracking system for scientific and commercial rocket launches. In 6th International Symposium on Launcher Technologies. Deutsches Zentrum für Luft- und Raumfahrt, 2005. [21] Markgraf M., Montenbruck O., Hassenpflug F., Turner P., and Bull B. A low cost gps system for real-time tracking of sounding rockets. In 15th ESA Symposium on European Rocket and Balloon Programmes and Related Research. AIAA, 2001. [22] Mumford P., Parkinson K., and Dempster G. Open gnss receiver platform. In Position, Location, And Navigation Symposium. IEEE/ION, 2006. [23] Roncagliolo P.A., De Blasis C.E., and Muravchik C.H. Gps digital tracking loops design for high dynamic launching vehicles. In IEEE Ninth International Symposium. IEEE, 2006. [24] Ward P.W. Performance comparisons between fll, pll and a novel fll-assistedpll carrier tracking loop under rf interference conditions. In 11th International Technical Meeting of The Sattellite Division of The Institute of navigation. ION, 1998. [25] Glad T. and Ljung L. Reglerteori. Studentlitteratur, 1997,2003. ISBN 91-4403003-7. [26] Wikipedia. Sounding rocket. Sounding_rocket, May 2008. URL: http://en.wikipedia.org/wiki/ Appendix A Quaternions For a more in-depth explanation of the quaternions see [16] where also this summary is based upon. In this thesis the quaternion is regarded as a rotation- and  T angle-representation. The quaternion is a four vector q = q0 q1 q2 q3 that can also be partitioned into   cos(θ/2) q= (A.1) E sin(θ/2) where E is a unit vector and θ is a positive rotation about E. q0 is called the scalar part which can be placed in either the beginning or the end of the quaternion vector in different literature. In this thesis the scalar part is always the first element in the quaternion vector. A.1 Quaternion operations The norm of a quaternion is always 1. ||q|| = q02 + q12 + q22 + q32 = 1 (A.2) The quaternion conjugate is  q ∗ = q0 −q1 −q2 −q3 T (A.3) The quaternion inverse is q −1 = q ∗ /||q|| (A.4) The quaternion multiplication is represented by ~ in this thesis. The quaternion multiplication between q and p can be written as   q0 p0 − q1 p1 − q2 p2 − q3 p3 q1 p0 + q0 p1 + q2 p3 − q3 p2   q~p= (A.5) q2 p0 + q0 p2 + q3 p1 − q1 p3  q3 p0 + q0 p3 + q1 p2 − q2 p1 The quaternion multiplication is non communicative which means q ~ p 6= p ~ q 69 70 A.2 Quaternions Quaternion Rotation To rotate the vector v by the quaternion q one can write the rotation as vrotated = q ~ v ~ q −1 (A.6) Instead consider the quaternion qab , which rotates a point from frame a to frame b. To transform the point xa which is in frame a to frame b one can write xb = qab ~ xa ~ qab −1 (A.7) Multiple rotations by different quaternions can be written as vrotated = (p ~ q) ~ v ~ (p ~ q)−1 (A.8) vrotated = p ~ q ~ v ~ q −1 ~ p−1 (A.9) or The rotation is first done by q then by p. A.3 Quaternion to Rotation Matrix The quaternion can also be written as a rotational matrix, consider the equation (A.7) to be written as xb = Cab xa (A.10) Cab can then be written as  2 q0 + q12 − q22 − q32 b Ca =  2(q1 q2 − q0 q3 ) 2(q1 q3 + q0 q2 ) 2(q1 q2 + q0 q3 ) q02 + q22 − q12 − q32 2(q2 q3 − q0 q1 )  2(q1 q3 − q0 q2 ) 2(q2 q3 + q0 q1 )  q02 + q32 − q12 − q22 (A.11) Appendix B Euler Angles For a more in-depth explanation of Euler angles see [16] where also this summary is based upon. The Euler angles are the angles between a reference frame and one rotated reference frame. The Euler angles can be written as     θ roll Ψ =  φ  = pitch ψ yaw (B.1) The roll angle is the rotation around the x-axis, the pitch angle is the rotation about the y-axis and the yaw angle is the rotation about the z-axis. pitch yaw roll Figure B.1. The Euler Angles 71 72 B.1 Euler Angles Euler Angle Rotation To rotate a vector by the Euler angles, a rotational matrix must be calculcated. " C= cos(ψ) cos(θ) − sin(ψ) cos(φ) + cos(ψ) sin(θ) sin(φ) sin(ψ) sin(φ) + cos(ψ) sin(θ) cos(φ) − sin(θ) · · · cos(θ) sin(φ) cos(θ) cos(φ) sin(ψ) cos(θ) cos(ψ) cos(φ) + sin(ψ) sin(θ) sin(φ) − cos(ψ) sin(φ) + sin(ψ) sin(θ) cos(φ) # (B.2) The rotated vector can now be written as vrotated = Cv B.2 (B.3) Euler Angle to Quaternion conversion The Euler angles can also be converted to a quaternion using   cos(φ/2) cos(θ/2) cos(ψ/2) + sin(φ/2) sin(θ/2) sin(ψ/2) sin(φ/2) cos(θ/2) cos(ψ/2) − cos(φ/2) sin(θ/2) sin(ψ/2)  q= cos(φ/2) sin(θ/2) cos(ψ/2) + sin(φ/2) cos(θ/2) sin(ψ/2) cos(φ/2) cos(θ/2) sin(ψ/2) + sin(φ/2) sin(θ/2) cos(ψ/2) given that the rotation is small one can also do the approximation   1  θ/2   q≈  φ/2  ψ/2 (B.4) (B.5) Appendix C Abbrevations 3-D Three dimensional ARM Advanced RISC Machine B-frame Body Fixed frame C/A Coarse Acquisition DLL Delay Lock Loop DLR Deutsches Zentrum für Luft und Raumfahrt DNSS Defence Navigation Satellite System E-frame Earth Centered Fixed frame EADS European Aeronautic Defence and Space Company EADS-ST EADS Space Transportation ECI Earth Centered Inertial ECEF Earth Centered Fixed EKF Extended Kalman Filter FLL Frequency-lock Loop FPGA Field-programmable Gate Array GNSS Global Navigation Satellite System GPS Global Positioning System GSOC German Space Operations Center I-frame Earth Centered Inertial frame 73 74 IF Intermediate Frequency INS Inertial navigation System KF Kalman Filter LKF Linear Kalman Filter MTi-G Miniature motion sensor with integrated GPS NCO Numerical Controlled Oscillator OSD Office of the Secretary of Defense PLL Phase-locked Loop PRN Pseudo Random SA Selective Availability SDK Software Development Kit SSC Swedish Space Corporation UTC Coordinated universal Time FOI Försvarets Forskningsinstitut ION The Insitute of Navigation Abbrevations