Embed Size (px)
Citation preview
GPS-Aided INS
with
Applications to Aircraft Turns
AE 494 – B.Tech. Project (Stage II)
By
Vaibhav V Unhelkar
07D01003
Under the guidance of
Prof. H.B. Hablani
Department of Aerospace Engineering,
Indian Institute of Technology, Bombay, May, 2011
Certificate
Certified that this B.Tech. Project (Stage II) Report titled “GPS-Aided INS with Applications to Aircraft Turns” by Vaibhav Unhelkar is
approved by me for submission. It is certified further that, to the best
of my knowledge, the report represents work carried out by the student.
Date: Prof. H.B. Hablani
Abstract
The advent of Global Navigation Satellite Systems (GNSS) has brought about an additional navigational aid for aircrafts. The
presence of GNSS (specifically, Global Positioning System - GPS)
receivers has become commonplace in the aircrafts of today. Due to their complementary error characteristic, approaches involving GPS
coupled with the Inertial Navigation System (INS) are being looked
into for complete navigation and attitude estimation of the aircraft.
During the first stage of the project, a simplified model concerning
GPS in aircrafts with a decoupled and ideal INS was considered. A
Position Velocity – Extended Kalman Filter was developed for the above model and simulations were carried out for the same.
In this report we present an integrated GPS – INS coupled approach
with open loop tightly coupled architecture. Pseudorange measurements are used as navigational aid for the INS. A more
detailed model considering spherical earth as opposed to the flat earth
assumption for navigation equations is studied and simulated. Lastly, the simulations for a generic INS-configuration EKF are presented.
Keywords: Navigation, GPS, INS, Extended Kalman Filter
Contents
List of Figures………………………………………………………………1
1 Introduction……………………………………………………………2
2 Global Positioning System…………………………………………….3
3 Inertial Navigation System……………………………………………4
3.1 Gyros……………………………………………………………….4
3.2 Accelerometers…………………………………………………….4
4 Position Velocity – EKF for GPS……………………………………. 5
4.1 Flight Path………………………………………………………... 5
4.2 Development of Simulation……………………………………… 5
4.3 Extended Kalman Filter for GPS………………………………...5
4.4 Results…………………………………………………………….. 7
5 GPS – INS Integration Architectures………………………………...9
6 INS – Configuration Kalman Filter…………………………………11
6.1 True Motion………………………………………………………11
6.2 Sensor Models…………………………………………………….14
6.3 INS - Propagation……………………………………………….. 15
6.4 INS - Configuration Filter……………………………………… 16
7 Simulation Results………………………………………………….. 21
7.1 Flight Trajectory………………………………………………… 21
7.2 INS Measurements……………………………………………… 21
7.3 Filter Estimates………………………………………………….. 22
7.4 Circular Motion…………………………………………………. 24
7.5 Variation with Satellite Visibility………………………………. 25
8 Conclusion and Future Work………………………………………. 27
9 References……………………………………………………………. 29
1
List of Figures
Figure 1 : Generic GPS – INS Integration (Source : Groves, 2008) ....................................... 2
Figure 2 : Aircraft True Trajectory (PV – EKF) ....................................................................... 5
Figure 3 : PV - EKF Estimate - Y Position ............................................................................. 7
Figure 4 : PV - EKF Estimates - Y Velocity ............................................................................ 7
Figure 5 : Coordinated Turn – No INS (PV-EKF) ................................................................... 8
Figure 6 : Coordinated Turn – With INS (PV-EKF) ................................................................ 8
Figure 7 : Open Loop v/s Closed Loop Architectures (Source : Groves, 2008) ...................... 9
Figure 8 : Loosely Coupled and Tightly Coupled Approaches (Source : Groves, 2008) ....... 10
Figure 9 : Aircraft Bank Angle (INS – EKF) ......................................................................... 11
Figure 10 : Roll Rate and Yaw Rate due to Bank Command ............................................... 12
Figure 11 : True Aircraft Trajectory (INS – EKF) .................................................................. 13
Figure 12 : Pseudorange Measurement Noise (INS – EKF) ................................................ 14
Figure 13 : Clock Bias (INS – EKF) ..................................................................................... 15
Figure 14: Flow Chart of the EKF (Source: Bar Shalom, 2001) ........................................... 20
Figure 15 : Flowchart for Simulation of GPS – INS Configuration Kalman Filter .................. 20
Figure 16 : Lat – Long Error due to INS............................................................................... 21
Figure 17 : Gyro Measurement Error ................................................................................... 21
Figure 18 : Accelerometer Measurement ............................................................................ 22
Figure 19 : GDOP ............................................................................................................... 22
Figure 20 : North Velocity Error Estimates .......................................................................... 22
Figure 21 : Estimation Errors Velocity (Navigation Frame) .................................................. 23
Figure 22 : Estimation Error Tilt ........................................................................................... 23
Figure 23 : Estimation Error Position ................................................................................... 24
Figure 24 : Circular Aircraft Trajectory (Constant Bank) ...................................................... 24
Figure 25 : Estimation Error Velocity (Navigation Frame) – Circular Trajectory ................... 25
Figure 26 : Filter Performance (3 Satellites) Figure 27 : Filter Performance (1 Satellite).. 25
Figure 28 : Estimation Error Velocity – Comparative Study with Satellite Visibility ............... 26
Figure 29 : Estimation Error Position – Comparative Study with Satellite Visibility .............. 26
List of Acronyms
DGPS, Differential Global Positioning System
EKF, Extended Kalman Filter
GNSS, Global Navigation Satellite System
GPS, Global Positioning System
INS, Inertial Navigation System
PV, Position – Velocity
2
1 Introduction
Global Positioning System (GPS) receivers have become commonplace in the modern
aircrafts, and can be used for aircraft navigation. Furthermore, GPS receivers have
complementary error characteristics to the Inertial Navigation System (INS) used in aircrafts.
Thus, GPS – INS coupled approaches are being looked into so as to better the estimates
obtained from both of the sensors.
During the first stage of the project, to appreciate the use of GPS in aircrafts, a simplified
model of GPS on an aircraft was studied. For modelling purposes, the aircraft was assumed to
be a point mass and hence INS was decoupled from the system. Further to understand how
GPS data is processed, basics of Kalman Filtering were studied and an Extended Kalman
Filter (EKF) was designed for the above-mentioned model. Lastly, simulations were carried
out in order to observe the performance of the EKF. Firstly, we present a brief overview of
the above model and its simulation results.
The performance of the above filter motivates the use of GPS INS coupled approaches. In
order to incorporate the Inertial Navigation System in our simulations, basic models of the
Navigation Error Equations were studied. Also, in order to understand GPS - INS integration
various integration architectures, viz. loosely-coupled, tightly-coupled and deeply-coupled
were studied. A generic approach to the GPS – INS coupling is shown below. To develop
upon the simulations carried out earlier, covariance analysis of a simplified GPS – INS EKF
was carried out. Lastly, to carry out a detailed simulation for the GPS – INS integrated EKF,
the formulation was studied from (Rogers, 2007). The wander azimuth frame was used as the
navigation frame for the flight computer and GPS – INS integrated EKF. Based on the above
formulation simulations were carried out for a 600 second long flight trajectories over
Mumbai.
Figure 1 : Generic GPS – INS Integration (Source : Groves, 2008)
The report first describes the basic principles of the GPS and INS and the errors involved.
Then, a basic overview of GPS – INS integration is provided. Next, briefly the results of
simplified GPS – PV EKF are presented. Lastly, the equations and results of the detailed
simulations are presented.
3
2 Global Positioning System
The Global Positioning System is a global satellite based navigation system, and amongst its
many applications can be used to obtain position and velocity estimates of aircrafts equipped
with GPS receivers. Here, we briefly describe the fundamental principles of GPS, the error
characteristics and how it can be used to obtain position and velocity estimates of an aircraft.
The GPS system comprises of three sub-systems, namely Space, Control and User Segments
details of which can be found in (Parkinson, 1996).
GPS Receivers using the signals can find out pseudorange, the distance between the receiver
and the broadcasting GPS satellite, by measuring the time taken by the signal to reach the
user from the satellite. This is done by replicating the satellite and finding out the time shift.
Based on these pseudorange measurements, and employing the principle of trilateration one
can estimate the user position. As the receiver clock is not as accurate, while estimating
position, receiver‟s clock bias is considered as an unknown. Hence, by using four
pseudorange measurement position and clock bias can be determined. The measurements
described above are found due to time shift of the GPS signal, however frequency (phase)
shift can also be used alternatively. Further by use of Differential GPS (DGPS) or/and dual
frequency receivers better measurements (with less error) can be made.
, 1,2,...,i i ir c t w i n
The pseudorange measurements obtained are erroneous. These errors have been analyzed
in detail in literature, and are attributed to the following components:
Satellite Ephemeris Broadcast error
Ionospheric Refraction – caused due to signal propagation through troposphere
o Diurnal variation of Ionospheric Error
o Statistical models based on local position used for modelling the error
o Can be removed almost completely by using Differential GPS
Tropospheric Refraction – caused due to signal propagation through troposphere
Multipath Errors
o Path of the GPS signal may not be unique near earth due to reflection
Receiver Clock Errors
o Usually modelled using two parameters Clock Bias and Drift
Ionospheric and Tropospheric errors can be eliminated by using carrier phase
measurements and/or DGPS. Further, the satellite geometry too has an effect on the
accuracy of the measurement. The error due satellite geometry is quantified by the
parameter called Geometric Dilution of Precision (GDOP). The farther away the satellites,
the smaller (and the better) the value of GDOP will be. A satellite configuration GDOP
value of less than 5 is considered as a good geometry.
2 2 2( ) ( ) ( )
299792458
( , , )
th
i
i i i i
th
i
th
i i i
pseudorange between the user and i satellite
r X x Y y Z z true range
t receiver clock offset from GPS time
c speed of light m s
w measurement error for i satellite
X Y Z position of i sate
llite
4
3 Inertial Navigation System
Inertial Navigation System (INS) is a dead reckoning navigational aid. Generally, an INS
consists of gyroscopes and accelerometers (the IMU) along with a processor to integrate
navigation equations, which provide the data required for automatic navigation. INS provides
a way of dead reckoning, i.e. it doesn‟t require any external signal hence can remain
undetected. Gyros (gyroscopes) measure angular rates, whilst the accelerometers measure
acceleration. Here, we briefly describe the error characteristics of the gyro and
accelerometers.
3.1 Gyros
Gyros measure the inertial angular rates and are of two types, namely, gimballed and
strap-down. Gimballed gyros measure the inertial angular rate of the navigation frame
while the Strap-down gyros measure the inertial rate of the body frame. Both the systems
have their pros and cons. However, strap-down gyros are low-cost and are used as the
sensor of choice in our detailed simulations.
3.2 Accelerometers
As opposed to gyros which measure the inertial rates, the accelerometers measure the
body referenced accelerations. Accelerometer hardware is available in various types,
including Microelectromechanical Systems (MEMS) type.
Error models for gyro and accelerometers are required in order for propagation and
estimation. Various detailed error models exist for Accelerometers and Gyro errors,
although for our study a simplified error model is considered. Errors in measurement
include:
Bias
o Also known as the zero error
o The bias varies based on operation condition such as, Operating
Temperature and Vibration Temperature
Random Noise
o Can be simulated as a random walk
Scale Factor
o This is given by the slope of the best-fit line describing the variation of the
output versus the input
Tilt Misalignment
Nonlinearities
o Dead Band
o Threshold
o Hysteresis
The mean values of above error parameters are listed in the specification sheets for the
sensors. Though a lot more can be said about GPS, INS and their applications, here we have
included the minimum background required for the following sections.
5
4 Position Velocity – EKF for GPS
In order to understand the implementation of GPS for an aircraft, a hands-on approach was
adopted. A design problem mentioned in Reference [1] was used for this purpose. Detailed
formulation was included in the Stage I, here we briefly present the results. In this simulation
GPS was simulated alone, decoupled from the INS. The flight trajectory was carried out in
the local-level frame with flat earth assumption.
4.1 Flight Path
Figure 2 : Aircraft True Trajectory (PV – EKF)
The aircraft at beginning moves straight at a constant velocity of 120 m/s in the y
– direction of the locally level reference frame and continues to do so for 500 s.
white white whitex v y v z v
It then completes a 1◦/s “coordinated turn” of 90◦ in the horizontal x–y plane from
due north to due east, starting at t = 500 s and till t = 590 s.
turn white turn white whitex y v y x v z v
After it completes this turn, the aircraft continues moving straight with a constant
velocity till t = 1200 s. (Governing equations same as that of the initial part)
4.2 Development of Simulation
The models generated for simulation were:
GPS Satellite Constellation
Aircraft True Trajectory
GPS Receiver Clock for Pseudorange Measurement
Further, to simulate noise in all these models random number generation is required.
4.3 Extended Kalman Filter for GPS
The Extended Kalman Filter is to be used for our problem, since the pseudorange
6
measurement equation is non-linear. A seven state EKF was written with states as
follows, with (x,y,z) in local-level reference frame: T
x x y y z b dx
The system matrix,
1 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 0 1 0 0
0 0 0 0 0 1
0 0 0 0 0 0 1
T
T
F
T
,
And the process noise covariance (describing process noise) is given as,
3 2
2
3 2
2
3 2
2
3 2 0 0 0 0 0
2 0 0 0 0 0
0 0 3 2 0 0 0
0 0 2 0 0 0
0 0 0 0 0 0
0 0 0 0 0 3 2
0 0 0 0 0 2
q q
q q
q q
q q
z
b d d
d d
S T S T
S T S T
S T S T
Q S T S T
S T
S T S T S T
S T S T
,
Where, Sq (for x-y components of position and velocity), Sz(for z component of position
and velocity), Sb (for clock bias) and Sd (for clock drift) are filter parameters which
indicate the intensity of continuous time process noise. The parameter are as follows:
Pseudorange measurements as described earlier are used, and the linearized measurement
matrix is given as:
The measurement matrix,
1 1 1
2 2 2
3 3 3
4 4 4
5 5 5
6 6 6
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
x y z
x y z
x y z
x y z
x y z
x y z
h h h c
h h h c
h h h cH
h h h c
h h h c
h h h c
,
The measurement noise covariance matrix, R is given as a diagonal matrix with all the
diagonal entries equal to the variance of the pseudorange measurement noise, 2
p ( p in
our case equals 30m). The measurement noise covariance matrix is diagonal because the
measurement errors are uncorrelated across the different satellites.
For initialization of the filter initial estimate of covariance is required, as well as initial
value of state vector is required.
7
4.4 Results
Here we have plotted the time history of Y – Coordinate (above) and velocity in Y –
Coordinate (below) for the initial straight portion of the trajectory. Similar
performance is observed in the other two coordinates
Figure 3 : PV - EKF Estimate - Y Position
The first plot of each figure shows the evolution of Estimated State and the True State.
The second plot of each figure shows the Estimation Error in the respective
component, and the bounds placed by the iiP , where the results of ith
state are being
plotted.
Figure 4 : PV - EKF Estimates - Y Velocity
8
Now, we discuss the performance of filter in the coordinated turn. Here, as the GPS and
INS are decoupled, hence no information from the INS is provided to EKF and the
process model remains same.
Figure 5 : Coordinated Turn – No INS (PV-EKF)
It can be clearly seen that the estimates during the coordinated turn, i.e. for t ∈
[500,590] go haywire. This motivates use of integrated GPS – INS systems. Just for a
comparative study, in case the INS is used it will yield the correct process model for the
EKF. In such a case following profile of estimate is observed:
Figure 6 : Coordinated Turn – With INS (PV-EKF)
The above comparison though a limiting case, shows what GPS - INS coupled approaches
can result in. Lastly, the above limitations on process model also indicate the need of
modelling the EKF in terms of error states, so as to obtain a unified process model.
9
5 GPS – INS Integration Architectures
From the previous sections one can gauge what GPS – INS Integration has to offer in terms
of navigation accuracy. In this section we learn basics of GPS – INS integration and discuss
various architectures that have been studied.
Inertial navigation system (IMU along with propagation equations) has error characteristics
complementary to the GPS receiver. The INS can operate at a high rate (25-100 Hz) and has
low short term noise. The INS can independently provide estimates of position, velocity and
attitude along with angular rate and accelerometer measurements. Moreover, the INS cannot
be jammed as it is „dead reckoning‟. But the accuracy of these estimates is short-lived, as the
estimates deteriorate with time as errors get integrated and accumulate.
GPS measurements on the other hand, although do not operate as quickly (1-2 Hz), have
noise right from the point that the system gets initialized and the GPS measurements can be
jammed. The GPS measurements can also provide position and velocity estimates. However,
the advantage that GPS measurement is that error doesn‟t grow as quickly, rather as seen in
the previous section, by proper choice of Kalman Filter parameters this error can be reduced.
Thus by combining both the measurements accurate estimates can be obtained both initially
and for a prolonged time.
The crudest form of integration is to obtain a solution from GPS of position and velocity
estimates so as to reinitialize the inertial navigation equations. In this case the equations of
GPS and INS are totally uncoupled, but as soon as the INS errors grow out of bounds, the
errors are brought back to the accuracy provided by the GPS.
The variation in integration architectures lies in mainly three fields:
The way in which INS errors are corrected by GPS measurements
The type of GPS measurement being used
The way in which GPS receiver is aided by the INS
Figure 7 : Open Loop v/s Closed Loop Architectures (Source : Groves, 2008)
10
Also depending upon whether the correction is applied to reinitialize the INS after every
time-step, or whether the INS is not reset, we have open and close loop architectures.
Based on the above parameters the various GPS-INS approaches are defined as:
Loosely Coupled
o GPS position and velocity solution is used for aiding
o Involves cascaded Kalman Filters, the first PV – EKF for GPS and then the
integrated Kalman Filter
o Is the simplest type of architecture, usually used while retrofitting the GPS in
existing systems with INS
Tightly Coupled
o GPS measurements are used directly in the form pseudorange and/or range
rate for aiding the INS
o Both the Kalman Filter are in a way joint into one, hence, only a single
Kalman Filter is required in this type of architecture
o This architecture doesn‟t involve full fix of position and velocity from the
GPS making it more robust
Figure 8 : Loosely Coupled and Tightly Coupled Approaches (Source : Groves, 2008)
Deeply Coupled
o This is an advanced architecture involving deep integration of INS and GPS
o It combines GPS Navigation and Tracking, information from the INS is used
to track the satellites
o This also uses a single Kalman Filter wherein all the states required for GPS-
INS integration and Satellite tracking are estimated
The INS configuration Kalman Filter presented in the next section is open loop architecture,
as the navigation corrections are not used to reset the INS propagation equations. Moreover,
it can be said to be a tightly coupled configuration since pseudorange measurements are being
used and a combined EKF finds out the navigation solution.
11
6 INS – Configuration Kalman Filter
The INS – Configuration Kalman Filter is a 12-state EKF which uses GPS measurements to
obtain estimates of error states of position, velocity and platform tilt. The Position Velocity –
EKF described earlier is limited to the flight trajectory, however, since in the INS –
configuration KF has error in the desired variables as its states, it can work for various flight
stages by proper tuning of Q matrix. The formulation and initial code of this EKF are from
Reference [9]. This section describes an open loop tightly coupled GPS – INS architecture,
the author of Ref. [9] however refers to it as an INS – Configuration Kalman Filter.
6.1 True Motion
The aircraft motion is simulated using navigation equation in the NED – Frame. The
aircraft initiates at
Latitude = 18.92 degrees (Mumbai)
Longitude = 72.90 degrees (Mumbai)
Altitude = 6000 ft
Velocity in East direction = 650 km/h
Velocity in North and Down Direction = 0
The aircraft motion is governed by the bank command, and the aircraft is following the
bank-to-turn manoeuvre. Two aircraft trajectories were simulated by varying the bank
command. The first trajectory changes the latitude of the aircraft by appropriate bank
command, the second trajectory describes motion of the aircraft in a circle. Equations of
motions for both the trajectories are similar and the only difference lies in the bank
command. The bank command for circular motion of the aircraft is, obviously, the
constant bank angle. The bank command for the first trajectory is as follows:
Figure 9 : Aircraft Bank Angle (INS – EKF)
12
The body rates obtained from the above bank command are as follows:
Figure 10 : Roll Rate and Yaw Rate due to Bank Command
The environment model (which can be replaced by data from a real flight) gives the true
value of
Latitude, Longitude and Altitude of the Aircraft
Velocity in NED Frame
Transformation Matrix from Body to NED Frame, Cbv
Inertial Angular Velocity of the aircraft expressed in Body Frame, /
b
i b
o Which is measured by the gyros
Acceleration with respect to NED frame expressed in Body Frame, fb
The equations to generate above information for the aircraft are described below. The
bank angle is denoted by „η‟, then rate of turn which contributes to the body rate is:
tangRate of Turn
V
The acceleration in the navigation frame is obtained by the following expression and then
is converted to the body frame of reference,
/ /( 2 )g g
g g e g i e g g
b bg g
f v v g
f C f
The rates required in the above equation are given as follows:
/
cos( )
0
sin( )
earth
g
i e
earth
13
/
cos( )
sin( )
g
e g
3 2 1
/ 3 1 / 2
2 1 3
0
0 ;
0
x y
x y x y
The inertial angular rate of the aircraft expressed in body frame is given by:
/ / /
/ / / /
/ / / /
( )
( )
b b b
i b i g g b
b b b b
i b i e e g g b
b g g g
i b bg i e bg e g bn g bC C C
The attitude dynamics of the aircraft is described by:
/
b
bn n b bnC C
Lastly, the propagation of latitude, longitude and altitude is given by:
( )cos( )
N
E
D
V
R h
V
R h
h V
True Motion of the aircraft is obtained, after numerically integrating above mentioned
equations obtained from Reference [9], as:
Figure 11 : True Aircraft Trajectory (INS – EKF)
14
6.2 Sensor Models
We simulate three aircraft sensors for our problem
Gyro
o It measures /
b
i b , Inertial Body Rate expressed in Body Frame
o Gyro errors were described earlier, for our problem we have considered the
gyro to have bias and white noise
o Bias = 0.1 degree/hour (Ref. [9])
o White Noise Power Spectral Density(PSD) = 2.35*10-11
rad/s (Ref. [2])
/( ) ; 1b
scale i b scaleGyroscope Measurement K Bias White Noise K
Accelerometer
o It measures acceleration of the aircraft in navigation frame expressed in
body frame, fB
o Accelerometer is also modelled to have bias and white noise
o Bias = 0.01g (Ref. [9])
o White Noise PSD = 0.0036 (m/sec2)
2/(rad/sec) (Ref. [2])
( ) ; 1scale b scaleAccelerometer Measurement K f Bias White Noise K
GPS Receiver
o GPS Noise is approximated as exponentially auto-correlated noise
Figure 12 : Pseudorange Measurement Noise (INS – EKF)
The GPS receiver clock is modelled using two state components, clock bias b and clock
drift d. The governing equations for the clock bias are given as:
( ) ( ) ( )
( ) ( )
b
d
b t d t v t
d t v t
In discrete time form, 2( 1) ( ) ( )c c cx k F x k v k
where, [ ]T
cx c b d , and vc(k) has zero mean and the following covariance matrix
2
1
0 1
TF
15
Figure 13 : Clock Bias (INS – EKF)
3 2
2 2
2
1 0 3 2
0 0 2c b d
T TQ S Tc S c
T T
The parameters Sb and Sd are the noise parameters for a clock, and there mean value
depends upon the type of the clock. For crystal clocks the typical values are of the order
of Sb = 4 × 10-20, and Sd = 8 × 10-19.
Apart from the above mentioned sensors, altimeter is also simulated in some cases. The
GPS-INS Kalman filter can work without altimeter, however since the altimeter makes
the vertical channel stable, it reduces the rate of growth of error.
6.3 INS - Propagation
Based on the sensor data, which is in turn obtained from the environment model the flight
computer propagates the navigation equation to obtain the estimates of aircraft position,
velocity and tilt. In this section we summarize the equation used for the above mentioned
propagation.
To initialize the propagation equation we require:
Transformation matrix from body to navigation frame
o Detailed methods exist for in-motion alignment and initialization of the
INS system
Velocity in Navigation Frame
o Can be obtained based on some external measurement
Transformation matrix from ECEF to Navigation Frame
o This can be obtained based on latitude and longitude information of the
aircraft
The attitude dynamics can be propagated in various ways, here we use quaternions as they
reduce number of variables required for propagation.
16
2 2 2 2
1 2 0 3 1 3 0 20 1 2 3
2 2 2 21 2 0 3 2 3 0 10 1 2 3
2 2 2 2
1 3 0 2 2 3 0 1 0 1 2 3
2 2
2 2
2 2
n
bq q q q q q +q qC = q +q q q
q q +q q q q +q qq q +q q
q q q q q q +q q q q q +q
Next we use gyro measurement in order to obtain:
/ / / /
/ 1 2 3
b b b n b n
n b i b n i e n e n
Tb
n b
C C
3 2 11 1
3 1 22 2
2 1 33 3
1 2 34 4
0
01
02
0
q q
q qd
q qdt
q q
The quaternions have to be renormalized after every iteration, so as to maintain the
transformation matrix orthogonal. The equations used here are obtained from Ref. [9].
Next, we use the acceleration measurement to obtain the propagation equation for velocity
/ /2
n
n b b
n n
n n e n i e n n
f C f
v f v g
Lastly, dynamics of transformation matrix from navigation frame to ECEF is given as
/
e e n
n n e nC C
The above transformations are used to obtain the latitude, longitude, altitude, wander
azimuth angle and velocity in geographic frame. These are then fed to the Kalman Filter
along with GPS pseudorange measurements to obtain estimates of error states.
6.4 INS - Configuration Filter
The configuration EKF also resides on the Flight Computer and integrates the data from
the INS and GPS to give refined estimates of navigation error. The EKF consists of 12
states namely
Position Error Vector in Navigation Frame – 3 States
Velocity Error Vector in Navigation Frame – 3 States
INS platform attitude error vector – 3 States
Altimeter Bias – 1 State
o Used only if altimeter is being used
GPS Receiver Clock Bias – 1 State
o Clock Error*Speed of Light
GPS Receiver Clock Drift – 1 State
o Clock Drift*Speed of Light
17
The state space equation governing the above states (Process Model) is given by,
( ) ( ) ( ) ( )x t A t x t v t
where v(t) denotes the random white noise in the process.
The A matrix (continuous time) is obtained from governing equations of each of the state
variables, described in previous sections.
0 0 1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0
0 0 0 2 2 0 0 0 0
0 0 2 ( 2 ) 0 0 0 0
22 ( 2 ) 0 0 0 0 0
( )
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0
y
x
y x
zz y y z y
z zz x x z x
yxy y x x y x
z y y
z x x
y y x x
f gf f
R
f g vf f
R R
ff gf f
A t R R R
10 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 0 0 0 0
h
Where, /
/
n
e n
n
i e
To obtain the discrete time version of the A(t) matrix we use the approximation
( ) ( )F k I A t T
The Process Noise Covariance Matrix used in our simulations is given as follows:
3 3 2 12 12 12
1:10
11:12
1 1 1 10 10 10 10 10 10 10
c
Q diag T
Q Q
The diagonal elements represent the covariance in respective state, i.e. the first three
components represent covariance of 1 m2/s in the process noise, the next three component
represent covariance of 10-3
m2/s
3 in lateral direction and of 10
-3m
2/s
3 in vertical channel.
Similarly, rest of the components represent covariance of process noise of the respective
state. By combining the two matrices Q1:10 and Qc we obtain the whole process noise
covariance.
1:10 10 2
2 10 11:12
0
0
Q
18
Now we describe the Measurement Model. Pseudorange measurements are being used as
an aid to the INS system. The pseudorange equation as described earlier is nonlinear,
hence we linearize it and obtain the H matrix. The linearized measurement matrix is
obtained by Taylor series expansion of the pseudorange equation about the latest available
estimate, i.e. ˆ( | 1)k k x . The measurement equation is:
( ) [ , ( )] ( )z k h k k w k x
Based on our states the H matrix is given as,
, 1 3 1 3
,
0 0 0 1 0i LOS i
i i i
LOS i
H e
h h he Unit Vector along LOS
x y z
2 2 2
2 2 2
2 2 2
( )
( ) ( ) ( )
( )
( ) ( ) ( )
( )
( ) ( ) ( )
x ii
i i i
y ii
i i i
z ii
i i i
X xh
X x Y y Z z
Y yh
X x Y y Z z
Z zh
X x Y y Z z
Here, we are working with four pseudorange measurements (the minimum required to
obtain a full fix), hence H has four rows. The number of rows in H equal the number of
pseudorange measurements being used. For the case of four satellites the H matrix is
given as follows, the size of the matrix will vary depending on the number of pseudorange
measurements being used:
,1 1 3 1 31
,2 1 3 1 32
,3 1 3 1 33
,4 1 3 1 34
0 0 0 1 0
0 0 0 1 0
0 0 0 1 0
0 0 0 1 0
LOS
LOS
LOS
LOS
eH
eHH
eH
eH
The measurement noise covariance R, indicates error covariance in each pseudorange
measurement. Although, in the truth model the measurement noise for each satellite is
exponentially autocorrelated, in the filter the error in measurement is assumed to have
30m standard deviation. Hence, superior estimates can be found by increasing the states
of the Kalman Filter so as to include drift in pseudorange as a state. Each pseudorange
measurement is uncorrelated, hence R is diagonal and is given as [Ref. 9]:
2
1 0 0 0
0 1 0 030
0 0 1 0
0 0 0 1
R
19
Lastly, to initialize the filter we need the initial estimate of covariance matrix P(0|0). The
values for this depend on the accuracy of external measurements which are used to
initialize the system. The altimeter bias will be a useful state when the altimeter is being
used. Based on accuracy of initialization the parameters are given as follows (the values
have been obtained from [Ref. 9]):
8 8 8 2
1:3
2 2 2 2
4:6
6 6 6 2
7:9
2 2 2 2
10:12
10.76 10 10 10 Position Covariance
10.76 10 10 10 (m/s) Velocity Covariance
10 10 10 (rad) Tilt Covariance
10.76 10 10 10 Altimeter, Cloc
P diag m
P diag
P diag
P diag m
1:3 3 3 3 3 3 3
3 3 4:6 3 3 3 3
3 3 3 3 7:9 3 3
3 3 3 3 3 3 10:12
k Bias and Drift Covariance
0 0 0
0 0 0
0 0 0
0 0 0
P
PP
P
P
In order to implement the EKF algorithm above mention parameters are used, a brief
overview of their role in the algorithm is as follows:
Initial State – assumed to be a random variable. The mean of this random variable
is used as the initial estimate, whilst the expected accuracy of the mean is used as
the initial covariance.
System’s Dynamic Model – a model linear or non – linear of the process whose
state‟s estimates‟ are to be obtained. In case the model is non-linear, the linearized
model about latest available estimate is used for propagation of the
covariance matrix associated with the estimate. Note that the linearized model is
used only for the covariance propagation.
Process Noise Covariance ( Q – Matrix ) – the Q matrix gives an estimate of the
noise entering into the Dynamic Model, and quantifies the accuracy of the process
model.
Measurement Equation – a model linear or non – linear relating measurement to
the state vector. Again if the model is non-linear, a linearized version is to be
used for covariance propagation.
Measurement Noise Covariance ( R – Matrix ) – the R matrix gives an estimate
of the noise entering into the Measurement Model, and quantifies the accuracy of
the measurement model.
The noise entering the system or the measurement should be white and mutually
uncorrelated. The filter performance depends on the accuracy of covariance matrices Q
and R. The EKF algorithm is summarized in the following figure.
20
Figure 14: Flow Chart of the EKF (Source: Bar Shalom, 2001)
The Filter Gain W indicates the relative accuracy of predicator versus corrector step.
The overall simulation flow-chart is represented as follows:
Figure 15 : Flowchart for Simulation of GPS – INS Configuration Kalman Filter
{Adapted from Ref. [9] – Simulates open loop tightly coupled Kalman Filter}
21
7 Simulation Results
In this section we summarize the performance of the INS – Configuration EKF described in
the previous section. The simulation results are shown for the first of the two trajectories
described earlier.
7.1 Flight Trajectory
The flight trajectory can be initially obtained directly from the INS Propagation, this
trajectory is thus the pre-filtered trajectory. It shows the error that arises due to the INS
Propagation without any external(GPS) aiding:
Figure 16 : Lat – Long Error due to INS
7.2 INS Measurements
To obtain the above flight trajectory, INS measurements were used by the flight
computer. These INS measurements are erroneous and hence requirement of a filter
arises. The gyro measurements were shown earlier(Figure 10), measurement errors are as
follows:
Figure 17 : Gyro Measurement Error
22
The errors in accelerometer measurements are shown below:
Figure 18 : Accelerometer Measurement
The errors in pseudorange measurements have been presented in the previous section. To
gauge the satellite visibility we plot the GDOP:
Figure 19 : GDOP
7.3 Filter Estimates
Based on the above measurements and INS propagation as the input, the filter obtains
estimates of the INS errors.
Figure 20 : North Velocity Error Estimates
23
In this section we present the filter estimates of the state vector. The velocity estimates of
the filter in x-direction are shown above (see previous page). The first subplot shows the
True velocity, velocity computed by INS and the velocity obtained after filtering. The
second subplot shows reduction in error after filtering.
The velocity estimates in all the three direction are presented as follows:
Figure 21 : Estimation Errors Velocity (Navigation Frame)
The tilt estimates in all the three direction are presented as follows:
Figure 22 : Estimation Error Tilt
The two envelopes in the above plots represent the square root of respective diagonal
element of P matrix, which indicates the estimated standard deviation in the estimate.
Lastly, the position estimates in the three directions are shown in the following figure. We
can observe that the position estimates are not as superior as the tilt and velocity
estimates. The filter performance can be made better by fine tuning the covariance
matrices.
24
Figure 23 : Estimation Error Position
7.4 Circular Motion
To verify that the filter is indeed generic, an alternate flight trajectory, namely, motion in
a circle, was simulated. Such trajectories are very common for aircrafts hovering near
airports, when they are bereft of landing space. The flight trajectory was generated by
constant bank angle command and is given as:
Figure 24 : Circular Aircraft Trajectory (Constant Bank)
The circular flight was to be completed in 600 seconds and speed of 650 km/h, leading to
requirement on bank angle of 11.06 degrees. Since, general civil aircrafts can bank up to
30 – 35 degrees; this bank angle could be used. The filter performance was similar to the
one observed in the previous trajectory, validating that the filter written with error
parameters as the state is generic. The velocity estimates for this case are shown on the
next page, other estimates were also observed to be acceptable.
25
Figure 25 : Estimation Error Velocity (Navigation Frame) – Circular Trajectory
7.5 Variation with Satellite Visibility
The PV-EKF requires at least four satellites to obtain the position fix of the receiver.
However, in GPS – INS coupled architectures estimates of position can still be obtained
albeit with reduced accuracy, since INS provides estimates of the position and velocity.
This is very useful in places such as war zones, where GPS signals can be jammed by the
enemy to prevent navigation of opponent‟s aircraft. To observe the deterioration in
performance the system was simulated with reduced satellite visibility, i.e with number of
visible satellites to be three and one, respectively.
Velocity estimates in both the cases are given below. The variation in other filter
parameters is also similar. The major difference as compared to four satellite case, is that
the standard deviation increases as opposed to be decreasing in time, highlighting the
need of at least four satellites for improvement in estimates.
Figure 26 : Filter Performance (3 Satellites) Figure 27 : Filter Performance (1 Satellite)
26
To observe the comparative performance of the filter, errors in position and velocity for
all three cases are compared below:
Figure 28 : Estimation Error Velocity – Comparative Study with Satellite Visibility
Figure 29 : Estimation Error Position – Comparative Study with Satellite Visibility
As expected both the errors increase. However, even with one observation velocity error
accuracy of the order of 4 m/s and position error accuracy of the order of 20 m was
observed after 400 seconds. Hence, even in the undesirable situation of reduced satellite
filter the INS – Configuration EKF could give satisfactory performance for a short period
of time. Using more evolved architectures the performance during such situations can be
further enhanced.
27
8 Conclusion and Future Work
The GPS – INS Kalman filter was gradually developed by first developing the GPS Position
Velocity EKF and then proceeding on to the Integrated GPS – INS Kalman Filter. The filter
performance, presented in the previous section, was found to be satisfactory for two different
types of trajectories.
Following key-points were learnt after studying the above formulations and simulating them:
The process model should be as generic as possible so as to cater to variety of flight
trajectories. In the initial set of simulations this was not possible as no information
was available from the INS. In the integrated Kalman Filter, the process model was
written in terms of Error States rather than the actual states. This resulted, with
angular velocity information from the INS, in a generic process model which was
tested by simulations for two separate flight paths. Hence, writing the process model
in terms of error states is more beneficial.
The problem of jamming is a possibility when one uses GPS signals, especially in war
zones where in the opponent may try to jam the GPS signals in order to prevent
successful navigation. Thus it is important that the Integrated Kalman Filter is robust
and works in the case of reduced visibility. Our Integrated EKF was simulated for the
case of reduced GPS satellite visibility, and the errors were shown not to grow
drastically. However, for more robust performance deeply coupled approach should
be adopted.
The Integrated KF presented earlier is an open loop architecture, i.e. the INS
corrections obtained from the filter are not used to reset the INS. This results in
growth of INS error. Since, our simulation was for a trajectory of 600 seconds an
open loop architecture sufficed, although for longer flight trajectories, in order to
keep the errors tractable closed loop corrections should be applied to reset the INS
after regular time intervals.
The clock bias and drift in the simulations are states in both the EKF formulations.
Since, the values of these two quantities can be very small it is desirable to represent
the states in terms of range, by multiplying it with speed of light c.
Random Number Generation is a critical requirement while performing above
simulations. Although, the Matlab platform in which above simulations were done
offers function to create pseudorandom numbers (scalars), the Mahalanobis
Transformation comes to aid to generate pseudorandom vectors which have cross-
correlated covariance matrices. Lastly it is important to check whether the random
numbers generated using the pseudorandom number generators have desired
covariances.
The above simulations although give useful insights into the problem of aircraft
navigation using GPS, and specifically into the problem of GPS – INS integrations, the
results obtained are under certain assumptions. Development of models is required in
order to make the simulations more realistic.
28
Now, we highlight certain improvements that can be made in our simulation and alternate
strategies that can be looked into concerning our problem:
Error models currently used in the above simulations are above elementary.
Although, the error models have been more sophisticated as compared to the first
stage of the project, in order to make our system more realistic detailed error
models concerning errors in both GPS (Two-shell error model, Klobuchar error
model for ionospheric errors, etc.) and INS (drift, nonlinearities, etc.) should be
incorporated.
Alternative Architecture of GPS INS integration should be looked into and a
comparative study should be carried out so as to understand advantages and
disadvantages of each strategy. As mentioned earlier deeply coupled architecture
can make the system more robust in case of reduced satellite visibility.
The filter performance shown in previous sections is for a single run. Although
similar performance was observed for various random runs, in order to quantify
the confidence in an EKF it is necessary to carry out Monte Carlo simulations.
Monte Carlo simulations involve various reruns of the simulations so as to
quantify based on probability the confidence that can be put in the filter.
Initialization of the filter and that of the inertial navigation system is a problem in
itself. How to initialize the system based on available information should be
explored further.
In the above simulations there was no mention of computational power required to
operate the integrated algorithm. Since, the filter goes on-board a flight vehicle
there is constraint on the amount of computational resources available, hence
choice of integration architecture depends on the flight computer capability. This
aspect of algorithm should be looked into.
Lastly, in our study we have considered only the Navigation of a flight vehicle.
For instance, bank command was used to obtain the desired bank-to-turn
trajectory, however how this bank command is obtained (the control and dynamics
sub-system) and implemented (the flight actuators) is assumed to be ideal and not
simulated. Thus, in order to obtain a holistic view it will be helpful if the
navigations sub-system is simulated along with other related sub-systems such as
flight guidance and control.
29
9 References
[1] Bar-Shalom, Y., Li, X., & Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. John Wiley & Sons.
[2] Brown, R. G., & Hwang, P. Y. (1996). Introduction To Random Signals And Applied Kalman Filtering, 3rd Edition. John Wiley & Sons.
[3] Grewal, M., Weill, L., & Andrews, A. (2007). Global Positioning Systems, Inertial Navigation And Integration. John wiley & Sons.
[4] Groves, P. D. (2008). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House. [5] Mahalanobis Transformation. (n.d.). Retrieved September 15, 2010, from http://fedc.wiwi.hu-berlin.de/xplore/tutorials/mvahtmlnode34.html [6] Mathworks. (n.d.). Matlab Documentation. [7] Pamadi, B. N. (1998). Performance, Stability, Dynamics, and Control of Airplanes. AIAA
Education Series. [8] Parkinson, B. (1996). Global Positioning System: Theory & Applications (Volume One) .
[9] Rogers, R. M. (2007). Applied Mathematics in Integrated Navigation Systems, 3rd Edition. AIAA Education Series.
Apart from the above references which were of great help, I would also like to acknowledge the help obtained from my supervisor Prof. H. Hablani, and the Aerospace Curriculum at the Department of Aerospace Engineering; for providing the knowledge required (spread across various courses), to carry out the above study.
