PROJECT REPORT

Study of Inertial Navigation Systems

and
Aiding of INS solutions with GPS sensor
readings

Kunal Dhawan
Sophomore , Electronics and Communication Engineering
Indian Institute of Technology , Guwahati
Roll Number 150102030
[email protected]
 Problem Statement

To obtain navigation solution (position, velocity, attitude) and their accuracies

using inertial sensors i.e. accelerometers and gyroscopes. This will be the
inertial navigation system (INS) output. Thereafter ,fuse this solution with GPS
readings ( available at much lower frequency) using Kalman Filter to improve
the accuracy of the final solution .
 Introduction to Major Concepts

Navigation

Navigation may be defined as the process of determining vehicle position (also

known as Localization). This is distinct from Guidance (or Control) which is the
process of controlling a vehicle to achieve a desired trajectory. An autonomous
vehicular system generally must include these two basic competencies in order
to perform any useful task.

An Historical Perspective

The navigation information that is generally needed is orientation, speed and

position of the vehicle. Modern navigation techniques were introduced several
hundred years ago to aid vessels crossing the ocean. The first navigation
techniques were used to estimate the position of a ship through dead
reckoning, using observations of the ships speed and heading. With this
information, the trajectory of the ship was able to be predicted, albeit with
errors that accumulated over time. The heading of the ship was determined by
observing the position of the sun, or other stars. The velocity was found with a
log, compensating for the effects of local currents. Absolute information was
used to provide a position fix, to compensate for the errors accumulated
through dead reckoning. These fixes were obtained when well known natural or
artificial landmarks were recognized.

A new navigational problem arose amongst the explorers of the fifteenth

century. Even with the invention of the compass, dead reckoning was only
useful for short periods of time due to difficulties in obtaining accurate
measurements of velocity. The difficulties in compensating for local currents
made dead reckoning only useful for periods of a few hours. In the open sea,
natural landmarks are scarcely available, making an accurate position update
not possible. It was clear at that time that new methods, or new sensors, were
needed to obtain absolute position information.

This problem was of fundamental economic importance since many ships were
lost at sea due to total uncertainty in Longitude. Two approaches were mainly
investigated at this time. One method was based on accurate prediction and
observation of the moon. Difficulties in predicting the lunar orbit with enough
precision and accuracy required of the instrumentation made this approach
almost impossible to implement in standard ships at that time. The other
important approach was based on knowing the time with enough accuracy to
evaluate the Longitude.
A Modern Perspective

The previous section introduced the essential elements of navigation, Prediction

and Correction. Prediction can be considered to be the use of a model of some
description to provide dead reckoning information. Dead reckoning has the
property of accumulating position error with time. Correction is the process
whereby the observation of landmarks (either natural or artificial) can reduce
the location uncertainty inherent in dead reckoning. It may be argued that with
the advent of modern sensors such as the Global Positioning System (GPS) that
dead reckoning is no longer a necessary part of navigation. This assertion may
be readily refuted by the following statement: There is no such thing as a
perfect sensor. All sensors have some measure of error or uncertainty present
within every measurement. Similarly, if it were possible to perfectly model
vehicle motion, external sensors would not be needed. Therefore it is essential
to understand not only the sensors used for navigation, but also the model
used for prediction, as they both contribute to the fidelity of the position
solution. As both prediction and correction steps contain uncertainty, it is
useful to pose navigation as an Estimation problem. If the error in prediction
and the error in correction can be modeled as probability distributions then the
Kalman filter can be used to fuse all available information into a common
estimate that may then be used for guidance.

The Kalman Filter

A consistent methodology for estimating position from navigation sensors is

through the use of Kalman filtering and, for nonlinear systems, through the use
of the extended Kalman filter. The Kalman filter is a linear statistical algorithm
used to recursively estimate the states of interest. The states of interest will
usually consist of the vehicle pose and other relevant vehicle parameters. In
map building, the state vector can be augmented with feature positions, so that
they too may be estimated. To aid in the estimation of the states, the Kalman
filter requires that there be two mathematical models: the process model and
the observation model. The process model describes how the states evolve over
time, together with an estimate of the errors committed by the system. The
observation model explicitly describes the information supplied by a sensor as
a function of the state, together with a model of measurement noise. These
models correspond to prediction and correction respectively. For a linear
system subject to Gaussian, uncorrelated, zero mean measurement and process
noises, the Kalman filter is the optimal minimum mean squared error
estimator. An additional benefit of using the Kalman filter estimator is that it
keeps track of the uncertainties present in the system via the computation of a
covariance matrix. This is important in many applications where it is desired to
know how well (or how poorly) a system is performing.
 Introduction to important Navigation Systems

1) Inertial Navigation System (INS) : An inertial navigation system (INS),

sometimes known as an inertial navigation unit (INU), is a complete three-
dimensional dead-reckoning navigation system. It comprises a set of inertial
sensors, known as an inertial measurement unit (IMU), together with a
navigation processor. The inertial sensors usually comprise three mutually
orthogonal accelerometers and three gyroscopes aligned with the
accelerometers. The navigation processor integrates the IMU outputs to give the
position, velocity, and attitude. Inertial navigation system is a dead reckoning
type of navigation system that computes its position based on motion sensors.
Once the initial latitude and longitude is established, the system receives
impulses from motion detectors that measure the acceleration along three or
more axes enabling it to continually and accurately calculate the current
latitude and longitude. Its advantages over other navigation systems are that,
once the starting position is set, it does not require outside information, it is
not affected by adverse weather conditions and it cannot be detected or
jammed. Its disadvantage is that since the current position is calculated solely
from previous positions, its errors are cumulative, increasing at a rate roughly
proportional to the time since the initial position was input. Inertial navigation
systems must therefore be frequently corrected with a location 'fix' from some
other type of navigation system.

2) Global Navigation Satellite System (GNSS) : GNSS is the term for satellite
navigation systems that provide positioning with global coverage. A GNSS
allows small electronic receivers to determine their location (longitude, latitude,
and altitude) to within a few metres using time signals transmitted along a line
of sight by radio from satellites. Receivers on the ground with a fixed position
can also be used to calculate the precise time as a reference for scientific
 Coordinate Frames Used in Navigation

1)Earth-Centered Inertial Frame(i) : an inertial coordinate frame is one that does

not accelerate or rotate with respect to the rest of the Universe. The i frame is
centered at the Earths center of mass and oriented with respect to the Earths
spin axis and the stars. The z-axis always points along the Earths axis of
rotation from the center to the north pole (true, not magnetic). The x- and y-
axes lie within the equatorial plane.

Figure : Axis representation in I frame

2) Earth-Centered Earth-Fixed Frame(e): The e frame rotates along with earth ,

thus all its axes remain fixed with respect to the Earth . The z-axis always
points along the Earths axis of rotation from the center to the North Pole (true,
not magnetic). The x-axis points from the center to the intersection of the
equator with the IERS reference meridian (IRM) or conventional zero meridian
(CZM), which defines 0 degree longitude. The y-axis completes the right-handed
orthogonal set, pointing from the center to the intersection of the equator with
the 90-degree east meridian

Figure : Axis representation in E frame

3) Local Navigation Frame (n) : It is also called the geodetic or geographic frame.
It is centered at the host vehicles center of mass and is the most preferred
frame for calculation of navigation solution . The z axis, also known as the
down (D) axis, is defined as the normal to the surface of Earth . The x-axis, or
north (N) axis, is the projection in the plane orthogonal to the z-axis of the line
from the user to the north pole. By completing the orthogonal set, the y-axis
always points east and is hence known as the east (E) axis.

Figure : Axis representation in N frame

 Introduction to Inertial Navigation
An inertial navigation system (INS), sometimes known as an inertial navigation
unit (INU), is a dead-reckoning navigation system, comprising an inertial
measurement unit and a navigation processor. The IMU, incorporates a set of
accelerometers and gyros produces measurements of the specific force and
angular rate of the body frame with respect to inertial space in body-frame
axes.

Figure : Schematic of an inertial navigation processor

One of my project objectives was to design and implement equations which

could help us track the position , velocity and attitude of our ship / submarine
given the sensor readings from accelerometer and gyroscope . These equations
are called navigation equations .

Navigation Equations comprises of 4 steps:-

1) Attitude update
2) Specific-Force Frame Transformation
3) Velocity update
4) Position update
 figure : flow of navigation processor using the navigation equations at each step

Thus , for my project , I coded and implemented three versions of Navigation

equations (the version varied on the level of complexity and load for navigation
processor ) . All of them were implemented in the N frame as for all practical
purposes , the position and velocity solution are required in body frame of the
vehicle for the solutions to be comprehendible . The three versions of
Navigation equations were as follows :

1) Linearized Navigation Equations

2) Precision Navigation Equations
3) Differential Equations Navigation Equations

In the following section , I only mention the final equation that are
implemented . Link to full derivation will be provided in the references section .

Please NOTE : List of symbols and their meanings are provided at the end of the
report .
 Linearized Navigation Equations

1)Attitude update

2) Specific-Force Frame Transformation

3) Velocity Update

4) Position Update
 Precision Navigation Equations

1) Attitude update

2) Specific-Force Frame Transformation

3) Velocity Update

4) Position Update
 Differential Equation implementation of Navigation
Equations

1) Attitude update

2) Specific-Force Frame Transformation

3) Velocity Update

This was done solving the following systems of ordinary differential equations-
This was achieved using the inbuilt function ODE45 of MATLAB . Snippets of
code for the same are as follows :
(some portion has been omitted fit the explanation of the method used to 2
pages)

Here:
dx(1)=North velocity
dx(2)=East velocity
dx(3)=Down velocity
dx(4)=Latitude
dx(5)=Height
 GPS Aiding of Inertial Navigation Processor

Need for GPS addition

Inertial navigation has a number of advantages. It operates continuously, bar

hardware faults; provides high-bandwidth output at atleast 50 Hz; and exhibits
low short-term noise. It also provides effective attitude, angular rate, and
acceleration measurements, as well as position and velocity. However, the
accuracy of an inertial navigation solution degrades with time as the inertial
instrument errors are integrated through the navigation equations. GNSS
provides a high long-term position accuracy with errors limited to a few meters
(stand-alone). However, compared to INS, the output rate is low, typically
around 10 Hz, the short-term noise of a code-based position solution is high,
and standard GNSS user equipment does not measure attitude. GNSS signals are
also subject to obstruction and interference, so GNSS cannot be relied upon to
provide a continuous navigation solution. The benefits and drawbacks of INS
and GNSS are complementary, so by integrating them, the advantages of both
technologies are combined to give a continuous, high-bandwidth, complete
navigation solution with high long- and short-term accuracy. In an integrated
INS/GNSS, or GNSS/INS, navigation system, GNSS measurements prevent the
inertial solution drifting, while the INS smoothes the GNSS solution and bridges
signal outages.

Figure : Block Diagram for a general INS and GPS integration architecture
 Integration of INS and GPS data available at different
rates

How to accurately combine the information available from two sources ,

namely INS and GNSS so as to get a precise solution ? This problem is solved
with the help of Kalman Filter

The Kalman filter is an estimation algorithm, rather than a filter. It maintains

real-time estimates of a number of parameters of a system, such as its position
and velocity, that may continually change. The estimates are updated using a
stream of measurements that are subject to noise. The measurements must be
functions of the parameters estimated, but the set of measurements at a given
time need not contain sufficient information to uniquely determine the values
of the parameters at the time. The Kalman filter uses knowledge of the
deterministic and statistical properties of the system parameters and the
measurements to obtain optimal estimates given the information available. To
do this, it must carry more information from iteration to iteration than just the
parameter estimates. Therefore, the Kalman filter also maintains a set of
uncertainties in its estimates and a measure of the correlations between the
errors in the estimates of the different parameters.

The Kalman filter is a Bayesian estimation technique. It is supplied with an

initial set of estimates and then operates recursively, updating its working
estimates as a weighted average of their previous values and new values
derived from the latest measurement data. By contrast, nonrecursive estimation
algorithms derive their parameter estimates from the whole set of
measurement data without prior estimates. For real-time applications, such as
navigation, the recursive approach is more processor efficient, as only the new
measurement data need be processed on each iteration. Old measurement data
may be discarded.

The five core elements of the Kalman filter are : the state vector and covariance,
the system model, the measurement vector and covariance, the measurement
model, and the algorithm:

1. The state vector is the set of parameters describing a system, known as

states, which the Kalman filter estimates. Associated with the state vector is an
error covariance matrix. This represents the uncertainties in the Kalman filter's
state estimates and the degree of correlation between the errors in those
estimates.

2. The system model, also known as the process model or time-propagation

model, describes how the Kalman filter states and error covariance matrix vary
with time. The system model is deterministic for the states as it is based on
known properties of the system.The variation in the true values of the states is
known as system noise, and its statistical properties are usually estimated in
advance by the Kalman filter designer.

3. The measurement vector is a set of simultaneous measurements of

properties of the system, which are functions of the state vector. Associated
with the measurement vector is a measurement noise covariance matrix, which
describes the statistics of the noise on the measurements.

4. The measurement model describes how the measurement vector varies as a

function of the true state vector (as opposed to the state vector estimate) in the
absence of measurement noise. Like the system model, the measurement model
is deterministic, based on known properties of the system.

5. The Kalman filter algorithm uses the measurement vector, measurement

model, and system model to maintain optimal estimates of the state vector.

Figure : Flow diagram of working of a Kalman Filter Algorithm

 Implementation
The integrated navigation solution of an INS/GNSS integrated navigation system
is the corrected inertial navigation solution. In an integration architecture
using an error-state Kalman filter and separate inertial navigation processing,
correction may be either open-loop or closed-loop . These two ways differ in
their approach of feeding back of position , velocity and attitude values to the
INS processor ( remember INS is a dead reckoning system , hence it requires the
above mentioned values at previous time intervals so as to calculate the
solution at the next time instant )

In the open-loop configuration, the estimated position, velocity, and attitude

errors are used to correct the inertial navigation solution within the integration
algorithm at each iteration but are not fed back to the INS.

In the closed-loop configuration, the estimated position, velocity, and attitude

errors are fed back to the inertial navigation processor, either on each Kalman
filter iteration or periodically, where they are used to correct the inertial
navigation solution itself. The Kalman filters position, velocity, and attitude
estimates are zeroed after each set of corrections is fed back. Consequently,
there is no independent uncorrected inertial navigation solution. In simple
words , we feed back the correct position and velocity values to the INS
processor for future calculations.

Figure - Comparison of open and closed loop corrections

Currently , in literature and practice , Closed loop corrections is largely
preferred and there is no work available on open loop correction . This is
because performance of closed loop is much better as compared to open loop
corrections as we always feedback the corrected values to the INS processor
and hence the INS error increases very slowly over time .

But due to constraints of unavailability of access to the navigation processor ,

my task was to design and implement the open loop correction algorithm which
will work in the given constraints .

Thus , I formulated the algorithm and coded a total of three implementations ,

which are listed as follows :

1) Loosely coupled integration of INS and GPS with closed loop correction (E
frame calculation)
2) Loosely coupled integration of INS and GPS with open loop correction (N
frame implementation)
3) Loosely coupled integration of INS and GPS with open loop correction (E
frame implementation)

Results for all the above are presented after the next section .
Working of Code ( Loosely coupled Integration of INS and
GPS sensor data with Open Loop Corrections)

Initialize INS , GPS and Kalman Filter Biases

Initialize the x(x vector) , P(error covariance matrix) , Q(system noise covariance
matrix) and R(measurement noise covariance matrix) matrices for working of
the Kalman filter

Generate the true Position , Velocity and Attitude values from the trajectory of
the vehicle

Now repeat the following steps for each epoch (ie each time we receive sensor
values from the IMU

Determine Position and Velocity of all the satellites present in the constellation

Generate GNSS measurements ( ie pseudo range and range rate values using the
above information )

Find the GPS values for object vehicle position and velocity using the range and
range rate values
Generate the accelerometer and gyro reading using the true trajectory and add
appropriate errors to the sensor readings

Find the INS values for vehicle position and velocity using the navigation
equations on the inertial sensor readings

Now run the Kalman filter function (Details of the same cant be revealed
currently as we are writing a paper on the same)

Next we store the calculated position , velocity and attitude values in a matrix
which helps us plot the trajectory . After this , we return to the next iteration of
the main for loop and wait of the INS sensor readings
 Results and Plots

A) Loosely Coupled Integration of INS and GPS with Closed loop

correction

Two Hour Simulation Results

Figure : True path of the vehicle ( shown in N frame )

Figure : Path followed by the vehicle ( shown in N frame )

Figure : Corresponding error for the vehicle trajectory

 24 Hour Simulation Results

Figure : True path of the vehicle ( shown in N frame )

Figure : Path followed by the vehicle ( shown in N frame )

Figure : Corresponding error for the vehicle trajectory

 48 Hour Simulation Results

Figure : True path of the vehicle ( shown in N frame )

Figure : Path followed by the vehicle ( shown in N frame )

Figure : Corresponding error for the vehicle trajectory

 B) Loosely Coupled Integration of INS and GPS with Open loop
correction

1) N frame implementation :

One Hour Simulation Results

Figure : True path of the vehicle ( shown in N frame )

Figure : Path followed by the vehicle ( shown in N frame )

Figure : Corresponding error for the vehicle trajectory

Comparison of the algorithm with simple INS solution for
the same trajectory

For comparing the results obtained from the algorithm implemented with the
case when no correction was provided by GPS integration , I plot the errors
accumulated by both the methods over a period of 1 hour when both the
algorithms are subjected to the same path and sensor errors . The plots have
plotted in the N frame .

Figure : Comparison plot for error in latitude reading introduced by all three algorithms
Figure : Comparison plot for error in longitude reading introduced by all three algorithms

Figure : Comparison plot for error in Height reading introduced by all three algorithms
 Explanation of Working of Open Loop corrections :

In open loop integration of INS and GPS readings , the formulation of x vector (
for Kalman Filter ) is done is the following way ( for N frame ) :

Thus , using the X vector , we try to track the errors in the INS which integrate
with time and use this to correct the readings that we get from the IMU sensors
.

To show the working of my algorithm , I plot the x vector along with INS errors
across a duration of 1 hour . It can be seen that the algorithm is doing a good
job to approximate the INS errors
Figure : Plot showing the propagation of the latitude reading component of x vector and the
INS errors plot with time .

Figure : Plot showing the propagation of the longitude reading component of x vector and the
INS errors plot with time .
Figure : Plot showing the propagation of the height reading component of x vector and the INS
errors plot with time .
1) E frame implementation :

One Hour Simulation Results

Figure : True path of the vehicle ( shown in N frame )

Figure : Path followed by the vehicle ( shown in N frame )

Figure : Corresponding error for the vehicle trajectory

Comparison of the algorithm with simple INS solution for
the same trajectory

For comparing the results obtained from the algorithm implemented with the
case when no correction was provided by GPS integration , I plot the errors
accumulated by both the methods over a period of 1 hour when both the
algorithms are subjected to the same path and sensor errors . The plots have
plotted in the E frame .

Figure : Comparison plot for error in x position reading introduced by the two algorithms
Figure : Comparison plot for error in y position reading introduced by the two algorithms

Figure : Comparison plot for error in z position reading introduced by the two algorithms
 Tracking Due to incorrect knowledge of lever arm

Concept Lever arm

Sometimes, there is a requirement to transpose a navigation solution from one

position to another on a vehicle, such as between an INS and a GPS antenna,
between an INS and the center of gravity, or between a reference and an
aligning INS. This is required because generally the GPS antenna and IMU
sensors are present at different locations on the vehicle . But when integrating
the position and velocity solutions from both together to a common solution ,
we need to convert the readings to a common frame so that they can be
appropriately added . This is done using the lever arm , which is a vector in 3
Dimension pointing from one frame to the other , thus it helps us in
transporting the solution to the required frame

Problem statement

Given the scenario that we would have multiple GPS receivers on board and
readings could be available from any of them without previous knowledge(ie we
dont know the algorithm via which the GPS receivers are chosen and hence we
cant incorporate that in the integration Kalman filter and get the correct lever
arm for whichever GPS is chosen). Thus we have the knowledge of only one
lever arm but the reading may not be coming from that GPS receiver , so we
have to estimate the error in the final position output due to this mismatch.

How does this affects the setup of Kalman Filter :

As the problem of lever arm affects the GPS readings, thus it only affects the
measurement model of the Kalman Filter . Specifically , it affects the z vector
and H matrix formation . The following equations highlight the affect of lever
arm on the above mentioned matrices

Z vector formation :
Measurement Matrix formulation :

Explaining the simulation

Tested the code for 3 cases with incorrect lever arm of around 5 to 8 meters as
required. Used 3 random positions of the GPS antenna with respect to the INS
body axis origin .

The cases used were

[0;0;0] the basis case ; [5;5;5] norm = 8.66m ; [4.5,3.7,6.5] norm = 8.728m ;
[7.5;3;3.2] norm = 8.69m ; [3,3,3] norm = 5.196m ; [2.5;3.1;2.8] norm =
4.868m
 Results and Plots

2 Hour Simulation Results

Figure plot showing variation of North Position Error with time for the different cases of lever
arm as mentioned in the Key
Figure plot showing variation of East Position Error with time for the different cases of lever
arm as mentioned in the Key

Figure plot showing variation of Down Position Error with time for the different cases of lever
arm as mentioned in the Key
 48 hours simulation results

Figure plot showing variation of North Position Error with time for the different cases of lever
arm as mentioned with the plots
Figure plot showing variation of East Position Error with time for the different cases of lever
arm as mentioned with the plots

Figure plot showing variation of Down Position Error with time for the different cases of lever
arm as mentioned with the plots
Figure plot showing variation of North Velocity Error with time for the different cases of lever
arm as mentioned with the plots

Figure magnified version of above plot for first 1000 seconds

Figure plot showing variation of East Velocity Error with time for the different cases of lever
arm as mentioned with the plots

Figure magnified version of above plot for first 1000 seconds

Figure plot showing variation of Down Velocity Error with time for the different cases of lever
arm as mentioned with the plots

Figure magnified version of above plot for first 1000 seconds