Navigation System Design
(KC-3)
Eduardo Nebot
Centre of Excellence for Autonomous Systems
The University of Sydney
nebot@cas.edu.au
April 2006
Navigation System Design
Eduardo Nebot
Slide 1
Course Outline
Part 1 Presents the introductory navigation topics of vehicle modelling, beacon (known feature)
based navigation. The Fundamental principles of Global Positioning System is also presented.
Laboratory: localisation assignment with outdoor experimental data.
Part 2 Presents an introduction to inertial sensors, inertial navigation and GPS / INS data fusion
algorithms. laboratory: Calibration, Alignment and integration of data obtained with a full six
degree of freedom inertial measurement unit (IMU), Integration of GPS/INS data.
Part 3 Presents the fundamentals of Simultaneous Localisation and Mapping. Optimal and
Suboptimal algorithms to reduce the computational and memory requirements of SLAM. laboratory:
SLAM assignment with outdoor experimental data.
Navigation System Design
Eduardo Nebot
Slide 2
Navigation Systems
Navigation ( Localisation ) may be defined as the process of determining vehicle pose, that is:
• vehicle position
• vehicle orientation
• vehicle velocity
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.
In this course, the problem of navigation is discussed in detail. Probabilistic methods are used in
order to perform accurate, predictable localisation.
Navigation System Design
Eduardo Nebot
Slide 3
An Historical Perspective
The first navigation techniques were used to estimate the position of a ship through dead reckoning,
using observations of the ships speed and heading.
Absolute information was used to provide a position fix. These fixes were obtained when well known
natural or artificial landmarks were recognised. In the open sea, natural landmarks are scarcely
available, making an accurate position update not possible.
Techniques to determine Latitude were developed in the early 1550’s by the Portuguese.
The determination of Longitude took another 300 years to be solved. The approaches were based
on accurate prediction and observation of the moon and by knowing the time with enough accuracy
to evaluate the Longitude.
Captain Cook tested one of Harrison’s chronometers (the Mark IV), on his last journey.
Navigation System Design
Eduardo Nebot
Slide 4
A Modern Perspective
The previous slide introduced the essential elements of navigation, Prediction and Correction.
Prediction can be considered to be the use of a model or some description to provide dead
reckoning information.
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 is not true since 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.
Navigation System Design
Eduardo Nebot
Slide 5
A Modern Perspective
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 accuracy 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 modelled 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.
Navigation System Design
Eduardo Nebot
Slide 6
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. 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. It also keeps track of the
uncertainties in the estimates.
Navigation System Design
Eduardo Nebot
Slide 7
Vehicle Modelling
The vehicle model can be a very important component of a navigation system.
One of the most important issues with modelling is knowing the limitation of the model selected.
For example, in the case of a pure kinematic model it is important to know the operational range
where the model is valid. A simple kinematics model can be very accurate to predict the trajectory of
a vehicle while moving in straight direction.
However it can have significant degradation when:
• turning ( tire deformation )
• under different load conditions ( changing wheel radius).
Navigation System Design
Eduardo Nebot
Slide 8
Vehicle Modelling
There are different type of models that can be used for a particular application:
• Kinematics models
• Dynamics models
• Lumped parameter models
In this course basic kinematics Ackerman steered vehicle models will be presented and used as a
running example for the different navigation algorithms introduced.
Navigation System Design
Eduardo Nebot
Slide 9
Kinematic Models
One of the simplest method of vehicle modelling is simply to exploit the vehicle’s kinematic
constraints. These constraints are known as the rigid body and rolling motion constraints.
The rigid body constraint makes the assumption that the vehicle frame is rigid, wheras the rolling
motion constraint assumes that all points on the vehicle rotate about the Instantaneous Centre of
Rotation with the same angular velocity.
It is further assumed that there is no slip between the vehicle tyres and the ground, and that vehicle
motion may be adequately be represented as a two-dimensional model whose motion is restricted to a
plane.
Navigation System Design
Eduardo Nebot
Slide 10
Kinematic Models
It is often convenient for modelling purposes to replace the four wheels of an ackerman steered vehicle
with two ‘virtual’ wheels located along the vehicle centerline as can be seen in Figure 1.
These virtual wheels encapsulate the two degrees of freedom we are left with once rigid body and
rolling motion constraints are applied.
gl
gr
x
y
B
'Virtual' Wheels
O
W
Rrr
Figure 1: Ackerman Steered Vehicle Geometry
Navigation System Design
Eduardo Nebot
Slide 11
Basic Kinematic Model
Consider the vehicle model shown in Figure 2, it can be shown that the continuous time form of the
vehicle model (with respect to the center of the front wheel) can be derived as follows:
ẋ(t) = V (t) cos(φ(t) + γ(t))
ẏ(t) = V (t) sin(φ(t) + γ(t))
V (t) sin(γ(t))
φ̇(t) =
B
(1)
V = wR
g
x
y
B
O
Figure 2: Simplified Kinematic Vehicle Model
where x(t) and y(t) denote the position, φ(t) is the orientation with respect to the x axis, and V (t) is
the velocity of the front wheel. γ is defined as the steer angle and B is the base length.
Navigation System Design
Eduardo Nebot
Slide 12
Basic Kinematic Model
It should be noted that for nonlinear models such as this, a closed form solution for the discrete time
vehicle model can not in general be found by integrating Equation 1 between the time intervals tk
and tk−1 as is possible in the linear case.
For nonlinear models, an Euler approximation of the integral can be used, assuming the control
inputs are approximately constant over this interval. This approximation yields a discrete time
vehicle model of the form
x(k) = x(k − 1) + ∆T V (k − 1) cos(φ(k − 1) + γ(k − 1))
y(k) = y(k − 1) + ∆T V (k − 1) sin(φ(k − 1) + γ(k − 1))
V (k − 1) sin(γ(k − 1))
φ(k) = φ(k − 1) + ∆T
B
(2)
where ∆T is defined as the sample time from tk−1 to tk . The input of this system are the wheel’s
angular velocity ω and steer angle γ.
Navigation System Design
Eduardo Nebot
Slide 13
Importance of Modelling
1. LHD (Paper)
2. Straddle Carrier
3. Haul Trucks
Navigation System Design
Eduardo Nebot
Slide 14
Kinematic Model of the Utility Car (UTE)
This section presents a more realistic model of a standard outdoor vehicle shown in Figure 3.
In general the models that predict the trajectory of the vehicle and the models that relates the
observation with the states are non-linear. The navigation algorithms to be used require the
linearisation of these models. In this case the Jacobian of the process and observation are needed
to propagate the uncertainties.
Figure 3: Utility car used for the experiments. The vehicle is equipped with a Sick laser range and bearing sensor, linear variable differential transformer sensor
for the steering and back wheel velocity encoders.
Navigation System Design
Eduardo Nebot
Slide 15
Kinematic Model of the Utility Car (UTE)
The vehicle is equipped with dead reckoning capabilities and an external sensor capable of measuring
relative distance between vehicle and the environment as shown in Figure 4.
The steering control α, and the speed υc are used with the kinematic model to predict the position of
the vehicle. The external sensor returns range and bearing information to features Bi(i=1..n) with
respect to the vehicle coordinates (xl , yl ), that is z(k) = (r, β) , where r and β are range and bearing
to this feature.
Bi
b
z(k)=(r,b)
yL
B3
yl
xl
f
vc
a
B1
xL
Figure 4: Vehicle Coordinate System
Navigation System Design
Eduardo Nebot
Slide 16
Kinematic Model of the Utility Car (UTE)
Considering that the vehicle is controlled through a demanded velocity υc and steering angle α the
process model that predicts the trajectory of the centre of the back axle is given by




ẋ
v · cos (φ)
 c  c

  

 ẏc  =  vc · sin (φ)  + γ
  

vc
φ̇c
L · tan (α)
(3)
Where L is the distance between wheel axles as shown in Figure 5.
Laser
Encoder
yl
xl
H
Pc (yc, xc)
yL
b
L
xL
a
Figure 5: Kinematics parameters
Navigation System Design
Eduardo Nebot
Slide 17
Kinematic Model of the Utility Car (UTE)
The kinematic model of the vehicle is designed to represent the trajectory of the centre of the laser.
Based on Figure 4 and 6, the translation of the centre of the back axle can be given
PL = PC + a · T~φ + b · T~φ+π/
2
(4)
Laser
Encoder
yl
xl
H
Pc (yc, xc)
yL
b
L
xL
a
Figure 6: Kinematics parameters
PL and PC are the position of the laser and the centre of the back axle in global coordinates
respectively.
Navigation System Design
Eduardo Nebot
Slide 18
Kinematic Model of the Utility Car (UTE)
The transformation is defined by the orientation angle, according to the following vectorial expression:
T~φ = (cos (φ) , sin (φ))
(5)
The scalar representation is
xL = xc + a · cos (φ) + b · cos (φ + π/2)
yL = yc + a · sin (φ) + b · sin (φ + π/2)
Finally the full state representation can be written
 


vc · cos (φ) − vLc · (a · sin (φ) + b · cos (φ)) · tan (α)
ẋL
 


 


v
 ẏL  =  vc · sin (φ) + Lc · (a · cos (φ) − b · sin (φ)) · tan (α)  + γ
 


vc
φ̇L
L · tan (α)
The velocity, υc , is measured with an encoder located in the back left wheel. This velocity is
translated to the centre of the axle with the following equation:
νe
¢
vc = ¡
H
1 − tan (α) · L
Navigation System Design
Eduardo Nebot
Slide 19
(6)
(7)
Kinematic Model of the Utility Car (UTE)
Finally the discrete model in global coordinates can be approximated with the following set of
equations:





 


x(k)

 

 
 y(k)  = 

 


φ(k)





x(k − 1) + ∆T vc(k − 1) · cos (φ(k − 1)) −


vc

·
(a
·
sin
(φ(k
−
1))
+
b
·
cos
(φ(k
−
1)))
L



· tan (α(k − 1))


y(k − 1) + ∆T vc(k − 1) · sin (φ(k − 1)) +  + γ


vc (k−1)

·
(a
·
cos
(φ(k
−
1))
−
b
·
sin
(φ(k
−
1)))
L



· tan (α(k − 1))

vc (k−1)
φ(k − 1) + L · tan (α(k − 1))
where ∆t is the sampling time, that in this case is not constant.
Navigation System Design
Eduardo Nebot
Slide 20
(8)
Kinematic Model of the Utility Car (UTE)
The observation equation relating the vehicle states to the observations is

q
2
2
(xi − xL) + (yi − yL)
zri



 + γh
³
´
z = h (X, xi, yi) =
=
(y
−y
)
atan (xii−xL ) − φL + π/2
zβi
L


(9)
where z is the observation vector, is the coordinates of the landmarks, xL , yL and φL are the vehicle
states defined at the external sensor location and γh the sensor noise.
The complete non-linear model can be expressed in general form as:
X (k + 1) = F (X (k) , u (k) + γu (k)) + γf (k)
(10)
z (k) = h (X (k)) + γh (k)
where γu (k) , γh (k) and γf (k) are white Gaussian noise of the input, process and observation
respectively.
Navigation System Design
Eduardo Nebot
Slide 21
Kinematic Model of the Utility Car (UTE)
The propagation of uncertainties and estimation of states will require the evaluation of the Jacobians
of the process and measurement models.
The Jacobian of the Process model with respect to the state variables is:


1 0 −∆T (vc sin φ + vLc tan α(a cos φ − b sin φ)

∂F

Fx =
=  0 1 ∆T (vc cos φ − vLc tan α(a sin φ − b cos φ)
∂X 
0 0
1




where vc needs to be replaced by:
vc = ¡
Navigation System Design
νe
¢
1 − tan (α) · HL
Eduardo Nebot
(11)
Slide 22
Kinematic Model of the Utility Car (UTE)
The evaluation of the Jacobian with respect to the input require a few additional mathematical steps.
Two cases are presented here:
• Using Steering encoder:
φ̇ =
1
· v1 · β
L
with
β = tan α
• Using Gyro information:
φ̇ = φz
Navigation System Design
Eduardo Nebot
Slide 23
Kinematic Model of the Utility Car (UTE)
The general kinematic model is


v · cos (φ) + φ̇ · ηx (φ)
 1



F v1, φ̇ =  v1 · sin (φ) + φ̇ · ηy (φ) 


φ̇
v2
v1 =
1 − β HL
1
φ̇ = · v1 · β or φ̇ = φz
L
³
´
G (v2, β, φz ) = F v1, φ̇
³
´
with
ηx = −(a sin φ + b cos φ)
ηy = (a cos φ − b sin φ)
Navigation System Design
Eduardo Nebot
Slide 24
Kinematic Model of the Utility Car (UTE)
The general partial derivatives with respect to the three inputs:
∂G ∂F ∂v1 ∂F ∂ φ̇ ∂v1
=
·
+
·
·
∂v2 ∂v1 ∂v2 ∂ φ̇ ∂v1 ∂v2
Ã
!
∂G ∂F ∂v1 ∂F
∂ φ̇ ∂v1 ∂ φ̇
=
·
+
·
·
+
∂β
∂v1 ∂β
∂v1 ∂β ∂β
∂ φ̇
∂G
∂F ∂ φ̇
·
=
∂φz
∂ φ̇ ∂φz
h
iT
∂F
= cos (φ) sin (φ) 0
∂v1
iT
∂F h
= ηx (φ) ηy (φ) 1
∂ φ̇
Navigation System Design
Eduardo Nebot
Slide 25
Kinematic Model of the Utility Car (UTE)
∂G ∂F ∂v1 ∂F ∂ φ̇ ∂v1
=
·
+
·
·
∂v2 ∂v1 ∂v2 ∂ φ̇ ∂v1 ∂v2
Ã
!
∂G ∂F ∂v1 ∂F
∂ φ̇ ∂v1 ∂ φ̇
=
+
·
+
·
·
∂β
∂v1 ∂β
∂v1 ∂β ∂β
∂ φ̇
∂G
∂F ∂ φ̇
=
·
∂φz
∂ φ̇ ∂φz
∂v1
1
=
,
∂v2 1 − β HL
∂v1
v2
H
=¡
·
¢
2
∂β
L
1 − β HL
∂ φ̇
β
∂ φ̇
=
or
=0
∂v1 L
∂v1
∂ φ̇
∂ φ̇ v1
=
or
=0
∂β
L
∂v1
∂ φ̇
∂ φ̇
= 0 or
=1
∂φz
∂φz
Navigation System Design
Eduardo Nebot
Slide 26
Kinematic Model of the Utility Car (UTE)
If the steering angle is used to predict (as the derivative) the orientation then the gradient is
h
∂G
=
∂ (v2, β, φz )
i
∂G
∂v2
∂G
∂β
∂G
∂φz
∂G ∂F ∂v1 ∂F ∂ φ̇ ∂v1
=
·
+
·
·
=
∂v2 ∂v1 ∂v2 ∂ φ̇ ∂v1 ∂v2




cos (φ)
ηx (φ)



 β
∂G 
1
1



=  sin (φ)  ·
+
·
·


η
(φ)
∂v2 
 1 − β HL  y
 L 1 − β HL
0
1
Navigation System Design
Eduardo Nebot
Slide 27
Kinematic Model of the Utility Car (UTE)
∂G ∂F ∂v1 ∂F
=
+
·
·
∂β
∂v1 ∂β
∂ φ̇


Ã
!
∂ φ̇ ∂v1 ∂ φ̇
+
=
·
∂v1 ∂β ∂β


!
Ã
cos (φ)
ηx (φ)




v2
∂G 
v2
H 
β
H v1


=  sin (φ)  · ¡
·
+
·
·
·
+


η
(φ)
¢
¡
¢
∂β 
L 1 − βH 2 L
L
 1 − βH 2 L  y

L
L
0
1


ηx (φ)


∂G
∂F ∂ φ̇


·
=
=  ηy (φ)  · 0 = 0
∂φz

∂ φ̇ ∂φz 
1
Navigation System Design
Eduardo Nebot
Slide 28
If a Gyro is used to predict the change in orientation then the following gradient is used:
h
∂G
=
∂ (v2, β, φz )

i
∂G
∂v2
∂G
∂β
∂G
∂φz

cos (φ)


∂G 
1

=  sin (φ)  ·
∂v2 
 1 − β HL
0


cos (φ)


v2
∂G 
H

=  sin (φ)  · ¡
·
¢
∂β 
 1 − βH 2 L
L
0




ηx (φ)
ηx (φ)




∂G 



=  ηy (φ)  · 1 =  ηy (φ) 
∂φz 



1
1
Navigation System Design
Eduardo Nebot
Slide 29
Kinematic Model of the Utility Car (UTE)
Finally the Jacobian of the observation model with respect to the state variables can be evaluated
with the following equation Then the output Jacobian matrix, evaluated in (xL, yL, ϕ) is

∂ (r, β)
∂H
=
=
∂ (xL, yL, ϕ) ∂ (xL, yL, ϕ)
(xL −xi )
√
d
− (yLd−yi)
(yL −yi )
√
d
(xL −xi )
d

0

−1
d= (xL − xi)2 + (yL − yi)2
These models will be used with a standard EKF algorithms to navigate, build and maintain a
navigation map of the environment.
Navigation System Design
Eduardo Nebot
Slide 30
Beacon Based Navigation
Navigation System Design
Eduardo Nebot
Slide 31
Beacon Based Navigation
In this section, navigation systems employing artificial beacons (or targets) are presented. The
kinematic vehicle model developed in the previous section will be used.
The generally accepted definition of a ‘beacon’, is a target whose position (and perhaps other
parameters) is known to the navigation system.
By observing a beacon whose absolute position is known, the position of the vehicle with respect to
the beacon may be easily computed. Therefore, beacon based navigation systems are perhaps the
easiest to implement.
The drawback, of course, is that infrastructure is usually needed to implement a navigation system of
this type. A form of navigation that overcomes this limitation will be discussed in the SLAM section.
A Brief introduction of the various estimation filtering techniques used is presented.
Navigation System Design
Eduardo Nebot
Slide 32
Linear Kalman Filter Estimator
The Kalman filter is a linear estimator that fuses information obtained from model, observation and
prior knowledge in an optimal manner.
The system we are interested can be modeled by:
x(k) = F (k)x(k − 1) + B(k)u(k) + G(k)v(k)
(12)
where x(k − 1) is the state of the system at time k − 1, u(k) is an input, v(k) is additive noise, B(k)
and G(k) are input and noise transition matrices and F (k) is the transition matrix that allows to
propagate x(k) to x(k + 1), being k the index to denote one incremental step of time.
The observation model have the following form:
z(k) = H(k)x(k) + w(k)
(13)
where z(k) is the observation at time k, H(k) is the observation matrix that relates the states with
the observations and w(k) is additive noise.
Navigation System Design
Eduardo Nebot
Slide 33
Linear Kalman Filter Estimator
It is also assumed that all noises are Gaussian, uncorrelated and zero mean.
E[v(k)] = E[w(k)] = 0
with covariances:
E[v(i)v T (j)] = δij Q(i) , E[w(i)wT (j)] = δij R(i)
The process and observation noises are also assumed uncorrelated:
E[v(i)wT (j)] = 0
Finally it is assumed that we know with a given uncertainty the initial state:
x(0) = x0 , P (0) = P0
where P0 is in general a covariance diagonal Matrix with an initial guess of the uncertainty on the
initial value x0.
Navigation System Design
Eduardo Nebot
Slide 34
Prediction Stage
With the process and observation models F, B, G, H, covariances Q, R and P0 and initial state x0
the following Kalman Filter equations can be applied to recursively estimate the state i two stages:
Prediction and Update
The Prediction stage is responsible for the high frequency information and uses dead reckoning
sensors inputs to drive the process model and generate a prediction of the state x at time k − 1 given
all the information up to time k.
x̂(k/k − 1) = F (k)x̂(k − 1/k − 1) + B(k)u(k)
(14)
The uncertainty of this prediction is also evaluated:
P (k/k − 1) = F (k)P (k − 1/k − 1)F T (k) + G(k)Q(k)GT (k)
Navigation System Design
Eduardo Nebot
(15)
Slide 35
Update Stage
The filter will work in prediction mode until an observation become available. The state x(k) will
then be updated with the observation obtained at time k.
x̂(k/k) = x̂(k/k − 1) + W T (k)(z(k) − H(k)x̂(k/k − 1))
(16)
x̂(k/k) is obtained with the prediction x̂(k/k − 1) plus a correction factor formed by a product of the
Kalman Gain and the difference between the observation and the predicted observation.
Since an observation was included the uncertainty of the state, P (k/k), must decrease:
P (k/k) = P (k/k − 1) − W (k)S(k)W T (k)
Navigation System Design
Eduardo Nebot
(17)
Slide 36
Update Stage
The Kalman gain matrix W (k) and innovation covariance S are given by
W (k) = P (k/k − 1)H T (k)S(k)−1
(18)
S(k) = H(k)P (k/k − 1)H T (k/k − 1) + R(k)
(19)
.
S equation represents the covariance of the innovation sequence:
ν(k) = z(k) − H(k)x(k/k − 1)
Since in most cases the true value of the state is never known the innovation sequence gives an
indication of how well the filter is working. This measure can be obtained by comparing the actual
value of the innovation sequence with the expected variance predicted by S(k). This approach will be
used to validate the data before incorporating the observation and for data association purposes.
Navigation System Design
Eduardo Nebot
Slide 37
The Extended Kalman Filter
In most real cases the process and/or observation models do not behave linearly and hence the linear
Kalman filter can not be used.
The Extended Kalman filter (EKF) was developed for non-linear systems and provides the best
MMSE of the estimate. In principle it is a linear estimator which linearises the process and
observation models about the current estimated state.
The non-linear discrete time system is described as
x(k) = F(x(k − 1), u(k), k) + w(k),
(20)
where F(·, ·, k) is a non-linear state transition function at time k which forms the current state from
the previous state and the current control input.
The non-linear observation model is represented as
z(k) = H(x(k)) + v(k).
Navigation System Design
Eduardo Nebot
(21)
Slide 38
Prediction Stage
The state is predicted with the non-linear process model
x̂(k|k − 1) = F(x̂(k − 1|k − 1), u(k)),
(22)
and the predicted covariance matrix is
P(k|k − 1) = ∇Fx(k)P(k − 1|k − 1)∇FTx (k) + Q(k).
(23)
The term ∇Fx(k) is the Jacobian of the current non-linear state transition matrix F(k) with respect
to the predicted state x(k|k − 1).
Navigation System Design
Eduardo Nebot
Slide 39
Update Stage
When an observation occurs, the state vector is updated according to
x̂(k|k) = x̂(k|k − 1) + W(k)ν(k)
(24)
where ν(k) is the nonlinear innovation,
ν(k) = z(k) − H(x̂(k|k − 1)).
(25)
The gain and innovation covariance matrices are
W(k) = P(k|k − 1)∇HTx (k)S−1(k),
(26)
S(k) = ∇Hx(k)P(k|k − 1)∇HTx (k) + R(k),
(27)
where the term ∇Hx(k) is the Jacobian of the current observation model with respect to the
estimated state x(k|k).
The updated covariance matrix is
P(k|k) = (I − W(k)∇Hx(k))P(k|k − 1)(I − W(k)∇Hx(k))T +
W(k)R(k)W(k).
Navigation System Design
Eduardo Nebot
(28)
Slide 40
The Information Filter
The information filter is mathematically equivalent to the Kalman filter. The key components in the
information filter are the information matrix Y(k|k), and the information vector ŷ(k|k).
Assuming that the noise is Gaussian then Y(k|k) is the inverse of the covariance matrix,
Y(k|k) = P−1(k|k),
(29)
and hence represents the amount of information in the states of interest and their correlations. The
information vector relates to the states through:
ŷ(k|k) = Y(k|k)x̂(k|k).
(30)
The predicted information vector is
ŷ(k|k − 1) = L(k|k − 1)ŷ(k − 1|k − 1) + B(k)u(k),
where L(k|k − 1) = Y(k − 1|k − 1)F(k)Y−1(k − 1|k − 1).
The transformation matrix B(k) transforms the control input u(k) to information space.
Navigation System Design
Eduardo Nebot
Slide 41
(31)
The Information Filter
The corresponding predicted information matrix is
Y(k|k − 1) = [F(k)Y−1(k − 1|k − 1)FT (k) + Q(k)]−1.
(32)
The information contribution to the states due to an observation is:
i(k) = HT (k)R−1(k)z(k).
(33)
The amount of certainty associated with this observation is:
I(k) = HT (k)R−1(k)H(k)
(34)
The benefit of the information filter lies in the update stage. Since the observation has been
transformed over to information space, the update procedure is simply the information contribution
of this observation to the prediction,
Navigation System Design
ŷ(k|k) = ŷ(k|k − 1) + i(k)
(35)
Y(k|k) = Y(k|k − 1) + I(k).
(36)
Eduardo Nebot
Slide 42
The Information Filter
If there is more than one sensor providing observations:
ŷ(k|k) = ŷ(k|k − 1) +
X
ij (k)
X
Y(k|k) = Y(k|k − 1) +
Ij (k).
(37)
(38)
Advantages associated with the Information filter:
• No innovation covariance matrix S(k) has to be computed;
• Furthermore, there is no gain matrix W(k) which needs to be computed;
• The initial information matrix can be easily initialised to zero information;
• It is computationally easier to implement an information filter in a multisensor environment since
it is simply the summation of the individual information contributions.
Disadvantages associated with the Information filter:
• Not appropriate when working with large number of states and few sensors.
• State has to be reconstructed if needed
Navigation System Design
Eduardo Nebot
Slide 43
The Extended Information Filter
The extended information filter (EIF) estimates the states of interest given non-linear process and/or
observation models in information space.
The predicted information vector and matrix is
ŷ(k|k − 1) = Y(k − 1|k − 1)F(x̂(k − 1|k − 1), u(k)),
(39)
Y(k|k − 1) = [∇Fx(k)Y−1(k − 1|k − 1)∇FTx (k) + Q(k)]−1.
(40)
When an observation occurs, the information contribution and its associated information matrix is
i(k) = ∇HTx (k)R−1(k)[ν(k) + ∇Hx(k)x̂(k|k − 1)],
(41)
I(k) = ∇HTx (k)R−1(k)∇Hx(k),
(42)
where the innovation is
ν(k) = z(k) − H(x̂(k|k − 1)).
(43)
Although the non-trivial derivation of the Jacobians still occurs in this form, the EIF has the benefit
of being easily initialised, that is, one can set the initial conditions to zero information. Furthermore,
there is the added benefit of decentralising the filter across multiple nodes and the incorporation of
multiple observations to a single filter
Navigation System Design
Eduardo Nebot
Slide 44
Implementation Example of Beacon Based Navigation
Navigation System Design
Eduardo Nebot
Slide 45
Sensors Used for Beacon Based Navigation
The sensors used in beacon based navigation systems typically fall into two distinct categories:
• bearing only sensors: return the bearing and/or azimuth to a given beacon. Some laser
systems employ this principle. Single cameras also fall into this category.
Figure 7: Bearing Laser, range and bearing radar and laser
• range and bearing sensors: return Range and bearing. Most common type include sonar,
laser, radar, and stereo vision.
In general the beacons in the environment will be clearly differentiated from the background but will
not report its identity. The system will have a list with the location of all the beacons will determine
which is the feature the sensor has detected. This process is called data association and will be
obviously more difficult and less reliable with bearing only sensors.
Navigation System Design
Eduardo Nebot
Slide 46
Land Vehicle Application: Basic Modeling
Navigation System Design
Eduardo Nebot
Slide 47
State Transition (Prediction) Equations
The discretised vehicle model (state transition equation) used to predict the next state of the system
is ;
x(k) = x(k − 1) + ∆T V (k − 1) cos(φ(k − 1) + γ(k − 1))
y(k) = y(k − 1) + ∆T V (k − 1) sin(φ(k − 1) + γ(k − 1))
V (k − 1) sin(γ(k − 1))
φ(k) = φ(k − 1) + ∆T
(44)
B
Assume that the kinematic assumptions (rigid body, rolling motion) hold true. When this is the case,
the only source of process noise is injected via the control inputs; steer angle, and wheel velocity such
that these parameter may be modelled as:
ω(t) = ω(t) + δω(t)
γ(t) = γ(t) + δγ(t)
where δω(t) and δγ(t) are Gaussian, uncorrelated, zero mean random noise processes with variances
σω2 and σγ2 respectively.
Navigation System Design
Eduardo Nebot
Slide 48
State Transition (Prediction) Equations
The discrete time state vector at time k + 1 can now be defined as
x(k + 1) = f (x(k), u(k), k) + v(k)
(45)
= [x(k + 1), y(k + 1), φ(k + 1)]T
where
u(k) = [ω(k), γ(k)]T
is the control vector at time k, v(k) is additive process noise representing the uncertainty in slip
angles and wheel radius, and f (·) is the nonlinear state transition function mapping the previous
state and current control inputs to the current state.
Navigation System Design
Eduardo Nebot
Slide 49
State Transition (Prediction) Equations
Assume the system has an estimate available at time k which is equal to the conditional mean
x̂(k|k) = E[x(k)|Zk ]
(46)
The prediction x̂(k + 1|k) for the state at time k + 1 based on information up to time k is given by
expanding Equation 45 as a Taylor series about the estimate x̂(k|k), eliminating second and higher
order terms, and taking expectations conditioned on the first k observations, giving
x̂(k + 1|k) = E[x(k + 1)|Zk ] = f (x(k|k), u(k), k)
(47)
The vector state prediction function f (·) is defined assuming zero process and control noise.
The prediction of state is therefore obtained by simply substituting the previous state and current
control inputs into the state transition equation with no noise.
Navigation System Design
Eduardo Nebot
Slide 50
Covariance Transition (Prediction) Equations
The noise source vector can be written as follow
δw(k) = [δω(k), δγ(k)]T
The error between the true state and the estimated state is given by
δx(k|k) = x(k) − x̂(k|k)
(48)
the prediction of covariance is then obtained as
P(k + 1|k) = E[δx(k|k)δx(k|k)T |zk ]
(49)
= ∇fx(k)P(k|k)∇fxT (k) + ∇fw (k)Σ(k)∇fwT (k)
Navigation System Design
Eduardo Nebot
(50)
Slide 51
Covariance Transition (Prediction) Equations
∇fx(k) represents the gradient or Jacobian of f (·) evaluated at time k with respect to the states,
∇fw (k) is the Jacobian of f (·) with respect to the noise sources, and Σ(k) is the noise strength
matrix given by


Σ(k) = 
σω2 0
0

(51)
σγ2


1 0 ∆T V sin(φ(k|k) + γ(k))




∇fx(k) =  0 1 ∆T V cos(φ(k|k) + γ(k)) 


0 0 1
and

(52)

∆T cos(φ(k|k) + γ(k)) −∆T V sin(φ(k|k) + γ(k))




∇fw (k) =  ∆T sin(φ(k|k) + γ(k)) ∆T V cos(φ(k|k) + γ(k)) 


∆T sin(γ(k))
B
(53)
∆T V cos(γ(k))
B
For the UTE model the reader needs to select the type of information used to determine the change of
bearing, steering encoder or Gyro, and then select the appropriate equation to evaluate the Jacobian.
Navigation System Design
Eduardo Nebot
Slide 52
Bearing Only Navigation
Navigation System Design
Eduardo Nebot
Slide 53
Sensor Prediction Equations
Assume that the vehicle has a laser sensor attached such that the bearing to a number of fixed
beacons Bi = [Xi, Yi], i = 1, . . . , n can be obtained.
This laser provides a nonlinear observation which may be considered to be of the form
z(t) = h(x(t)) + v(t)
(54)
The bearing to a beacon is given by arctan( XYii−y(t)
−x(t) ). However the platform is oriented in the direction
φ, so for this system, the measurement equation for each beacon detected by the laser is given by the
model
·
µ
¶
¸
£ i ¤
Y
−
y(t)
i
i
zθ (t) = arctan
− φ(t) + vθ (t)
Xi − x(t)
(55)
where the laser observation noise vθi , is uncorrelated zero mean and Gaussian with strength σθ2, and
is assumed to be identical for each beacon observed.
Navigation System Design
Eduardo Nebot
Slide 54
Sensor Prediction Equations
The predicted observation is found by taking expectations conditioned on all previous observations,
truncating at first order to give
ẑ(k + 1|k) = E[z(k + 1)|Zk ]
(56)
= h(x̂(k + 1|k))
If there is a predicted state for the vehicle, x̂(k + 1|k), we can therefore predict the observations that
will be made at that state. From Equation 55 and from Equation 56, we have the predicted
observations as
¶
¸
·
µ
£ i
¤
Yi − ŷ(k + 1|k)
− φ̂(k + 1|k)
z(k + 1|k) = ẑθ (k + 1|k) = arctan
Xi − x̂(k + 1|k)
Navigation System Design
Eduardo Nebot
Slide 55
(57)
Sensor Prediction Equations
Now, the innovation or observation prediction error covariance S(k), used in the calculation of the
Kalman gains must be computed. This is found by squaring the estimated observation error and
taking expectations of the first k measurements to give the following,
S(k + 1) = ∇hx(k + 1)P(k + 1|k)∇hTx (k + 1) + R(k + 1)
(58)
In this case, the Jacobian ∇hx(k + 1) is given by
h
i
ŷ(k+1|k)−Yi x̂(k+1|k)−Xi
∇hx(k + 1) = −
(59)
−1
d2
d2
p
where d = (Xi − x̂(k + 1|k))2 + (Yi − ŷ(k + 1|k))2 is the predicted distance between a beacon and
the vehicle.
The observation variance term R(k) is given by
£ 2¤
R(k) = σθ
Navigation System Design
Eduardo Nebot
(60)
Slide 56
Sensor Prediction Equations
The state update equations for the EKF are given by
x̂(k + 1|k + 1) = x̂(k + 1|k) + K(k + 1)[z(k + 1) − h(x̂(k + 1|k))]
(61)
where K(k), S(k) and h(k) are:
K(k + 1) = P(k + 1|k)∇hTx (k + 1)S(k + 1)−1
(62)
S(k + 1) = ∇hx(k + 1)P(k + 1|k)∇hTx (k + 1) + R(k + 1)
(63)
h
∇hx(k + 1) =
Navigation System Design
i
− ŷ(k+1|k)−Y
d2
Eduardo Nebot
x̂(k+1|k)−Xi
d2
i
(64)
−1
Slide 57
Range and Bearing Navigation
Navigation System Design
Eduardo Nebot
Slide 58
Range-Bearing Navigation
The implementation aspect of a range–bearing beacon based navigation system is very similar to the
bearing only system presented before.
The only major difference is in the derivation of the sensor model. The sensor still observes a number
of fixed beacons, however it now also returns the range to these beacons.
The prediction equations for such a sensor can be derived as


ẑir (k + 1|k)
p

(Xi − x̂(k + 1|k)2 + (Yi − ŷ(k + 1|k))2
=

³
´
z(k + 1|k) = 
Y
−ŷ(k+1|k)
arctan Xii−x̂(k+1|k) − φ̂(k + 1|k)
ẑiθ (k + 1|k)
where zir is the range measurement returned by the sensor when observing the i’th beacon.
Navigation System Design
Eduardo Nebot
Slide 59
(65)
Range-Bearing Navigation
The Jacobian ∇hx(k + 1) is given by

∇hx(k + 1) = 
where d =
p
x̂(k+1|k)−Xi
d
i
− ŷ(k+1|k)−Y
2
d
ŷ(k+1|k)−Yi
d
x̂(k+1|k)−Xi
d2

0

(66)
−1
(Xi − x̂(k + 1|k))2 + (Yi − ŷ(k + 1|k))2 is (as before), the predicted distance between
the i’th beacon and the vehicle.
The observation variance term R(k) is given by

R(k) = 

σr2
0
0
σθ2

(67)
where σr2 is the variance associated with the range measurement. As before it is assumed that the
sensor is corrupted by Gaussian, zero-mean, uncorrelated measurement noise.
The state and covariance prediction, and update steps are identical to those in bearing only
algorithm. The advantages of including range will be discussed in the discussion session.
Navigation System Design
Eduardo Nebot
Slide 60
Data Association
Navigation System Design
Eduardo Nebot
Slide 61
Data Association
This is a fundamental problem in beacon navigation: We have a list of beacons with position
information but when a beacon is sensed we have the following question:
What is the identity of this beacon ?
Solution:
• Use beacons that are not identical. A common method for achieving this in bearing only
navigation is to use bar codes for uniquely identifying targets (Pasive barcodes ). In some cases the
beacon broadcast their identity (Active beacons).
• Use statistics to determine how likely a given beacon is the one that was sensed.
The fist method makes the association very simple but it can become limited with respect to the
number of beacons possible or expensive to be used in large areas.
The second method can use an unlimited number identical pasive beacons (reflective tape) but will
make the data association more complex.
This section will discuss basic data association algorithm based on statistical methods.
Navigation System Design
Eduardo Nebot
Slide 62
Data Association: The basic idea
Possible area
for a beacon
BEARING
ONLY
Vehicle
Navigation System Design
Possible area
for a beacon
RANGE
BEARING
Vehicle
Eduardo Nebot
Slide 63
Data Association
Methods to perform data association:
• Gate Validation: Associate only if there is one possible hypothesis
• Nearest Neighbour: Select the nearest state to the observation
• Multi Hypothesis Tracking: Use all the possible hypothesis
• Batch / delay data association
Navigation System Design
Eduardo Nebot
Slide 64
Data Association
A few methods are based on testing the innovation sequence. The innovation (ν) for a navigation
filter is defined as the difference between the observed and predicted observations as
ν(k) = z(k) − H(k)x̂(k/k − 1)
(68)
The innovation mean can be found as
£
¤
£
¤
k−1
k−1
E ν(k)|Z
= E z(k) − ẑ(k|k − 1)|Z
¤
£
= E z(k)|Zk−1 − ẑ(k|k − 1)
= 0
(69)
with variance
£
¤
T
S(k) = E ν(k)ν (k)
£
¤
= E (z(k) − H(k)x̂(k|k − 1))(z(k) − H(k)x̂(k|k − 1))T
£
¤
= E (H(k) [x(k) − x̂(k|k − 1)] + v(k))(H(k) [x(k) − x̂(k|k − 1)] + v(k))T
= R(k) + H(k)P(k|k − 1)HT (k)
(70)
The innovation sequence is white with zero mean and variance S(k).
Navigation System Design
Eduardo Nebot
Slide 65
Data Association
The properties of the innovation sequence can be exploited to determine how likely a beacon is to
being the current observation.
The normalised innovations squared q(k) are defined as
q(k) = ν T (k)S−1(k)ν(k)
(71)
which under the assumption that the filter is consistent can be shown to be a χ2 random
distribution with m degrees of freedom, where m = dim(z(k)), the dimension of the measurement
sequence.
For gating observations, q is simply formed (usually for each beacon Bi), a threshold is chosen from
χ2 tables (given a confidence value, 95% or 99% for example), and then q compared to the threshold.
If the value of q for a given beacon is less than the threshold, then that beacon is likely to have been
the one observed.
Navigation System Design
Eduardo Nebot
Slide 66
Data Association: Beacon Rejection
Depending on the geometry of the beacons, it is sometimes possible for multiple beacons to (quite
validly) match an observation. ( More often with bearing only sensors ! )
In these cases, it is safer to ignore the observation than trying to guess which beacon was observed.
This is very important since the incorporation of a wrong beacon will most likely generate a
catastrophic fault in the filter.
That is the filter will not be able to match any more beacons univocally since the vehicle will be in
another position and the state covariance error will grow indefinitely.
Possible area
for a beacon
BEARING
ONLY
Vehicle
Navigation System Design
Possible area
for a beacon
RANGE
BEARING
Vehicle
Eduardo Nebot
Slide 67
Filter Implementation
Any real localization application will have dead reckoning sensors that will provide high frequency
information at fixed intervals and external sensors that will provide low frequency information not
necessarily at a prefix interval.
An example of the dead reckoning sensors are velocity and steering encoders that are sampled at
given intervals. External sensors usually monitor the environment and eventually report information
when a possible beacon is detected.
In general we have various prediction cycles for each external information.
The external information will not in most cases happen at the predictions time. Furthermore, the
external information will be present in some cases with a significant delay due to the processing
necessary to extract the possible features or the validation process.
Navigation System Design
Eduardo Nebot
Slide 68
Filter Implementation
One example of asynchronous processing is shown in the Figure. In this case the system perform
predictions 1, 2 and 3. At time 3 an observation becomes available with time stamp somewhere
between 2 and 3.
The right procedure to incorporate this information is to save the previous states at 2 and make a
new prediction up to the observation time. At this time perform an update and then a prediction to
time 3 again.
Update
Pred 4
Pred5
Observation
Prediction 1
Prediction 2
Prediction 3
Figure 8: Asynchronous Prediction Update Cycle
This process will require the filter to save few previous states in accordance with the maximum delay
expected in the external information.
Navigation System Design
Eduardo Nebot
Slide 69
Filter Implementation
Observation Available
Prediction/Update/Prediction cycle
No Observation Available
Prediction cycle
Prediction
Prediction
observation
prediction
prediction
Estimation
Time
k-1
(k_obs)
observation time stamp
k
k+1
sampling time
Figure 9: Asynchronous Prediction Update Cycle
Navigation System Design
Eduardo Nebot
Slide 70
The Global Positioning System (GPS)
Navigation System Design
Eduardo Nebot
Slide 71
The Global Positioning System (GPS)
The Global Positioning System is an all-weather and continuous signal system designed to provide
information to evaluate accurate position worldwide.
GPS receivers observe time delays and process messages from several in line of sight satellites to
determine accurate receiver antenna position.
The GPS system achieved initial operation capabilities on 8 December 1993.
It consist of 3 major segments:
• Space Segment
• Control Segment
• User Segment
Navigation System Design
Eduardo Nebot
Slide 72
Space Segment
The GPS operational constellation consists of 24 satellites that orbit the earth with a period of 12
hours. There are 6 different orbital planes with 55 degrees inclination. Each plane has a radius of
20200 km and the path is followed by 4 satellites.
This distribution assures that at least four satellites are always in view anywhere around the world an
eight satellites 80% of the time.
Navigation System Design
Eduardo Nebot
Slide 73
Control Segment
This segment consist of a system of tracking stations distributed around the world.
The main objective is to determine the position of the satellites to update their ephemeris. Satellite
clock correction are also updated.
GPS Control
Segment
Ground Antenna ( 4 )
Navigation System Design
Master Station
( 1 + Backup )
Eduardo Nebot
Remote Monitoring
Station ( 5 )
Slide 74
User Segment
The user segment consists of the GPS receivers that use the GPS signal information. The GPS
receivers are passive, so the system can provide service to an unlimited number of users.
The only requirement is that the satellite need to be in the line of sight of the antenna: The GPS will
not work underground or inside buildings.
Navigation System Design
Eduardo Nebot
Slide 75
GPS Operation Overview
The satellites transmit information at frequencies L1=1575.42 Mhz and L2=1227.6 Mhz as shown in
the Figure.
The C/A code is a repeating 1 Mhz pseudo random binary sequence. There is a separate C/A code
for each satellite in operation. The code is used by the receiver to identify the satellite and to obtain
range information. Each GPS receiver has a correlator and the PRBS for all 31 possible satellites.
Navigation System Design
Eduardo Nebot
Slide 76
GPS Operation Overview
The GPS signal is modulated with a pseudo random C/A and P codes and with a 50 BPS navigation
message as shown.
Fundamental
Frequency
10.23 MHZ
x 154
x 120
X / 10
L1
1575.43 MHZ
C/A Code
1023 MHz
L2
1227.60 MHZ
50 BPS
P-Code
10.23 MHZ
P-Code
10.23 MHZ
Navigation Message
Each time it receives a signal from a satellite the reeceiver starts to correlate the received PRBS with
each of the sequences stored on board. This task is achieved by shifting receiver own sequence from
an estimated time t0 until a peak in the correlation is achieved. This peak identifies the satellite
number and the shift with respect to time indicates the distance to the satellite.
Navigation System Design
Eduardo Nebot
Slide 77
GPS Operation Overview
The navigation message is present in both the L1 and L2 frequencies. It contains information about
the precise satellite ephemeris, clock correction parameters, and low precision ephemeris data for the
other satellites.
After 30 seconds of continuous satellite tracking the GPS receiver is able to use the tracked
satellite for position determination. After approximately 12.5 minutes of uninterrupted tracking of
a given satellite the low precision ephimeris for the whole satellite constellation is downloaded. This
is called almanaque and is used by the receiver during startup to reduce the time to fix position.
The Precision Positioning System (PPS) is provided by the P code included in the L2 signal. This
code is only available to authorised users and has a period of 7 days.
The Standard Positioning System (SPS) is based on the C/A code that is included in both
frequencies. The accuracy of SPS is 100 m (2drms), 156 m vertical (95%) and 340 ns (95 %), with
Selective Availability (SA). 10 meters without SA. The accuracy of the PPS system is 22m
horizontal (2drms) 27.7 m vertical (95%), and 200 ns (95%) time.
Navigation System Design
Eduardo Nebot
Slide 78
GPS observables
Two types of observations are of main interest:
• Pseudo-range: distance from the satellite to the vehicle plus additional errors due to clock
drifts, the ionosphere, the troposphere, multi-path and Selective Availability, (SA)
• Doppler frequency information: As the vehicle and the satellites are in constant motion with
respect to each other the receiver signal will experience a change in frequency proportional to their
relative velocities. It can be used to generate very accurate velocity estimation
Not all GPS receiver generate velocity from Doppler observation. This make velocity information
independent of position. (Important for data fusion)
The precision of the solution is affected by two main factors:
• PDOP (position dilution of precision)
• Precision in range determination
Navigation System Design
Eduardo Nebot
Slide 79
Position Determination
The basic principle behind GPS position is shown in the Figure
Figure 10: Basic GPS principle
Navigation System Design
Eduardo Nebot
Slide 80
Position Determination
Possible position of a vehicle when receiving the signal from 1, 2 and 3 GPS respectively. The
position is univocally determined when ranges to at least 3 satellites are available.
The system uses 4 satellites to solve for the other unknown that is time to synchronize receiver and
satellite clocks. This enable the receivers to be built with inexpensive clocks while the satellites have
very accurate atomic clocks.
Navigation System Design
Eduardo Nebot
Slide 81
Position Determination
With ephimeris and ranges from 4 satellites the set of non-linear equations presented in Equation 72
can be solved. This equation is function of the known ranges and satellite position and unknown
position of the GPS receiver and clock drift.
r1 =
r2 =
r3 =
r4 =
p
p
p
p
(x − x1)2 + (y − y1)2 + (z − z1)2 + c∆t
(x − x2)2 + (y − y2)2 + (z − z2)2 + c∆t
)2
)2
(72)
)2
(x − x3 + (y − y3 + (z − z3 + c∆t
(x − x4)2 + (y − y4)2 + (z − z4)2 + c∆t
GPS receivers usually linearize these equations and evaluate the position by solving a linear set of
equations or by implementing a least square solution for the case where redundant information is
available
Navigation System Design
Eduardo Nebot
Slide 82
Linearisation to obtain position







r (p)
r (p )
x − x0
ε
 1
  1 0 

  x

 


 
 r2(p)   r2(p0) 
 y − y0   εy

=
 + A
+

 


 
 r3(p)   r3(p0) 
 z − z0   εz

 


 
r4(p)
r4(p0)
c∆t
εclock


x−x1 y−y1 z−z1
1
 δSi δSi δSi

 x−x1 y−y1 z−z1 
 δ

δSi
δSi 1 
Si
A=
 x−x1 y−y1 z−z1 
 δ
1
 Si δSi δSi

x−x1 y−y1 z−z1
1
δ
δ
δ
Si
and δSi
δSi =
p
Si








(73)
(74)
Si
(x − x1)2 + (y − y1)2 + (z − z1)2
(75)
Where p = [x, y, z, c∆t]T and ε are the errors due to range noise and missing higher order terms in
the linearization.
Navigation System Design
Eduardo Nebot
Slide 83
Position Determination
With this approximation the change in position can then be evaluated
∆p = A−1∆r
(76)
When ranges from more than four satellites are available a least square solution can be implemented:
∆p = (AT A)−1AT ∆r
(77)
Finally the previous position is updated with the correction to obtain the position of the rover at he
time stamp of the pseudo-range information:
p = p0 + ∆p
(78)
The psedudorange is obtained by the GPS hardware through a correlation process and the position of
the satellites needs to be computed using the precise satellite ephimeries broadcast as part of the
navigation message by each satellite.
Navigation System Design
Eduardo Nebot
Slide 84
Most Common GPS errors
Satellite Clock Errors: The GPS system ultimately depends on the accuracy of the satellite
clock. The ground stations are responsible for estimating the clock errors.
These stations do not correct the clock but upload the parameters for the correction formula that the
satellite broadcast as part of the navigation message.
Each GPS receiver needs to compensate the pseudorange information with a polynomial equation
function of these parameters.
Ephemeris Errors: These errors result from the satellite transmitting the ephimeris parameter
with errors. This will translate in errors in the prediction of the position of the satellite that will
generate an incorrect position fix of the GPS receiver.
These errors grow with time since the last update performed by the ground stations. The GPS
receiver usually do not use satellites with ephemeries older than 2 hours.
Navigation System Design
Eduardo Nebot
Slide 85
Most Common GPS errors
Ionosphere Errors Since there are free electron in the Ionosphere, the GPS signal does not travel
at the speed of light while in transit in this region.
The user can compensate for these errors using a diurnal model for these delays. The parameters of
this model are part of the GPS navigation message.
The error obtained after this compensation is in the order of 2-5 m. Another compensation method
uses the signals at both frequencies, L1 and L2 to solve for the delay. This method can reduce the
errors to 1-2 m.
Troposphere Errors Another delay is introduced by the variation of the speed of the signal due to
variation in temperature, pressure and humidity.
Model correction can reduce this error to the order of 1 meter.
Navigation System Design
Eduardo Nebot
Slide 86
Most Common GPS errors
Receiver Errors This is the error introduced by the GPS receiver when evaluating the range
though the correlation process. It is mostly dependent on non-linear effects and thermal noise.
The magnitude of this error is in the order of 0.2-0.5 m.
Multipath errors The range information is evaluated measuring the travel time of the signal from
the satellite to the rover.
Sometimes this path may be obstructed but the signal will still reach the rover antenna through an
indirect path by multiple reflections. The receiver will obtain erroneous range and phase carrier
difference information and an incorrect position fix will be generated.
The multi-path problem can be reduced with an appropriate selection of the antenna, GPS receiver,
and accepting observations only from satellites with a minimum elevation angle.
Navigation System Design
Eduardo Nebot
Slide 87
Most Common GPS errors
Selected Availability Selective Availability (SA) is a deliberate error introduced by the US
Department of Defense and consists of additional noise included in the transmitted satellite clock and
satellite ephemeris. SA has been disconnected since May 2000 and it is not the intent of the US to
ever use SA again on the civil GPS signal.
The Figure shows the longitude and latitude error in meters obtained with a standard GPS with SA.
This is a very low frequency error !!!
longitude error
100
meters
50
0
-50
-100
0
1000
2000
3000
4000
5000
6000
7000
8000
5000
6000
Time in sec.
7000
8000
latitude error
100
meters
0
-100
-200
0
1000
2000
3000
4000
Figure 11: Latitude and Longitude errors due to SA
Navigation System Design
Eduardo Nebot
Slide 88
Most Common GPS errors
Geometric Dilution of Precision The quality of the solution of the position and clock bias error
in relation with the error in the pseudorange measurement is a function of the matrix A, Equation 76.
Assuming that the standard deviation for the pseudorange observation is σ, the matrix covariance for
the state p, Equation 78, is given by:
−1
P = (AT A) σ 2 = Hσ 2
(79)
From this equation the various definition of estimation accuracy are defined:
p
V DOP = H33
p
HDOP = (H11) + (H22)
p
P DOP = (H11 + H22 + H33)
p
GDOP = (H11 + H22 + H33 + H44)
Navigation System Design
Eduardo Nebot
(80)
(81)
(82)
(83)
Slide 89
Most Common GPS errors
The estimated error of the individual component of the state vector p can be given as function of the
DOP variables
p
var(z) = σV DOP
(84)
p
var(x) + var(y) = σHDOP
(85)
p
var(x) + var(y) + var(z) = σP DOP
(86)
p
var(x) + var(y) + var(z) + var(c∆t) = σGDOP
(87)
Most GPS receivers evaluate these uncertainties in real time allowing the user to monitor the
accuracy of the solution.
This is important in real time application of stand alone GPS or when integration with INS is used.
In some cases the GPS may be able to report a solution but due to the geometrical configuration of
the satellites in view the accuracy of the solution can be very poor.
Navigation System Design
Eduardo Nebot
Slide 90
Fundamentals of DGPS (Differential GPS)
The errors in range determination can be significantly reduced with the use of another station placed
at a known surveyed location. The base station can evaluate the range errors and broadcast them to
the the other stations
The base station is usually placed in a location with good sky visibility. It processes information from
satellites with at least 5 degrees over the horizon to avoid multipath problems. This implementation
is called differential GPS (DGPS)
Navigation System Design
Eduardo Nebot
Slide 91
Fundamentals of DGPS (Differential GPS)
DGPS significantly reduces the errors due to delays in the Troposphere and Ionosphere.
It eliminates almost all the errors due to SA ( satellite ephimeris and clock dither )
DGPS was of fundamental importance with SA on. The positioning errors were reduced from
approximate 100 meters to under a few metres.
Without SA the reduction is not that important, specially for low quality DGPS.
The prediction of the magnitude of the errors for different type of GPS (SPS/PPS/DGPS with and
without SA implementation is presented in the following tables
Navigation System Design
Eduardo Nebot
Slide 92
Standard Errors ( metres ) - L1 C/a. Without SA
Error Source
Bias Random Total DGPS
Ephemeris
2.1
0
2.1
0
Satellite Clock
1
0.7
2.1
0
Ionosphere
4
0.5
4
0.4
Troposphere
0.5
0.5
0.7
0.2
Multipath
1
1
1.4
1.4
Receiver measurement
0.5
0.2
0.5
0.5
User Equiv Range Error
5.1
1.4
5.3
1.6
Vertical 1 σ error VDOP
12.8
3.9
Horizontal 1 σ error HDOP 2.0
10.2
3.1
Navigation System Design
Eduardo Nebot
Slide 93
Standard Errors ( metres ) - L1 C/a. With SA
Error Source
Bias Random Total DGPS
Ephemeris
2.1
0
2.1
0
Satellite Clock
20
0.7
20
0
Ionosphere
4
0.5
4
0.4
Troposphere
0.5
0.5
0.7
0.2
Multipath
1
1
1.4
1.4
Receiver measurement
0.5
0.2
0.5
0.5
User Equiv Range Error
20.5
1.4
20.6
1.6
Vertical 1 σ error VDOP
51.4
3.9
Horizontal 1 σ error HDOP 2.0
41.1
3.1
Navigation System Design
Eduardo Nebot
Slide 94
Standard Errors ( metres ) - PPS Dual Frequency P/Y Code
Error Source
Bias Random Total DGPS
Ephemeris
2.1
0
2.1
0
Satellite Clock
2.0
0.7
2.1
0
Ionosphere
1.0
0.5
1.2
0.1
Troposphere
1.0
1.0
1.4
1.4
Multipath
1
1
1.4
1.4
Receiver measurement
0.5
0.2
0.5
0.5
User Equiv Range Error
3.3
1.5
3.6
1.5
Vertical 1 σ error VDOP
8.6
3.7
Horizontal 1 σ error HDOP 2.0
6.6
3.0
Navigation System Design
Eduardo Nebot
Slide 95
Comparison of the errors between GPS and DGPS without SA
Experimental errors obtained from a 24 hour data set logged with a standard stand-alone GPS
receiver. It can be seen that the magnitude of the errors are smaller than the predicted values.
Confidence Hor. GPS Hor DGPS Vert GPS Vert DGPS
50%
2.5
1.6
5.6
1.8
68.27%
3.8
2.2
7.4
2.7
95.45%
7
4.2
14.4
6.4
99.7%
9.8
6.7
21.3
12.0
Navigation System Design
Eduardo Nebot
Slide 96
Phase carrier GPS
The more complex GPS receivers make use of phase carrier information to improve the accuracy of
the position fix.
Differential carrier phase tracking consists of measuring the phase shift between the
same signal received at the base and the rover stations. This phase shift is proportional to
the distance between these two stations.
Much more complicated hardware and software algorithms are needed for this
implementation since these measurements are subject to phase ambiguities, that is uncertainty on
the number of whole carrier waves between the receiver and the satellite.
Algorithm convergence is a vital factor in achieving the accuracy of the carrier phase measurements.
The resolution of the integer ambiguity plays a very important role in the solution of the baseline
vector.
Navigation System Design
Eduardo Nebot
Slide 97
Phase carrier GPS
When the receiver unit is switched on and commences logging the initial whole cycle difference
(ambiguity), between the satellite and the receiver is unknown.
Once the state of the ambiguity is held fixed, the receiver is said to have ”converged” and the
ambiguity is resolved. Once the ambiguities are said to have converged, each corresponding satellite
signal ambiguity, N, is held constant and the change in phase, ∆ψ, is used to calculate the change in
the receiver’s position
Navigation System Design
Eduardo Nebot
Slide 98
Convergence Time for Phase carrier GPS
In any navigation application it is expected that in many occasions the number of satellites in view
will not be enough to obtain a carrier phase solution.
The system will then be restarted and the ambiguities will need to be resolved again. During the
convergence period the accuracy is considerably degraded.
The convergence time depends on a number of different factors, including the baseline,
number of visible satellites, satellite configuration, the method of ambiguity
resolution: i.e. single frequency, dual frequency or combined dual frequency and code
pseudorange data.
Navigation System Design
Eduardo Nebot
Slide 99
Method for Ambiguity Resolution
This section presents experimental data obtained with a GPS receiver manufactured by Novotel
working with two different algorithms
• RT20: 20 cm accuracy using L1 Frequency
• RT2: 2 cm accuracy both the L1 and the L2 frequencies.
The Figure shows the comparison for steady state (RT20 and RT2) and different baselines for RT2
only.
Standard Deviation
Standard Deviation
1.8
1.2
1.6
1.4
1.2
0.8
Std Dev (m)
Dev (m)
1
Std
0.6
RT20
0.4
1
0.8
Baseline Approx. 1 km
0.6
Baseline Approx. 100 m
0.4
0.2
RT2
0.2
0
0
50
100
150
200
250
300
50
Time (sec)
Navigation System Design
100
150
200
250
300
350
Time (sec)
Eduardo Nebot
Slide 100
Other factors that influence the convergence
Visible Satellites Not only does the number of visible satellites influence the accuracy of the
generated position, but also, in the case of phase measurements, influences the resolution of the signal
ambiguities. ( make the solution of the ambiguities faster)
Baseline Length The length of the baseline between the remote and the reference station has
arguably the second largest influence on the time taken for a receiver to reach a converged state,
since, essentially the key to convergence is solving the starting vector.
For a receiver to reach convergence, the calculations performed by the relative carrier phase GPS
system must generate the position of the remote station with respect to the reference station, i.e.
baseline vector. Therefore the length of the baseline plays an important factor in the convergence
time.
Navigation System Design
Eduardo Nebot
Slide 101
GLONASS
The GLONASS is the Russian equivalent GPS system, in this case named Global Navigation Satellite
System. It also transmits navigation and range data on two frequencies L1 and L2.
Unlike GPS where the satellites are distinguished by the spread-spectrum code, the satellites in
GLONASS are distinguished by the frequency of the signal, L1 between 1597-1617 MHz and L2
between 1240-1260 MHz. The system is also envisioned to handle 24 satellites in almost the same
configuration as the GPS equivalent
If both systems are combined then a theoretical doubling of the number of satellites in view is
achievable. This increase in the number of satellites increases both the navigation performance and
fault detection capabilities.
There are already commercial GPS using both systems ( GLONASS + GPS )
Navigation System Design
Eduardo Nebot
Slide 102
Coordinate Transformation
GPS gives the solution in ECEF coordinates. This section presents coordinate transformation
equation that converts position and velocity given in the standard ECEF frame to the local
navigation frame used by the data fusion algorithms presented in this course.
The position Pg = {PX , PY , PZ } and velocity Vg = {VX , VY , VZ } obtained from the GNSS receiver
is delivered in the WGS-84 datum, represented by the ECEF coordinate frame,
Z
N
E
D
φ
Y
λ
X
Figure 12: Coordinate representations. XYZ represents the orthogonal axes of the ECEF coordinate frame used by the GNSS receiver. λ represents the longitude
and φ the latitude of the vehicle. NED represents the local navigation frame at which the vehicle states are evaluated to.
Navigation System Design
Eduardo Nebot
Slide 103
The latitude and longitude is obtained by transforming the position observation received by the
GNSS receiver unit from ECEF to geodetic coordinates. The equations are
Semimajor Earth axis (metres) a = 6378137.0
Undulation f = 0.003352810664747
Semiminor Earth axis (metres) b = a(1 − f )
Coefficients
:
q
p =
Px2 + Py 2
t = tan−1(
Pz × a
)
p×b
e1 = 2f − f 2
a2 − b 2
e2 =
b2
3
−1 PZ + e2 × b sin t
latitude (degrees) φ = tan (
)
p − e1 × a cos3 t
PY
longitude (degrees) γ = tan−1( )
PX
Given the latitude and longitude, a transformation matrix Cng can be formed which transforms the
Navigation System Design
Eduardo Nebot
Slide 104
position and velocity data given by the GNSS receiver from the ECEF frame over to the navigation
frame.

Cng


= 


− sin φ cos λ − sin φ sin λ
− sin λ
cos λ
cos φ
0




(88)
− cos φ cos λ − cos φ sin λ − sin φ
Thus the position and velocity of the vehicle in the local navigation frame is
Pn = CngPg
(89)
Vn = CngVg
(90)
If the vehicle’s work area has negligible change in latitude and longitude, then Cng is effectively fixed.
Equations 89 and 90 form the observations needed for the aided inertial navigation system.
Navigation System Design
Eduardo Nebot
Slide 105
Inertial Sensors
Navigation System Design
Eduardo Nebot
Slide 106
Inertial Sensors
Inertial sensors make measurements of the internal state of the vehicle. A major advantage of
inertial sensors is that they are non-radiating and non-jammable and may be packaged and sealed
from the environment. This makes them potentially robust in harsh environmental conditions.
Historically, Inertial Navigation Systems (INS) have been used in aerospace vehicles, military
applications. However, motivated by requirements for the automotive industry, a whole
variety of low cost inertial systems have now become available in diverse applications such as
heading and attitude determination.
The most common type of inertial sensors are
• Accelerometers: measure acceleration with respect to an inertial reference frame. This includes
gravitational and rotational acceleration as well as linear acceleration.
• Gyroscopes: measure the rate of rotation independent of the coordinate frame.
Navigation System Design
Eduardo Nebot
Slide 107
Inertial Sensors applications
The most common application of inertial sensors are:
• Heading: Integration of the gyro rate information provides the orientation of the platform
• Inclinometers: using accelerometers to determine the attitude by knowing the valued of gravity.
• Vibration Analysis: use of accelerometers to determine vibration.
• IMU: Six degree of freedom Inertial Measurement Unit to determine position,velocity and
attitude of a platform. They can also provide 3-D position information and, unlike encoders, have
the potential of observing wheel slip.
• AHRS: Attitude and heading reference systems.
Navigation System Design
Eduardo Nebot
Slide 108
Fundamental Principles of Accelerometers
Accelerometers measure the inertia force generated when a mass is affected by change in velocity.
This force may change the tension of a string or cause a deflection of beam or may even change the
vibrating frequency of a mass.
The accelerometers are composed of three main elements:
• a mass
• a suspension mechanism that positions the mass
• a sensing element that returns an observation proportional to the acceleration of the mass
• a servo loop that generates an apposite force to improve the linearity of the sensor ( HQ )
Navigation System Design
Eduardo Nebot
Slide 109
A basic one-degree of freedom accelerometer is shown in Figure 13. This accelerometer is usually
referred to as an open loop since the acceleration is indicated by the displacement of the mass.
Sensitive
axis
Spring
k
X
Mass
m
0
c
Damper
Frame
Figure 13: Basic Components of an open loop accelerometer
The accelerometer described in this example can be modelled with a second order equation:
d2 x
dx
F = m 2 + c + kx
(91)
dt
dt
where F is the applied force to be measured. The damping can be adjusted to obtain fast responses
without oscillatory behaviour.
Navigation System Design
Eduardo Nebot
Slide 110
Fundamental Principles of Accelerometers
Many of the actual commercial accelerometers are based on the pendulum principle. They are built
with a proof mass, a spring hinge and a sensing device. These accelerometers are usually constructed
with a feedback loop to constrain the movement of the mass and avoid cross coupling accelerations.
One important specification of the accelerometers is the minimum acceleration that can be measured.
The Figure shows the acceleration measured when travelling with a car at low speed. A accelerometer
for such applications is usually capable of measuring acceleration of less than 500 µg. The dependence
of the bias with temperature and the linearity of the device are also important specifications.
0.6
0.4
Acceleration (g)
0.2
0
−0.2
−0.4
−0.6
−0.8
0
50
100
150
Time (sec)
Navigation System Design
Eduardo Nebot
Slide 111
Fundamental Principles of Gyroscopes
These devices return a signal proportional to the rotational velocity. There is a large variety of
gyroscopes that are based on different principles. The price and quality of these sensors varies
significantly.
Vibratory gyroscopes
These types of gyroscopes are based on obtaining rotation rate by measuring coriolis acceleration.
They can be modelled by a simple mass-spring system as shown in the Figure. The purpose of the
gyroscope is to measure the angular velocity of the particle that is supposed to be rotating about the
axis OZ.
Y
Primary
motion
Secondary
motion
O
Navigation System Design
X
Eduardo Nebot
Slide 112
Vibratory gyroscopes
The particle is made to vibrate with constant amplitude along the axis OX. This primary motion
is controlled by an embedded circuit that maintains the oscillation at constant amplitude.
Under rotation the mass will experience a coriolis inertia force that will be proportional to the
applied rate of turn and will have a direction parallel to the OY axis. This secondary motion
amplitude is measured to provide information proportional to the angular rotation.
Y
Primary
motion
Secondary
motion
O
X
Some devices provide an additional feedback mechanism for this secondary motion to increase the
linearity of the sensor.
Navigation System Design
Eduardo Nebot
Slide 113
Vibratory gyroscopes
The Performance of gyroscopes is mainly characterized by two parameters: scale factor and drift.
A gyro with low distortion in the scale factor will be able to accurately sense angular rates at
different angular velocities. Various system imperfections and as well as environmental conditions can
cause significant variations in the scale factor.
Gyro drift is the nominal output of the gyro in steady state. This non-zero output is usually
temperature dependent.
Murata manufactures a gyroscope model Gyrostar ENV05 that is based on this principle. This
device presents very good high frequency characteristics but poor low frequency performance. The
drift expected is in the order of 1 degree / minute.
Better performance can be expected from the Vibrating Structure Gyroscopes from BAE and the
quartz rate sensors build by Systron Donner. These devices have a drift of less that 0.15
degree/minute and the linearity is better than 0.05 % of the full range.
Navigation System Design
Eduardo Nebot
Slide 114
Fiber Optic Gyros
The fiber optic gyros are based on the Sagnac effect discovered by Georges Sagnac in 1913:
Assume two waves of light circulating in opposite direction around a path of radius R. If the source is
rotating at speed ω, the light traveling in the opposite direction will reach the source sooner than the
wave traveling in the same direction, as shown in Figure. The wave traveling with the rotation will
covers a distance D1 in a transit time t1, while the other signal covers a distance t2 in time D2
D1 = 2πR − Rωt1
(92)
D2 = 2πR + Rωt2
t1
t2
Light
source
Navigation System Design
R
ω
Eduardo Nebot
Slide 115
Fiber Optic Gyros
Making the waves travel the path N times, the difference in transit time becomes:
4πN R2ω
∆t = N (t2 − t1) =
c2
It is important to relate this time difference with a phase shift at a particular frequency.
φ = 2π∆tf
(93)
(94)
For a given rotation ω the phase shift will be
8π 2RN
∆φ =
ω
(95)
c
Most low cost implementations of these devices works in an open loop manner. The maximum phase
shift that can be evaluated without ambiguities is 90 degrees.
There are commercial laser gyros such as the KVH model Ecore 2000 and 1000, which are capable
of drifts as low as 0.036 and 0.12 degree per minute respectively. Closed loop optical gyros are also
available but they are more expensive
Navigation System Design
Eduardo Nebot
Slide 116
Sensor Errors
The measurement errors of inertial sensors are dependent on the physical
operational principle of the sensor itself. The general error equation used for gyros is:


a
 x


δωx = b + bg  ay  + sf ωx + my ωy + mz ωz + η,


az
(96)
and that of the accelerometers as
δfx = b + sf ax + my ay + mz az + η,
(97)
• b is the residual biases
• bg is a 1 × 3 vector representing the g-dependent bias coefficients
• sf is the scale factor term
• m represents mounting and internal misalignments and
• η is random noise on the sensor signal
Apart from the random noise term η, all other error terms are, in principle, predictable thus
allowing for some form of compensation.
Navigation System Design
Eduardo Nebot
Slide 117
Evaluation of the Error Components
Tests which can be performed to gyros and accelerometers in order to systematically remove the
errors.
Temperature and memory effect play a significant role in the stability of the output of low
cost inertial sensors. It is for this reason that when one purchases low cost inertial units, not all the
values for the error terms are available, and so testing is required based on the application at hand.
If the gyro is left stationary then the only error term encountered is that of the g-independent
bias. This value is strongly correlated with temperature. Thus by cycling through the temperature
that would be encountered in the target application the bias on the gyro can be determined.
The better the gyro, the smaller the bias variation over the temperature range. Furthermore, the
better the sensor the greater the linearity of this bias variation.
Hysterisis effect is common in most inertial sensors. Thus in an ensemble of tests, cycling through
the temperature in the same manner each time, the variation in the bias between ensembles can be
determined. This is known as the switch-on to switch-on bias.
Navigation System Design
Eduardo Nebot
Slide 118
Evaluation of the Error Components
When testing for the remaining error components, the g-independent bias is assumed known and is
removed. Start with a rate table and place the gyro with the sensitive axis orthogonal to input
rotation vector. Any output signal will be due to internal misalignment of the sensor’s input axis.
With the misalignment and g-independent bias available, the gyro is placed on the rate table with its
sensitive axis parallel to rotation input.
The rate table is cycled from stationary to the maximum rotation measurable by the gyro in steps,
holding the rotation rate at particular points. The scale factor and the scale factor linearity
can then be determined by comparing the output of the gyro to the rotation rate input. With low
cost gyros the temperature also has a part to play with scale factor stability, thus the tests
should also be conducted with varying temperature.
Finally the gyro can be placed in a centrifuge machine which applies a rotation rate and acceleration
to the gyro. The output reading from the gyro minus the previous terms calibrated, results in the
determination of bias due to acceleration or g-dependent bias.
Navigation System Design
Eduardo Nebot
Slide 119
Evaluation of the Error Components
By mathematically integrating Equations 96 and 97, the effect of each error term on the performance
of the gyro can be determined. For each error term that is accounted for there is a corresponding
increase in performance.
Table 1 compares the error values available for the RLG, FOG, ceramic and silicon VSGs
Characteristic
RLG
FOG
g-independent bias o/hr
0.001-10
0.5-50
360-1800
> 2000
g-dependent bias o/hr/g
0
<1
36-180
36-180
5 - 100
5 - 100
scale factor non-linearity % 0.2 - 0.3 0.05 - 0.5
Ceramic VSG Si VSG
Table 1: Comparison of some of the major errors with various gyro implementations
Navigation System Design
Eduardo Nebot
Slide 120
Faults associated with Inertial Sensors
The accelerometers measure the absolute acceleration with respect to an inertial frame. We are
interested in the translational acceleration hence the algorithms used should compensate for all
other accelerations.
For practical purposes it can be considered that gravity is the only other acceleration present. We
have seen that the average acceleration in a land vehicle is smaller that 0.3 g. This implies that the
orientation of the accelerometer has to be known with very good accuracy in order to
compensate for gravity without introducing errors comparable to the actual translation acceleration.
0.6
0.4
Acceleration (g)
0.2
0
−0.2
−0.4
−0.6
−0.8
0
50
100
150
Time (sec)
This has been the main limitation that has prevented the widespread use of inertial sensors as
position sensors in low cost land navigation applications.
Navigation System Design
Eduardo Nebot
Slide 121
Faults associated with Inertial Sensors
The orientation of the accelerometer can be tracked using a gyroscope. The output of
these sensors are always contaminated by noise and drift. For short periods of time the drift can
be approximated by a constant bias.
The orientation is evaluated with a single integration of the gyroscope output:
Z
Z
θm = θ̇ + b + ν dt = θ + bt + ν dt
(98)
The integration returns the rotation angle with two additional undesired terms:
• Random walk: due to noise
• Bias generating an error that grow with time
Navigation System Design
Eduardo Nebot
Slide 122
Gyro drift
Figure 14: Errors due to drift in the gyro
From the figure we can see that the gyro drift will introduce an error in acceleration given by
a = ax sin(bt)
Navigation System Design
Eduardo Nebot
(99)
Slide 123
Gyro drift
For small angle increments the errors in acceleration, velocity and position are given by:
a = axbt
1
v = axbt2
2
1
p = axbt3
6
(100)
(101)
(102)
A constant gyro bias introduces an error in position determination that grows proportional to t3. For
standard low cost, good quality gyros the bias expected is in the order of 1-10 degrees / hour.
Without calibration, the bias could introduce an error of approximately 140 meters due to
the incorrect compensation of gravity after only 2 minutes of operation.
Another aspect to considered is the resolution of the analog to digital converter. The original noise
standard deviation of the gyro noise could be deterirated selecting an inappropriate digital to analog
converter. A gyro with stability of the order of 1 degree per hour that works in a range of +/-100
deg/sec and will require a 16-bit converter to operate under its specifications.
Navigation System Design
Eduardo Nebot
Slide 124
Accelerometer bias
The bias in the accelerometer will also increase the error in position and is proportional to the square
of time:
v = ba t
(103)
1
p = ba t2
(104)
2
The Figure presents the bias measured in a set of identical accelerometers corresponding to an
inertial unit over a period of six hours whilst they were stationary.
−3
5
x 10
X accelerometer
Y accelerometer
0.03
Acceleration (g)
Acceleration (g)
0
−5
−10
−15
−20
0
2
4
6
0.025
0.02
0.015
0.01
0
2
Z accelerometer
6
Temperature
−1
30
−1.01
Temp (C)
Acceleration (g)
4
−1.02
25
20
−1.03
−1.04
0
2
4
Time (hours)
6
15
0
2
4
Time (hours)
6
Figure 15: Bias change of the accelerometers over a period of 6 hours.
Navigation System Design
Eduardo Nebot
Slide 125
Accelerometer bias
The biases do not remain constant and in fact it can be clearly seen that any one accelerometer is not
indicative of the general performance of the other accelerometers.
The different biases values indicates that they are physically different low cost inertial sensors.
The change in the value occur due to an increase of the temperature of the unit due to ambient
temperature variations.
The estimation of the biases needs to be done each time the vehicle is stationary to minimize this
problem.
The calibration procedure must incorporate the identification of gyro and accelerometer biases at the
beginning of the navigation task and become active each time the vehicle stops. These identification
of the biases can also be done on-line but information from other sensors is required.
−3
5
x 10
X accelerometer
Y accelerometer
0.03
Acceleration (g)
Acceleration (g)
0
−5
−10
−15
−20
0
2
4
6
0.025
0.02
0.015
0.01
0
2
Z accelerometer
Temp (C)
Acceleration (g)
6
30
−1.01
−1.02
25
20
−1.03
−1.04
Navigation System Design
4
Temperature
−1
0
2
4
Time (hours)
6
15
0
2
4
Time (hours)
Eduardo Nebot
6
Slide 126
Random Walk
R
A second mayor error is due to the integration of random walk ( ν dt) which causes a growing error
term known as random walk.
The Figure presents the effects of integrating a gyro signal on a stationary unit for various runs after
the bias was removed. Although the average error is zero in any particular run, the error grow will
occur in a random direction.
Random Walk − X gyro
0.3
0.2
0.1
Angle (degrees)
0
−0.1
−0.2
−0.3
−0.4
−0.5
Navigation System Design
0
0.5
1
1.5
Time (min)
Eduardo Nebot
2
2.5
3
Slide 127
Random Walk
Assuming unity strength white Gaussian noise and letting x(k) =
Z t
Z t
E[ ν(u)δu] =
E[ν(u)]δu
0
and the variance is
Z
2
E[x (t)] = E[
0
ν(u)δ(u), then the mean is:
(105)
0
Z
t
ν(u)δu
0
Rt
Z tZ
t
t
ν(v)δv] =
0
E[ν(u)ν(v)]δuδv
0
(106)
0
E[ν(u)ν(v)]is the auto-correlation function, which in this case (assuming uncorrelated errors) is
simply a Delta Dirac function δ(u − v). Hence Equation 106 becomes:
Z tZ t
E[x2(t)] =
δ(u − v)δuδv = t
(107)
0
The standard deviation of the noise is:
0
√
σν = t
(108)
Therefore the white noise will cause an unbounded error growth whose value at any particular point
√
will be bounded by ±2 t 95 % of the time. For white noise of strength K the standard deviation is
√
σν = K t. The larger the standard deviation of the noise the greater the standard deviation of the
error. ( a good gyro can be degraded with inappropriate signal conditioning and electronic !)
Navigation System Design
Eduardo Nebot
Slide 128
Application Inertial Sensors
Inclinometers: These sensors are based on accelerometers that resolve the direction of the gravity
vector.
The gravity vector is a known quantity that is constant in magnitude and direction. The Figure
presents a basic example of a tilt sensor. These sensors are usually calibrated through a look-up table
G
G
to determine the offset as a function of temperature.
r
ete
rom
ele
Acc
B
Accelerometer
Vout=Accelerometer Offset
Navigation System Design
Vout=Scale Factor *G * Sin(B) +
Accelerometer Offset
Eduardo Nebot
Slide 129
Inclinometers
There are tilt sensors available for this purpose that can provide accurate information while the
vehicle is stationary.
Low cost devices can provide bank and elevation information with an accuracy of up to 0.1 degrees.
These sensors can not be used when the platform is moving since they are sensitive to translational
and rotational accelerations.
Pendulum accelerometers are also used to determine the bank and elevation angles. These
devices use the principle that a pendulum with an equivalent radius of the earth will always point to
the vertical irrespective of the acceleration. This effect can be approximated with appropriate
feedback loop compensation. Although they are able to obtain some rejection of the undesired
acceleration due to the movement of the platform they can only be used under very low vehicle
accelerations.
These sensors are also used to determine the initial attitude of inertial platform.
Navigation System Design
Eduardo Nebot
Slide 130
Vibration Analysis
Vibration Analysis consist of measuring the frequency strength over a range of frequencies of
vibrations.
Machinery vibration problems can consume excessive power and impose additional wear on bearings,
seals, couplings and foundations. Vibrations are typically caused by machinery misalignment and
unbalance.
These problems can be detected by obtaining the frequency response of the vibration of the machine
under investigation. This is commonly done via analysis of the FFT of an acceleration signal.
There are other application where vibration monitoring may be useful such as the analysis and
identification of vibration sources, problems in structures, vibration and shock testing, analysis to
ensure products comply with specified vibration required for the application etc.
Navigation System Design
Eduardo Nebot
Slide 131
Heading
The Integration of the gyro rate information provides the orientation of the platform.
A good quality gyro will have zero or constant bias, and small noise variance.
There are many types of gyroscopes but few in the price range that can be afforded in commercial
mobile robot applications.
Fiber optic and vibratory gyroscopes have been released with satisfactory specifications and
reasonable price ranges for many navigation applications
Navigation System Design
Eduardo Nebot
Slide 132
Inertial Measurement Unit(IMU)
A IMU consists of at least three (triaxial) accelerometers and three orthogonal gyroscopes that
provide measurements of acceleration in three dimensions and rotation rates about three axes.
The Physical implementation of inertial sensors can take on two forms:
• Gimballed arrangement
• Strapdown
Navigation System Design
Eduardo Nebot
Slide 133
Gimballed IMU
This unit consists of a platform suspended by a gymbal structure that that allows three degree of
freedom, as shown in the Figure. The vehicle can then perform any trajectory in 3-D while the
platform is maintained level with respect to a desired navigation frame using the feedback control
loops. These loops use the gyro signal to provide appropriate control signal to the actuators places in
each gymbal. With this approach the platform can be maintained aligned with respect to a
coordinate system. The information provided by the accelerometers can then be integrated directly
to obtain position and velocities.
Navigation System Design
Eduardo Nebot
Slide 134
Strapdown IMU
The IMU system assembled from low cost solid-state components is almost always constructed in a
”strap-down” configuration. This term means that all of the gyros and accelerometers are fixed to a
common chassis and are not actively controlled on gimbals to align themselves in a pre-specified
direction.
This design has the advantage of eliminating all moving parts.
Once true linear acceleration has been determined, vehicle position may be obtained, in principle, by
double integration of the acceleration. Vehicle orientation and attitude may also is determined by
integration of the rotation rates of the gyros.
z
Az
Gz
Ax
x
y
Ay
G
y
Gx
Navigation System Design
Eduardo Nebot
Slide 135
Coordinate Frames
Navigation System Design
Eduardo Nebot
Slide 136
Coordinate Frames
Inertial Navigation systems require the transformation of the different measurement quantities
between different frames of reference.
The accelerometer and gyros measure acceleration and rotation rates with respect to an inertial frame.
Other sensors such as Global Positioning System (GPS) report information in Earth Centered Earth
Fixed (ECEF) coordinate frame.
The navigation system needs to transform all the information to a common coordinate frame to
provide the best estimate and sometime be able to report the output in various other coordinate
frames. This section will introduce the different coordinate system used in this course.
Navigation System Design
Eduardo Nebot
Slide 137
Inertial Frame
According to Newtown’s law of motion, an inertial frame is a frame that does not rotate or
accelerate. It is almost impossible to find a true inertial frame.
A good approximation is the one that is inertial with a distant star.
For practical purposes we define an inertial frame with origin at the earth center of mass, with x axis
pointing towards the vernal equinox at time T0, the z axis along the earth’s spin axis and the y axis
completing the rigth handed system.
Navigation System Design
Eduardo Nebot
Slide 138
Earth-centered Earth-Fixed (ECEF)Frame
This frame has origin at the mass centre of the Earth, the x axis points to the Greenwich Meridian in
the equatorial plane, the y plane at 90 degree east in the same plane and the z axis in the direction of
rotation of the reference ellipsoid.
This frame is not an inertial frame. It can be approximated to an inertial frame by a negative
rotation equivalent to the Greenwich mean Sidereal Time (GMST). The earth’s geoid is a surface
that minimize the difference with the earth’s sea level. This geoid is approximated with an ellipsoid
around the earth’s minor axis. Different parameter are available for different areas of the world. The
most common reference ellipsoid used is the WGS 84.
Navigation System Design
Eduardo Nebot
Slide 139
Local Level Earth Frame
This frame, also known as Geographic or Earth frame, is defined locally relative to the earth’s geoid.
The axis x points to the North’s direction, the z axis is perpendicular to the tangent of the ellipsoid
pointing towards the interior of the earth, not necessarily to the centre of the earth. Finally the y
axis points east to complete a right handed orthogonal system
Geocentric frame:
This frame is similar to the Local Level frame with the difference that the z axis points to the centre
of the earth.
Local Geodetic frame:
This coordinate system is very similar to the Local level Frame. The main difference is that when the
system is in motion the tangent plane origin is fixed, while in the local level frame the origin is the
projection of the platform origin into the earth’s geodic. This is frame is mainly used to perform
navigation in local areas relative to a given origin point.
Navigation System Design
Eduardo Nebot
Slide 140
Wander Frame
The Local Level frame presents some numerical problems to integrate the data from inertial system
when close to the poles.
In this areas significant rotation around the axis z are required to maintain the x axis pointing
North. This is true even with small movement in the y direction. This problem can be solved
performing all the computation in a frame that does not require to point to North. The x axis then
wanders from North at a rate chosen by the user.
Body Frame
Most navigation systems have sensors attached to the vehicle. The body frame constitutes a new
frame that is rigidly attached to the vehicle. The origin of this frame can be arbitrarily chosen
although some location may simplify the kinematics models.
Navigation System Design
Eduardo Nebot
Slide 141
Summary of Frames
The Figure shows the coordinates system used in this course. Absolute sensors like GPS provide
position in ECEF coordinates, using X, Y, Z or latitude, Longitude and Height Φ, λ, H. Inertial
sensors will return information in its body frame, which is in permanent rotation and translation
from the navigation frame N, E, D. All the sensory information needs to be converted to the
navigation frame prior to perform the data fusion task to obtain the best position estimate.
Z
N
E
D
φ
Y
λ
X
Navigation System Design
Eduardo Nebot
Slide 142
Inertial Navigation Equations
This section presents the derivation of the inertial navigation equations for global and local area
navigation.
The basic Idea:
Once the acceleration in the direction of travel is obtained the position and velocity can be evaluated:
vx(t) =
px(t) =
R
R
ax(t) dt
vx(t) dt
Although the problem looks simple the main challenge is to discriminate the value of the
translational acceleration from the absolute acceleration measured by the accelerometer.
Navigation System Design
Eduardo Nebot
Slide 143
The Coriolis Theorem
Navigation with respect to a rotating frame such as the earth requires the Coriolis theorem.
The theorem states that the velocity of a vehicle with respect to a fixed inertial frame vi, is equal to
the ground speed of a vehicle ve (the velocity of the vehicle with respect to the earth), plus the
velocity of the vehicle due to the rotation rate of the earth with respect to the inertial frame ω ie, at
the point on earth where the vehicle is located r, that is,
vi = ve + ω ie × r,
(109)
where ω ie = [0 0 Ω] and Ω is the rotation rate of the Earth .
Differentiating this equation with respect to the inertial frame gives
v̇i|i = v̇e|i + ω̇ ie|i × r + ω ie × v|i.
Navigation System Design
Eduardo Nebot
(110)
Slide 144
The Coriolis Theorem
Assuming that the angular acceleration of the Earth is zero, that is ω̇ ie = 0, and substituting
Equation 109 into 110,
ω ie × v|i = ω ie × [ve + ω ie × r]
= ω ie × ve + ω ie × [ω ie × r],
(111)
v̇i|i = v̇e|i + ω ie × ve + ω ie × [ω ie × r].
(112)
hence Equation 110 becomes
Now v̇i|i = f + g where f is the specific force encountered due to the vehicle motion and g is the force
due to gravity. Hence Equation 112 becomes
f + g = v̇e|i + ω ie × ve + ω ie × [ω ie × r],
Navigation System Design
Eduardo Nebot
(113)
Slide 145
The Coriolis Theorem
that is,
v̇e|i = f − [ω ie × ve] + [g − ω ie × [ω ie × r]].
(114)
The Equation states that the acceleration over the Earth’s surface is equal to:
The acceleration measured by the accelerometers compensated for:
• The Coriolis acceleration encountered due to the velocity of the vehicle over a rotating Earth,
ω ie × ve
• The local gravity acceleration which comprises of
– the Earth’s gravity g,
– acceleration due to the rotation of the Earth, also known centripetal acceleration ω ie × [ω ie × r].
Navigation System Design
Eduardo Nebot
Slide 146
Navigation in large areas
The Transport frame is used when a vehicle travels over large distances around the earth such as in
missile or air transport applications.
The objective of this framework is to obtain the ground speed of a vehicle with respect to a
navigation frame, generally expressed in North, East and Down coordinates, at a given location,
expressed in latitude, longitude and height.
In such situations the rotation of the earth needs to be taken into consideration along with the fact
that the navigation frame is also rotating.
As an example, constantly keeping the North axis aligned on a flight from Sydney to London will
require the navigation frame to constantly rotate. This, coupled with the fact that the earth is
rotating during this transition, causes a Coriolis acceleration between the navigation frame, the earth
rotation and the ground speed of the vehicle.
Navigation System Design
Eduardo Nebot
Slide 147
Navigation in large areas
The Transport framework is defined as: the acceleration of the vehicle with respect to the navigation
frame v̇e|n, is equal to the acceleration of the vehicle with respect to the inertial frame v̇e|i, minus the
Coriolis acceleration due to the navigation frame rotating ω en, on a rotating earth ω ie. That is,
v̇e|n = v̇e|i − (ω ie + ω en) × ve.
(115)
Substituting in Equation 114,
v̇e|n = fn − [ω ie × ve] + [g − ω ie × [ω ie × r]] − (ω ie + ω en) × ve
= fn − (2ω ie + ω en) × ve + [g − ω ie × [ω ie × r]],
(116)
where fn represents the acceleration in the navigation frame. Since the inertial unit measures the
acceleration in the body frame fb, this acceleration needs to be transformed into the navigation
frame, that is,
fn = Υfb.
(117)
The transformation Υ, can take on three forms as will be discussed later.
Navigation System Design
Eduardo Nebot
Slide 148
Navigation in large areas
The rotation data relating the body to navigation frame ω bn is now required.
The gyros measure the total inertial rotation ω ib which comprises of ω bn plus the rotation of the
navigation frame with respect to the inertial frame, which is the rotation rate of the navigation frame
with respect to the earth plus the rotation rate of the earth with respect to the inertial frame,
ω bn = ω ib − [ω ie + ω en],
(118)
where ω en is known as the transport rate and is defined as
−vx −vy tan L
vy
,
,
].
(119)
ω en = [
Ro + h Ro + h Ro + h
L is the latitude of the vehicle, Ro is the radius of the Earth and h is the height of the vehicle above
the earth’s surface. The transport rate is what rotates the navigation frame as the vehicle travels
across the surface of the earth.
Navigation System Design
Eduardo Nebot
Slide 149
Navigation in local areas, Earth Frame
In the Earth frame the acceleration of the vehicle with respect to the earth v̇e|e, is equal to the
acceleration of the vehicle with respect to the inertial frame v̇e|i, minus the Coriolis acceleration due
to the velocity of the vehicle ve, over a rotating earth ω ie,
v̇e|e = v̇e|i − ω ie × ve.
(120)
The Earth frame will be used throughout this work since its definition is well suited for land vehicle
applications. Substituting in Equation 114 gives
v̇e|e = fe − 2[ω ie × ve] + [g − ω ie × [ω ie × re]].
(121)
Since inertial unit measures the acceleration in the body frame, the acceleration measurements need
to be transformed into the earth frame,
fe = Υf b,
(122)
where Υ now comprises of the rotation rates ω be which relates the body to earth frame. However,
the gyros measure the total inertial rotation ω ib which comprises of ω be plus the rotation of the earth
with respect to the inertial frame transformed over to the body frame, so
ω be = ω ib − Υbeω ie.
Navigation System Design
Eduardo Nebot
(123)
Slide 150
Schuler Damping
The major difference between the Transport and Earth frame equations is the transport rate defined
by Equation 124.
ω en = [
vy
−vx −vy tan L
,
,
].
Ro + h Ro + h Ro + h
(124)
The transport rate is subtracted from the body rates and hence defines the attitude of the system
with respect to the Transport frame.
Incorporating the transport rate also adds a bound to the errors associated with inertial systems.
This is because the acceleration in the navigation frame is twice integrated to provide velocity and
then position. The velocity is used to determine the transport rate which is then subtracted from the
body rates.
For example, assume that the pitch is in error by a small angle δβ. This error in turn causes an
incorrect calculation of the acceleration due to the misalignment and the measurement of gravity
given by gδβ. Repeating this process shows that misalignment feeds back onto itself.
Navigation System Design
Eduardo Nebot
Slide 151
Schuler Damping
The characteristic equation of such a system exhibits simple harmonic motion with period
r
g
,
ωs =
Ro
or 84.4 minutes. This is known as the Schuler oscillation. In essence, by implementing the Transport
frame, the errors are bounded within the Schuler oscillations.
If a system is tuned to the Schuler frequency then some general rules of thumb can be obtained for
inertial systems:
s t)
• An initial velocity error δvo, causes a position error of δvo sin(ω
ωs ;
• An initial attitude error δβ, causes a position error of δβR(1 − cos(ωst));
s t)
• An acceleration bias δf , causes a position error of δf ( 1−cos(ω
); and
2
ω
s
s t)
• A gyro bias δωb, causes a position error of δωbR(t − sin(ω
ωs ).
Thus for all errors, except for gyro bias, the position error is contained within the Schuler oscillation.
With accurate gyros, passenger airlines can travel vast distances relying solely on inertial navigation.
Navigation System Design
Eduardo Nebot
Slide 152
Attitude Algorithms
The transformation matrix Υ is required to evaluate the acceleration vector in the desired frame
given the acceleration vector in the body frame.
This transformation matrix has to be accurate since misalignment (errors in estimated attitude)
causes the components of the gravity vector measured by the accelerometers to be confused with true
vehicle acceleration. Integrated over time, even small misalignment errors will cause large estimated
errors.
The transformation matrix consists of the roll, pitch and yaw angles required to rotate the
body frame to the navigation frame and hence is updated continuously since the body frame
is always rotating with respect to the navigation frame.
The updating process propagates this matrix based on data obtained from the gyros, thus any errors
in Υ is caused by both the physical errors associated with the gyros and the errors in the algorithms
used to propagate the transformation matrix.
Navigation System Design
Eduardo Nebot
Slide 153
Attitude Algorithms
There are a number of algorithms available for attitude propagation. However, regardless of the type
of attitude algorithm implemented, in their analytical forms they provide identical results.
The choice of algorithm depends on the processing power available, on the errors introduced due to
discretisation of algorithms, and on the errors introduced due to the simplification of the algorithms.
The simplification of algorithms is to ease the computational load on the processor.
There are in principle three approaches to propagating the transformation matrix:
• Euler method
• Direction Cosine method
• Quaternion method
The Euler approach is conceptually easy to understand although it has the greatest computational
expense. Although the Quaternion approach is computational inexpensive compared to the other
two, it has no direct physical interpretation to the motion of the vehicle. The Direction Cosine
approach fits in between, both in terms of physical understanding and in computational expense
Navigation System Design
Eduardo Nebot
Slide 154
Euler Representation
The Euler representation is the mathematical analog form of a gimbal system. The roll θ, pitch β
and yaw γ angles used to represent the rotations from the body frame to the navigation frame are
placed in a transformation matrix Enb, by multiplying the consecutive rotation matrices representing
a roll, followed by a pitch and then a yaw,

E nb


1 0 0
β 0 βs
γ −γs 0

 c
 c



=  γs γc 0   0 1 0   0 θc −θs



0 θs θc
−βs 0 βc
0 0 1


β γ −θcγs + θsβsγc θsγs + θcβsγc

 c c


=  βcγs θcγc + θsβsγs −θsγc + θcβsγs 


−βs
θsβc
θcβc





(125)
where subscripts s and c represent sine and cosine of the angle.
Navigation System Design
Eduardo Nebot
Slide 155
Euler Representation
As the body frame rotates, the new angles are obtained from
θ̇ = (ωy sin θ + ωz cos θ) tan β + ωx
(126)
β̇ = ωy cos θ − ωz sin θ
(127)
γ̇ = (ωy sin θ + ωz cos θ) sec β.
(128)
where ω is the rotation rate measured by the gyros about the x, y and z axes.
Both the roll and yaw updates have singularities when the pitch of the vehicle is π2 .
In a strapdown system, as the vehicle approaches
π
2
then theoretically infinite word length and
iteration rates are required in order to accurately obtain the result.
It is for this reason that Euler updates are not generally implemented, although this is not a problem
for land vehicle applications.
This approach is computationally expensive due to the trigonometry required in the updates and in
forming E nb.
Navigation System Design
Eduardo Nebot
Slide 156
Euler Representation
The discretisation procedure is as follows:
• The new roll angle is obtained through integration by
Z
θ(i + 1) =
θ̇dt + θ(i)
(129)
and similarly for pitch and yaw. The angles are then placed into the matrix to form Enb(i + 1).
• Obtain the acceleration data in the body frame fb = [fx, fy , fz ] and evaluate the acceleration in
the navigation frame
fn = Enbfb
(130)
• Integrate the acceleration to obtain velocity and then position.
Navigation System Design
Eduardo Nebot
Slide 157
Direction Cosine Matrix (DCM) Representation
The direction cosine matrix Cnb, is a 3 × 3 matrix containing the cosine of the angles between the
body frame and the navigation frame. Cnb is propagated by
n
Ċ b = CnbΩbn,
(131)
where Ωbn is a skew-symmetric matrix representing rotation rates as measured by the gyros,

Ωbn

0 −ωz ωy




=  ωz
0 −ωx  .


−ωy ωx
0
(132)
The initial Cnb is simply the initial Enb.
Navigation System Design
Eduardo Nebot
Slide 158
Direction Cosine Matrix (DCM) Representation
The discretisation procedure is as follows:
• Obtain the gyro outputs ωx, ωy , ωz and integrate to determine the change in angle φx, φy , φz .
• Determine the angular vector magnitude
q
φ =
φ2x + φ2y + φ2z
(133)
• Determine the series coefficients
sin φ
φ
1 − cos φ
β =
φ2
(134)
α =
• Form the angular skew matrix

(135)

0 −φz φy




φ =  φz
0 −φx


−φy φx 0
Navigation System Design
Eduardo Nebot
(136)
Slide 159
Direction Cosine Matrix (DCM) Representation
• Update the direction cosine matrix
2
Cnb(i + 1) = Cnb(i)[I3×3 + αφ + βφ ]
(137)
The transformation matrix can be simplified if one assumes small angular rotations (< 0.05deg)
Cnb(i + 1) = Cnb(i) [I + ∆θ]
with

(138)

0
−∆θY ∆θP




∆θ =  ∆θY
0
−∆θR 


−∆θP ∆θR
0
(139)
• Obtain the acceleration in the body frame and evaluate the acceleration in the navigation frame
fn = Cnb(i + 1)fb
(140)
• Integrate the acceleration to obtain velocity and then position.
Navigation System Design
Eduardo Nebot
Slide 160
Quaternion Representation
In the Quaternion approach the rotation from one frame to another can be accomplished by a single
rotation about a vector q through an angle q. A quaternion consists of four parameters which are a
function of this vector and angle.
The initial quaternion is obtained from the roll, pitch and yaw angles defined in the Euler
representation or alternatively through the Direction Cosine parameters,




q(i) = 



0.5 ∗
q
1 + Cbn11 + Cbn22 + Cbn33
1
n
4×q(1) (Cb32
− Cbn23 )
1
n
4×q(1) (Cb13
−
Cbn31 )
1
n
4×q(1) (Cb21
− Cbn12 )








(141)
The discretisation procedure is as follows:
• Obtain the gyro outputs ωx, ωy , ωz and integrate to determine change in angle φx, φy , φz .
Navigation System Design
Eduardo Nebot
Slide 161
• Determine the quaternion operator coefficients
sin φ2
γ =
2
φ
δ = cos
2
• Determine the quaternion operator

(142)
(143)

δ




 γ φx 

h(i) = 


 γ φy 


γ φz
• Generate the quaternion matrix and update

q(i)1


q(i)2
q(i + 1) = 

q(i)3

q(i)4
Navigation System Design
(144)

−q(i)2 −q(i)3 −q(i)4


q(i)1 −q(i)4 q(i)3 
 h(i)

q(i)4 q(i)1 −q(i)2

−q(i)3 q(i)2 q(i)1
Eduardo Nebot
(145)
Slide 162
Quaternion Representation
• Obtain the acceleration data in the body frame and evaluate the acceleration in the navigation
frame.
fn = q(i + 1)fTb q∗(i + 1),
(146)
where q∗ is the complex conjugate of q. A less computationally expensive approach is taken by
converting the quaternion back into Cnb by


(q12 + q22 − q32 − q42)
2(q2q3 − q1q4)
2(q2q4 + q1q3)




n
Cb =  2(q2q3 + q1q4)
(q12 − q22 + q32 − q42)
2(q3q4 − q1q2)  ,


2(q2q4 − q1q3)
2(q3q4 + q1q2)
(q12 − q22 − q32 + q42)
and then
fn = Cnbfb
(147)
• Integrate the acceleration to obtain velocity and then position.
Navigation System Design
Eduardo Nebot
Slide 163
Discussion of Algorithms
The Euler approach is not commonly used due to the presence of the roll/yaw singularity.
This does not pose a problem for land vehicle applications. However, the Euler approach requires
high computational effort and thus powerful processing if fast updates are required.
The Quaternion has the benefit of only requiring the update of four variables. It is most
often used in military and space applications. In order to save on computational cost, the quaternion
is converted to a DCM representation for transformation of the acceleration from the body frame to
the navigation frame. This can be a significant computational burden if fast sampling rates are
required. To overcome this most military users implement a dual sampling system.
As an example, a Laser INS (LINS) system typically samples Ring Laser Gyros (RLGs) at 1200Hz
and the accelerometers at 400Hz. The lower sampling rate is sufficient for typical LINS
navigation applications. Three samples of the RLGs are used to update the quaternion,
thus forming a “super” quaternion representing a single rotation. This quaternion is then converted
to the DCM representation and used to transform the acceleration measurements.
Navigation System Design
Eduardo Nebot
Slide 164
Discussion of Algorithms
The DCM approach is commonly used in all forms of inertial navigation. It also offers the best
representation for land navigation.
Although nine variables are updated at each sample, it is less computationally expensive than the
Euler representation, and unlike the Quaternion approach, requires no conversion over to another
representation in order to transform the acceleration data from the body frame to the navigation
frame.
In order to satisfy this small angle assumption, the update rates generally have to be high. For
example, land vehicles can undertake rotation rates of 50deg/sec, and so a sampling rate of at least
1kHz is required (to keep the angles below 0.05deg).
Such a sampling rate is high for low cost inertial systems although in more expensive systems used in
military and space applications 20kHz is not uncommon. If sampling rates are not that high then
the approximation may introduce an additional error.
Navigation System Design
Eduardo Nebot
Slide 165
Attitude evaluation error sources
The accuracy of the evaluation of attitude can be affected by a number of factors such as the
approximation made by the computation algorithm and the hardware limitation of the technology
used.
Errors in attitude computation: The update for the DCM requires:
2
Cnb(i + 1) = Cnb(i)[I3×3 + αφ + βφ ]
(148)
with
sin φ
φ
1 − cos φ
β =
φ2
α =
(149)
(150)
If these coefficients can be determined exactly then the DCM generated would be exact. However, in
practice a Taylor series expansion is required,
φ2 φ4
α = 1−
+
− ...
3!
5!
1 φ2 φ4
β =
−
+
− ...
2! 4!
6!
Navigation System Design
Eduardo Nebot
(151)
(152)
Slide 166
Attitude evaluation error sources
The error in the DCM algorithm can be represented as
1
(φa1 cos φ − sin φ + φ2a2 sin φ),
(153)
Ddc =
δt
where a1 = 1 and a2 = 0 for a first order algorithm, a1 = 1 and a2 = 0.5 for a second order
2
algorithm, a1 = 1 − φ3! and a2 = 0.5 for a third order algorithm and so on.
Table 2 shows the drift in the algorithm when the sampling rates are set to 10, 125, 200 and 500Hz
and the order of the algorithm is increased from a first to a third.
Order of Algorithm 10Hz
Table 2:
125Hz
200 Hz
500 Hz
1
30
0.23
0.04
0.012
2
14.7
0.11
0.021
0.006
3
0.001 5 × 10−7 4.85 × 10−7 1.4 × 10−9
Algorithm Drift Rates in o/hr caused by sampling a continuous rotation rate of 20deg/sec
Navigation System Design
Eduardo Nebot
Slide 167
Vibration
Vehicle vibration has a detrimental affect on the performance of inertial navigation systems. This is
due to the low bandwidth of low cost inertial sensors, causing attenuation or total rejection
of motion vibration at high frequencies.
Vibration is generally a combination of both angular and translational motion. If the bandwidth of
both the gyros and accelerometers are equal, and the vibration of the vehicle at frequencies above
this bandwidth has smaller magnitude than the resolution of the sensors (which is likely with low cost
units), then vibratory motion can safely be ignored.
However, the bandwidth of low cost gyros is almost always significantly less than for low cost
accelerometers. Vibrations cause small changes in attitude which is measured by the
accelerometers as a component of gravity. This acceleration component will be inaccurately
evaluated as apparent vehicle motion since the gyros can not measure this change in attitude.
Navigation System Design
Eduardo Nebot
Slide 168
Vibration
To overcome this problem it is necessary to introduce vibration absorbers into the physical system to
remove high frequency vibration.
As an example, ceramic VSGs have a bandwidth of approximately 70Hz. Thus the approach would
be to obtain vibration absorbers that attenuate as much of the signal as possible above 70Hz.
The effect of vibration on the attitude algorithms is termed ‘‘coning” or “non-commutativity”. The
definition of coning motion is simply stated as the cyclic motion of one axis due to the rotational
motion of the other two axes.
If one axis has an angular vibration rotation of A sin(ωt) and the other axis A sin(ωt + %), then the
third axis will have a motion describing a cone. A perfect cone motion would occur if the phase
difference % between the two axes is ninety degrees, otherwise the motion will exhibit some other
form (and in reality would be random).
Navigation System Design
Eduardo Nebot
Slide 169
Minimising the Attitude Errors
In light of the arguments presented, the approach taken for minimising the errors associated with
attitude algorithms for low cost inertial units will consist of the following steps:
• The bandwidth of the gyros employed will be the limiting factor in performance. Generally the
accelerometers employed and the sampling rate achievable will be higher than the bandwidth of
the gyro. If vehicle vibration exceeds that of the bandwidth of the gyro, then the only
reasonable choice is to use vibration absorbers to attenuate incoming signals above the
bandwidth of the gyro, taking into consideration the natural frequency of the absorber.
• The sampling rate of the inertial sensors should be above the Nyquist frequency and
furthermore should be as high as possible.
• The higher the order of the algorithm the better and this will come down to the
sampling rate and the processing power available.
Navigation System Design
Eduardo Nebot
Slide 170
Error Models for Inertial Systems
Navigation System Design
Eduardo Nebot
Slide 171
Error Models
The accuracy of inertial navigation will depend on:
• The accuracy of the sensors employed
• The computer implementation of the inertial algorithms
The inertial error equations are presented to analyse how these two effects contribute to the inertial
navigation system development.
These equations provide the position, velocity and attitude error growth, given the sensor
and algorithm errors. The development of the error equations is accomplished by perturbing (or
linearising) the nominal equations.
These error equations will then be used to implement the fusion algorithm with external data to
perform online alignment and update position and velocity information.
Navigation System Design
Eduardo Nebot
Slide 172
Error Models
Perturbation in the true frame: The error in a particular state is the difference between the estimated
state and the true state. Thus given that the Earth frame velocity equation is
v̇e|e = Cebfb − 2[ω ie × ve] + [g − ω ie × [ω ie × re]],
(154)
and defining the total gravity acceleration as
gt = [g − ω ie × [ω ie × re]],
(155)
then by definition of the perturbation
˜ee − v̇ ee
δ v̇ = v̇
e
= [C̃ b f˜b − Cebfb] − [2ω̃ eie ×ṽ ee −2ω eie × vee] + [g̃ t − g t].
(156)
e
˜e is the evaluated velocity vector and v̇ ee is the true velocity vector. The evaluated
where v̇
e
transformation matrix C̃ b , is the true transformation matrix C eb, misaligned by the misalignment
angles δψ. The misalignment is represented in skew-symmetric form [δψ×]. Hence
e
C̃ b = [I3×3 − [δψ×]]C eb,
(157)
δ v̇ = C ebf˜b − C ebf b − [δψ×]C ebf˜b − δ[2ω eie × v ee] + δg t.
(158)
thus
Navigation System Design
Eduardo Nebot
Slide 173
Error Models
If the errors in Coriolis and gravity terms are insignificant then
δ v̇ = C ebδf b − [δψ×]C ebf˜b
= C ebδf b − [δψ×]f˜e.
(159)
T
−[ψ×]f˜e = f˜e [δψ×],
(160)
T
δ v̇ = f˜e [δψ×] + C ebδf b
= [f˜e×]δψ + C ebδf b.
(161)
Now
hence
Thus the propagation of velocity errors in the Earth frame δ v̇ is equal to the acceleration error in the
Earth frame due to the misalignment of the frame [f˜e×]δψ, together with the errors associated with
the measurements of the acceleration δf b transformed over to the Earth frame via C eb. The errors in
the measurements of the acceleration are associated with the accelerometers and will be discussed
later in this section.
Navigation System Design
Eduardo Nebot
Slide 174
Error Models
Rearranging the attitude error Equation 157
e
[δψ×] = I3×3 − C̃ b C ebT ,
(162)
e
e eT
˙
˙
[δψ×] = −C̃ b Ċ b − C̃ b C ebT .
(163)
and differentiating gives
The updating process of the transformation matrix both for the true and evaluated frames are
e
e
e
˙
e e
Ċ b = C b Ωbe and C̃ b = C̃ b Ωebe.
(164)
Substituting into Equation 163 gives
e
e
˙
[δψ×]
= −C̃ b [C ebΩbbe]T − [C̃ b Ωbbe]C ebT
e
b
= −C̃ b [Ω̃be − Ωbbe]C ebT .
(165)
Perturbation of the rotation update matrix gives
b
δΩbbe = Ω̃be − Ωbbe,
(166)
e
˙
[δψ×]
= −C̃ b δΩbbeC ebT .
(167)
therefore
Navigation System Design
Eduardo Nebot
Slide 175
Error Models
Substituting Equation
e
C̃ b = [I3×3 − [δψ×]]C eb,
(168)
we have
˙
[δψ×]
= −[[I3×3 − δψ×]C eb]δΩebeC ebT
= −C ebδΩbbeC ebT + [δψ×]C ebδΩbbeC ebT .
(169)
The product of small terms δψ× and δΩ, is assumed zero. Therefore
˙
[δψ×]
= −C ebδΩbbeC ebT
= −C ebδΩbbeC be.
= −[C ebδω bbe×].
(170)
δω bbe = δω bib − C beδω eie.
(171)
The error form of Equation (gyro error)123 is
Navigation System Design
Eduardo Nebot
Slide 176
Error Models
Given that the rotation rate of the earth is known, thus δω eie = 0, then Equation 170 can be
rewritten as
˙
[δψ×]
= −C eb[δω bib×]
or
δ ψ̇ = −C ebδω bib.
(172)
That is, the propagation of attitude errors δ ψ̇ is simply the errors associated with the rotation rate
measurements δω bib, transformed over to the Earth frame via C eb. The errors associated with the
measurements are purely defined with the gyros and are discussed in the next section.
The propagation of the attitude errors is independent of position. Thus given the velocity and
attitude error propagation equations and an input trajectory, the error growth of the inertial system
can be determined.
The only terms which need to be determined are the errors in the acceleration measurement
δf b = [δfx, δfy , δfz ]T , and the errors in the rotation rate δω bib = [δωx, δωy , δωz ]T . These terms can
be experimentally evaluated. More detailed information in [4].
Navigation System Design
Eduardo Nebot
Slide 177
Calibration and Alignment of Inertial Measurement Units
Navigation System Design
Eduardo Nebot
Slide 178
Calibration and Alignment
The objective of calibration is to determine the biases in the accelerometers and gyros. This is
obtained by first determining the initial alignment of the inertial unit and hence in turn evaluating
the initial direction cosine matrix.
Alignment Techniques: If the accelerometer readings are known perfectly then the attitude of
the inertial unit can be determined by resolving the gravity component. In order to determine the
gravity component measured by the accelerometers, Equation 140 is rearranged to give
fb = (Cnb)−1fn
(173)
Since Cnb is orthogonal, its inverse is simply its transpose. The inertial unit is stationary and hence
the only acceleration measured is that due to gravity along the vertical axis. Thus
h
iT
fn = 0 0 −g .
Navigation System Design
Eduardo Nebot
Slide 179
(174)
Calibration and Alignment of and Inertial Measurement Unit
Using Equation 173 the acceleration measured in the body frame can be evaluated:





f
 xT

 fyT

fzT
βcγc
βcγs
−βs
0








 =  −θcγs + θsβsγc θcγc + θsβsγs θsβc   0 




θsγs + θcβsγc −θsγc + θcβsγs θcβc
−g
(175)
where the subscript T represents the true acceleration component due to gravity. Hence
fxT = g sin β
(176)
fyT = −g sin θ cos β
(177)
fzT = −g cos θ cos β
(178)
Rearranging Equation 181 to determine the pitch β, and substituting this into either Equations 182
or 183 will solve for roll θ. This procedure for determining alignment is called “coarse alignment”.
Navigation System Design
Eduardo Nebot
Slide 180
Calibration and Alignment of and Inertial Measurement Unit
If the accuracy provided by coarse alignment is not sufficient for navigation performance then
external alignment information will be required.
This information can come from tilt sensors or GNSS attitude information for example to
determine roll and pitch.
The heading of the vehicle γ can be determined with Gyrocompassing techniques. Once the
attitude of the unit is found, the reading on the gyros can be used to determine the components of
the Earth’s rotation, and by knowing the initial position of the vehicle, heading can be resolved.
However, with low cost gyros, gyrocompassing is generally not possible due to the high noise and low
resolution of these sensors. In such cases a compass is required to determine initial heading. The
compass can be very unreliable in the presence of large machines.
Navigation System Design
Eduardo Nebot
Slide 181
Alignment for Low Cost Inertial Units
For low accuracy inertial units, none of the common self aligning or calibration methods provide
results accurate enough for navigation.
Instead, the inertial unit uses two tilt sensors which provide measurements of the bank and elevation
of the inertial platform, thus providing the initial alignment information required. If the tilt sensor
are not available, the bank and elevation are evaluated with the accelerometer information.
A positive bank will cause the y-accelerometer to measure a component of gravity equal to
fyT = −g sin(bank)
(179)
Similarly a positive elevation will cause the x-accelerometer to measure
fxT = g sin(elevation)
Navigation System Design
Eduardo Nebot
(180)
Slide 182
Alignment for Low Cost Inertial Units
From the Cnb Transformation equation
fxT = g sin β
(181)
fyT = −g sin θ cos β
(182)
fzT = −g cos θ cos β
(183)
Equating Equation 181 with 180, and Equation 182 with 179, the pitch and roll angles are
β = elevation
sin(bank)
θ = sin−1(
)
cos β
To resolve the heading a compass is used.
(184)
(185)
Equations 184 and 185 are used along with the initial heading angle to determine the initial Cnb.
Navigation System Design
Eduardo Nebot
Slide 183
Calibration for Low Cost Inertial Units
The simplest method of obtaining the biases of the inertial sensors is to measure the readings from
each sensor whilst the vehicle is stationary. These bias values are used to calibrate the IMU.
For gyros, the bias is simply the reading from these sensors when the vehicle is stationary.
However, the alignment of the inertial unit is required in order to determine the biases on the
accelerometers. This is accomplished during the alignment stage since the “expected” acceleration
due to gravity can be determined and hence any anomalies in these values are attributed to bias.
Thus the bias on the x-accelerometer is obtained by;
bfx = fx − fxT
(186)
where fx is the measured acceleration and fxT is the expected acceleration obtained during the
alignment stage. The bias is obtained similarly for the remaining accelerometers. For more
information and experimental results the reader can see [3]
Navigation System Design
Eduardo Nebot
Slide 184
Position, Velocity and Attitude Algorithms
The Figure presents the tasks required to work with a full inertial measuring unit.
The bank, elevation and heading information is used to calibrate and align the unit while the vehicle
is stationary.
Once the initial transformation matrix Cbn(0) is obtained, it is updated at fast rates with the
gyroscope information to obtain Cbn(k).
This matrix is used to transform acceleration in the body coordinate frame to the navigation
coordinate frame. Then the single and double integration is performed to obtain inertial velocity and
position.
Bank
Elevation
Calibration
Algorithm
Cbn (0)
Heading
Acceleration in
Navigation frame
Inertial
Measurement
Unit
Gyros
(R,P,Y)
Ckbn ()
Integral
Integral
Acceleration
Ax,Ay,Az
Navigation System Design
Velocity x,y,z
Position x, y ,z
Acceleration
in body frame
Eduardo Nebot
Slide 185
Position, Velocity and Attitude Algorithms
Bank
Elevation
Calibration
Algorithm
Cbn (0)
Heading
Acceleration in
Navigation frame
Inertial
Measurement
Unit
Gyros
(R,P,Y)
Ckbn ()
Integral
Integral
Acceleration
Ax,Ay,Az
Navigation System Design
Velocity x,y,z
Position x, y ,z
Acceleration
in body frame
Eduardo Nebot
Slide 186
GPS/INS Integration
Navigation System Design
Eduardo Nebot
Slide 187
GPS/INS Integration
Inertial sensors have been used in numerous applications for the past 50 years. This technology
originally developed for military purposes has started to appear in industrial applications.
This has been possible due to the significant reduction in cost of inertial sensors. Low cost full six
degree of freedom inertial system are currently available for less than US 10,000.
Unfortunately this reduction of cost comes with a substantial reduction in quality. These units
without any aiding can only perform navigation for very short period of time.
The solution to this problem is aiding inertial systems with external information to maintain the
error within certain bonds.
The most common aiding sensor for outdoor application has been the GPS in all its forms (
Autonomous / Differential / RTK ). This section presents various navigation architectures that fuse
GPS, INS and modeling information in an optimal manner.
Navigation System Design
Eduardo Nebot
Slide 188
Navigation architectures for Aided Inertial Navigation Systems
The navigation architecture depends on the types of sensors and models employed. For aided inertial
navigation systems the inertial component can be:
• An Inertial Measurement Unit (IMU), which only provides the raw acceleration and rotation rate
data
• An Inertial Navigation System (INS) providing position, velocity and attitude information
The aiding source can be:
• A sensor providing raw sensor information
• A navigation system providing the position, velocity and/or attitude information
The principle states of interest which are estimated by the filter, and hence which governs the type of
model implemented, are the position, velocity and attitude of the vehicle, or the position, velocity
and attitude errors.
Navigation System Design
Eduardo Nebot
Slide 189
Navigation architectures for Aided Inertial Navigation Systems
The most natural implementation of an aided inertial navigation system is to drive a non-linear filter
with the raw acceleration and rotation rate data provided by the IMU, as shown in the Figure.
IMU
Acceleration and
Rotation Rates
Non-Linear Filter
External Aiding
Sensor
Position, Velocities and
Attitude Estimates
Observations
The implementation is known as a “direct” filter structure. The process model usually represents the
kinematic relationship of the vehicle and the states of interest. The state vector is propagated by the
model and inertial data. The aiding information can be obtained from a navigation system where
observations such as position and velocity are supplied to the system. The estimate would be in the
form of the vehicle states.
Navigation System Design
Eduardo Nebot
Slide 190
Navigation architectures for Aided Inertial Navigation Systems
Disadvantages of the Direct Filter Architecture:
• The prediction equations have to be evaluated at each sample of the inertial data. This requires
substantial processing due to the high sample rates of IMUs
• In general, high frequency maneuvers are usually filtered in the linear (or linearised) model. The
consequence of this omission is that the filter will unnecessarily attenuate the high frequency
information provided by the INS. With this approach the system may not be able to track fast
maneuvers.
IMU
Acceleration and
Rotation Rates
Non-Linear Filter
External Aiding
Sensor
Navigation System Design
Position, Velocities and
Attitude Estimates
Observations
Eduardo Nebot
Slide 191
Navigation architectures for Aided Inertial Navigation Systems
These problems are solved if an INS is employed so that a constant stream of vehicle state
information is available external to the filter.
To correct any errors, the filter estimates the errors in these states. The inertial information can still
be obtained even if no additional information is available. The Figure shows this implementation that
is known as the ‘‘indirect feedback” method.
Inertial
Navigation
System
Position, Velocity
and Attitude
+
-
Estimated Position,
Velocity and Attitudes
Estimated Errors of Position, Velocity
and Attitude
+
-
External Aiding
System
Navigation System Design
Observed
Errors
Linear Filter
Position,
Velocities and
Attitude
Estimates
Observations
Eduardo Nebot
Slide 192
Navigation architectures for Aided Inertial Navigation Systems
The observation which is delivered to the filter is the “observed error” of the inertial navigation
solution, that is, the difference between the inertial navigation solution and the navigation solution
provided by the aiding source.
In this implementation the inertial high frequency information is fed directly to the
output without attenuation while the Kalman Filter provides the low frequency correction to the
IMU.
Inertial
Navigation
System
Position, Velocity
and Attitude
+
-
Estimated Position,
Velocity and Attitudes
Estimated Errors of Position, Velocity
and Attitude
+
-
External Aiding
System
Navigation System Design
Observed
Errors
Linear Filter
Position,
Velocities and
Attitude
Estimates
Observations
Eduardo Nebot
Slide 193
Navigation architectures for Aided Inertial Navigation Systems
Since the observation is the observed error of the inertial navigation solution, and since the filter is
estimating the errors in the inertial navigation solution, then the process model has to be in the form
of an error model of the standard inertial navigation equations.
Thus the inertial navigation equations are linearised to form error equations. Since
the equations are linearised the filter implementation takes on a linear form. Although the prediction
stage is still implemented, it can run at the same rate as the sampling rate of the INS or at lesser
intervals.
Inertial
Navigation
System
Position, Velocity
and Attitude
+
-
Estimated Position,
Velocity and Attitudes
Estimated Errors of Position, Velocity
and Attitude
+
-
External Aiding
System
Navigation System Design
Observed
Errors
Linear Filter
Position,
Velocities and
Attitude
Estimates
Observations
Eduardo Nebot
Slide 194
Navigation architectures for Aided Inertial Navigation Systems
Disadvantage of the indirect feedback method The unbounded error growth of the INS
which causes an unbounded growth in the error observation delivered to the filter.
This poses a problem to the linear filter since only small errors are allowed due to the linearisation
process.
That is, large drift in the state values from the INS cause large observation errors being fed into the
filter and thus invalidating the assumptions held by the filter.
Inertial
Navigation
System
Position, Velocity
and Attitude
+
-
Estimated Position,
Velocity and Attitudes
Estimated Errors of Position, Velocity
and Attitude
+
-
External Aiding
System
Navigation System Design
Observed
Errors
Linear Filter
Position,
Velocities and
Attitude
Estimates
Observations
Eduardo Nebot
Slide 195
Navigation architectures for Aided Inertial Navigation Systems
The optimal implementation is illustrated in the Figure and is known as the “direct feedback”
method.
In this structure the estimated errors are fed back to the INS, thus minimising the growth of the
observed error that is delivered as an observation to the filter.
Estimated Errors of Position,
Velocity and Attitude
Inertial
Navigation
System
Position, Velocity
and Attitude
+
-
External Aiding
System
Navigation System Design
Observed
Error
Linear Filter
Position,
Velocities and
Attitude
Estimates
Observations
Eduardo Nebot
Slide 196
Linear, Direct Feedback Implementation
This navigation architecture is driven with observation errors formed with the predicted and observed
position and/or velocities.
This filter requires a model of the propagation of the errors. The model that described the inertial
error equations in the Earth frame is:
δ ṗ = δv
δ v̇ = [f˜e×]δψ + C ebδf b
(187)
(188)
˙
[δψ×]
= −C ebδω bib.
(189)
Ceb transforms vectors from the body frame to the Earth frame. As in the GNSS implementation, the
Earth frame is represented by North (N), East (E) and Down (D) vectors and hence for clarification
the transformation matrix is represented as Cnb.
Navigation System Design
Eduardo Nebot
Slide 197
Linear Direct Feedback Implementation
The navigation loop implements a linear Kalman filter. The state vector consists of the error states,
h
iT
x = δpN δpE δpD δvN δvE δvD δψN δψE δψD .
(190)
The process model is


0


0


0


0


F = 0


0


0


0

0
Navigation System Design
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 0
0
−fD
0 0 0 0 0 fD
0
0 0 0 0 0 −fE
fN
0 0 0 0 0
0
0
0 0 0 0 0
0
0
0 0 0 0 0
0
0
Eduardo Nebot
0


0 


0 


fE 


−fN 


0 


0 


0 

0
(191)
Slide 198
The system can be written in compact form:
  
 

ṙ
0 I 0
r
I 0 0 0
  
 
 3∗3
  
 

 v̇  =  0 0 an   v  +  0 Cbn 0
  
 

φ̇
0 0 0
φ
0
0 Cbn


0




  fa 


ωibb
ẋ = Fg x + Gw
(192)
This model can be extended to consider the biases and drift of the accelerometers and gyros.
A gyro error model presented here consists of a first order Markov process with correlation time τ
plus white noise ν:
θ̇ = −(1/τ )θ + νg
E[νg ] = 0,
E[νg νgT ] = Rg
(193)
The matrix Tg that takes into account the time constant for the three gyros is


−1/τx
0
0




Tg =  0
−1/τy
0 


0
0
−1/τz
Navigation System Design
Eduardo Nebot
(194)
Slide 199
Accelerometer Model
A standard error model for the accelerometers consists of a random constant component plus white
noise:
a = νa
E[νa] = 0,
E[νaνaT ] = Ra
(195)
The matrix Ta that models the accelerometer errors is:


0 0 0




Ta =  0 0 0 


0 0 0
Navigation System Design
Eduardo Nebot
(196)
Slide 200
Extended Model
Finally the augmented model that considers the new states has the following extended matrixes :


0


 Fg
Cbn


F =
0


 0 0 0 Ta

0 0 0 0
0


0 


Cbn 


0 

Tg
h
x=

0


0


G=0


0

0

0
0 0 0


Cbn 0 0 0 


0 Cbn 0 0 


0 0 I 0

0 0 0 I
(197)
i
ri vi Φi fgi ωibb
Where r,v and ψ are the errors in position, velocity and angle misalignments, and fgi and ωibb are the
errors in the accelerometers and gyros in the body frame.
Navigation System Design
Eduardo Nebot
Slide 201
Discrete Model
In this implementation the acceleration inputs are fed directly into the process model and hence
there is no control vector, u = 0.
The process model F comprises of time-varying terms but they can be consider constant over the
sampling interval.
3
2
F
Navigation System Design
=
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
0
0
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
−fD
fE
0
0
0
0
0
0
fD
0
−fN
0
0
0
0
0
0
−fE
fN
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
Eduardo Nebot
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
(198)
Slide 202
Discrete Model
The state transition matrix can be evaluated as F(k) = exp(F∆t), where ∆t is the sampling time of
the inertial unit.
In the case of land vehicles, the dynamics are of a much lower frequency than the sampling frequency.
Hence over the sampling period F remains constant, hence
F(k) = exp(F∆t)
(F∆t)2
= I + F∆t +
+ ...
2!
The discretisation is only taken to the second term since any extra terms which are added to the final
state transition matrix are of negligible value.
Thus the state prediction is
x̂(k|k − 1) = F(k)x̂(k − 1|k − 1).
(199)
The advantage of using this model is that the implementation is linear and the model is independent
of the vehicle dynamics.
Navigation System Design
Eduardo Nebot
Slide 203
Filter Equations
Initially the inertial sensors are calibrated and all the errors are removed, thus x(1|0) is set to zero.
Thus the state prediction in the next cycle is also zero, and so forth. Therefore the state
prediction is always zero and no correction of the inertial errors occurs during the
prediction cycle.
The position, velocity and attitude information obtained from the navigation system is simply
the data from the INS since there are no predicted errors to correct it.
However, due to the drift in the INS solution, there is a corresponding growth in uncertainty in the
states and this is evaluated through the predicted covariance matrix is
P(k | k − 1) = F(k)P(k − 1 | k − 1)FT (k) + Q(k)
(200)
This is a 9 × 9 matrix representing the uncertainty in the inertial predicted errors.
Navigation System Design
Eduardo Nebot
Slide 204
Q(k) is the discrete process noise matrix also of dimension 9 × 9 and is evaluated using [2]
¤
1£
T
T
T
Q(k) =
F(k)G(k)Qc(k)G(k) F(k) + G(k)Qc(k)G(k) ∆t.
(201)
2
where Qc(k), which is the uncertainty in the process model over a period of time, is defined as


δ p̃ 0
0





Qc(k) =  0 δfb 0 
(202)
.


b
0 0 δωib
δ p̃ is the noise injected into the position error evaluation and its value is purely dependent on the
uncertainty in the evaluation of the position from the integration of velocity. The remaining error
terms in this matrix are the noise values on the sensor readings, that is, the errors in the body frame,
which can be obtained from manufacturer specifications or through experimentation. G(k) is


I
0
0
 3×3



n

.
(203)
G(k) =  0 Cb (k)
0



0
0
−Cnb(k)
Navigation System Design
Eduardo Nebot
Slide 205
Aiding in Direct Feedback Configurations
In a direct feedback structure, the model implemented in the filter is a linear error model representing
the errors in the vehicle states, generally position, velocity and attitude.
When an observation becomes available, the filter estimates the errors in these states.
Since the model is an error model of the inertial equations, the observation z(k) is the observed error
of the inertial navigation solution and not the observation delivered by the aiding sensor.
Thus if an aiding system (i.e GPS ) provides position and velocity data then the observation vector
becomes,

z(k) = 
Navigation System Design

zP (k)
zV (k)

=

Pinertial (k) − PGP S (k)
V
inertial
Eduardo Nebot
(k) − V
GP S

(204)
(k)
Slide 206
The Figure illustrates the observation structure [2].
The true acceleration, velocity and position of the vehicle have noise added to them to represent
measurements taken by the sensors.
The acceleration, as measured by the inertial navigation system, is integrated twice to obtain the
indicated velocity and position of the vehicle. The acceleration information is obtained by the
accelerometers and it is assumed that acceleration due to gravity has been compensated for.
wa
at
Inertial Unit
+
+
xi
vi
wv
vt
_
+
+
+
z2
vgps
wx
xt
_
+
_
xgps
_
z1
Navigation System Design
Eduardo Nebot
Slide 207
By defining the terms δP(k) and δV(k) as the position and velocity errors in the inertial data after
the integration process, the observation model becomes


inertial
GP S
P
(k) − P
(k)

z(k) = 
inertial
GP S
V
(k) − V
(k)

 
 

(PT (k) + δP(k)) − (PT (k) − vP (k))
δP(k)
v (k)
=
+ P

= 
(VT (k) + δV(k)) − (VT (k) − vV (k))
δV(k)
vV(k)
(205)
wa
at
Inertial Unit
+
+
xi
vi
wv
vt
_
+
+
+
z2
vgps
wx
xt
_
+
_
xgps
_
z1
Navigation System Design
Eduardo Nebot
Slide 208
Observation Model

z(k) = 

= 

P
inertial
(k) − P
GP S
(k)

Vinertial (k) − VGP S (k)
(PT (k) + δP(k)) − (PT (k) − vP (k))
(VT (k) + δV(k)) − (VT (k) − vV (k))


=

δP(k)
δV(k)

+

vP(k)

vV(k)
The observation is thus the error between the inertial indicated position and velocity and that of the
aiding sensor and the uncertainty in this observation is reflected by the noise of the aiding observation.
This offers another benefit in the direct feedback implementation and involves the tuning
implementation; the noise on the observation is the noise on the aiding sensor.
Thus once an inertial unit and process model is fixed then the process noise matrix Q(k) is also fixed,
and tuning the filter is solely based on the observation noise matrix R(k).
Navigation System Design
Eduardo Nebot
Slide 209
Filter Equations
The estimate of the error states at time k given all observations up to time k can then evaluated
using the standard kalman filter update equations:
x̂(k|k) = x̂(k|k − 1) + W(k)ν(k),
(206)
where W(k) is a gain matrix produced by the Kalman filter and ν(k) is the innovation vector.
ν(k) = z(k) − H(k)x̂(k|k − 1).
(207)
W(k) = P(k|k − 1)HT (k)S−1(k),
(208)
where S(k) is known as the innovation covariance and is obtained by
S(k) = H(k)P(k|k − 1)HT (k) + R(k).
(209)
Finally the covariance matrix is updated due to this observation:
P(k|k) = (I − W(k)H(k))P(k|k − 1)(I − W(k)H(k))T +
W(k)R(k)WT (k)
Navigation System Design
Eduardo Nebot
(210)
Slide 210
Filter Equations
Since the prediction of the error states x̂(k|k − 1) is always zero then the state estimate becomes
x̂(k|k) = W(k)z(k)
(211)
That is, the update is simply a weighted sum of the observations.
The updated position and velocity errors can now be used to correct the position and velocity of the
INS
Pinertial (k|k) = Pinertial (k|k − 1) − δp(k|k)
(212)
Vinertial (k|k) = Vinertial (k|k − 1) − δv(k|k)
(213)
Navigation System Design
Eduardo Nebot
Slide 211
Filter Equations
Finally the transformation matrix Cnb can also be updated with
Cnb(k|k) = [I3×3 + [δψ×]]Cnb(k|k − 1)
where

(214)

0
−δψD δψE




[δψ×] =  δψD
0
−δψN 


−δψE δψN
0
(215)
[δψ×] is in the navigation frame yet it is used to correct the Cnb matrix whose elements are defined in
the body frame. It is assumed that the misalignment errors are small and hence the errors associated
about the navigation frame are equal to those about the body frame. When large misalignments are
encountered, the linear assumptions held are not valid. [1] deals with such situations.
Navigation System Design
Eduardo Nebot
Slide 212
Real Time Implementation Issues
Navigation System Design
Eduardo Nebot
Slide 213
Real Time Implementation Issues
Filter Consistency and Fault Detection: Few methods to estimate the state of interest were
presented. In real time applications it is usually not possible to know the true value of the
state. This implies that here is no way of determining whether the filter is computing correct
estimates.
The only information available from the real world is the observation, and hence the only
form of measure for determining the behaviour of the filter is the difference between the observation
and the predicted observation, that is, the innovation equation.
ν(k) = z(k) − H(k)x(k)
The innovation has the property that it must be both unbiased and white, and have covariance S(k)
if the filter is operating correctly. To determine whether this is the case, the innovation is normalised,
γ = ν T (k)S−1(k)ν(k).
Navigation System Design
Eduardo Nebot
(216)
Slide 214
Real Time Implementation Issues
Filter Consistency and Fault Detection ...
If the filter assumptions are correct then the samples of γ are distributed as a χ2 distribution in m
degrees of freedom (the number of observations being estimated). Instead of using Equation 217 as a
method of determining filter consistency, it can be used as a gating function.
γ = ν T (k)S−1(k)ν(k).
(217)
When an observation is obtained, Equation 217 is formed, and if the value γ is less than a predefined
threshold, then the observation is accepted. This allows for a means of detecting any faults within the
observation.
The threshold value is obtained from standard χ2 tables and is chosen based on the confidence level
required. Thus for example, a 95% confidence level, and for a state vector which includes three states
of position and three of velocity, then γ = 12.6.
Navigation System Design
Eduardo Nebot
Slide 215
Real Time Implementation Issues
High Frequency faults: These faults arise when the GNSS signals undergo multipath errors.
This results in a longer time delay of the signals affecting the fix of the Standard Differential receiver,
and also altering the phase of the signal thus affecting the Carrier Phase Differential fix.
Another high frequency fault is when the receiver utilises a different set of satellites in order to
determine the position fix. The accuracy of the fix is dependent on the geometry of the observed
satellites. Changes in satellite configuration due to blockages of the satellite view will in turn alter
the resulting fix.
Both forms of high frequency faults cause abrupt jumps in the position and velocity fixes
obtained by the GNSS receiver.
High frequency faults are therefore environment dependent. An open area such as a quarry will be
less likely to produce multipath errors than will a container terminal for example.
Consequently, the tuning of the filter which fuses the inertial unit and GNSS data is dependent on
the environment.
Navigation System Design
Eduardo Nebot
Slide 216
High Frequency Faults
The most common method of rejecting multipath errors is obtained when the receiver can
distinguish between the true signal and the reflected signal.
How well the receiver can distinguish between these two signals is dependent on the accuracy of the
receiver’s internal timing procedures etc. There is also special electronic hardware and antenna
designs to address multipath.
However, we cannot guarantee that the system will be able to reject multipath errors
completely, and even with the constant improvement of GNSS technology high frequency faults
such as multipath always need to be factored into the development of the navigation system.
Equation γ = ν T (k)S−1(k)ν(k) can be used to maintain filter consistency by evaluating a gating
function which has the property of being a χ2 distribution. Thus it can potentially determine if the
multipath errors have occurred.
Navigation System Design
Eduardo Nebot
Slide 217
High Frequency Faults
The GNSS fix in the vertical plane is significantly less accurate than that in the horizontal
plane. (This is due to the GPS Design specification )
Consequently the fix in North and East may lie well within the validation region, whilst that of Down
may exceed it and force the result of the gating function beyond the threshold if the same noise
values were used for all the terms in the observation noise matrix R(k). However, if these noise
terms are accounted for by taking the values from the GNSS receiver directly, than
the validation gate will detect multipath errors correctly.
Changes in satellite geometry cause the Dilution of Precision (DOP) to vary. These changes
occur when part of the sky is invisible to the receiver antenna due to obstacles blocking the GNSS
signals. Consequently, a change in DOP will affect the GNSS solution causing high frequency faults.
These faults can be detected using the same technique as discussed for multipath errors. However,
these changes are not as large as those encountered for multipath errors and generally go undetected,
unless the accuracy of the inertial unit is comparable to that of the GNSS receiver.
Navigation System Design
Eduardo Nebot
Slide 218
Filter Tuning
Navigation System Design
Eduardo Nebot
Slide 219
Filter Tuning
There are two stages in the filter flow:
• The prediction stage where the predicted inertial errors are always zero and the uncertainty grows
with time
• The estimation stage where the estimates of the inertial errors are obtained by a weighted sum of
the observations and the uncertainty is bounded
If no observation is obtained for an extended period of time, or if GNSS fixes are rejected due to
errors, the filter will cycle in prediction mode and no corrections to the navigation solution will be
made. ( The uncertain will keep growing !).
GPS
correction
Inertia
When a fix does occur, the observation may pass the gating function test even though the fix may be
in error since the uncertainty is the inertial navigation solution is large.
Navigation System Design
Eduardo Nebot
Slide 220
Filter Tuning
As with any Kalman filter implementation, tuning lies in the choice of values for the process Q(k)
and observation R(k) covariance matrices.
A large Q(k) will imply an inaccurate inertial system. During the prediction stage the uncertainty in
the inertial data will grow according to the magnitude of Q(k). When a GNSS fix does occur
there is a greater possibility the inertial unit will be corrected using the first available GNSS
fix, irrespective of the accuracy of this fix.
A small values in R(k) will imply accurate GNSS fixes which may pose a problem when the fix is in
error and is being fused with low accuracy inertial sensors.
Thus tuning becomes a delicate adjustment of both the Q(k) and R(k) matrices along with the
employment of the gating function in order to reject the high frequency faults of the GNSS.
Navigation System Design
Eduardo Nebot
Slide 221
Filter Tuning
The variances along the diagonal of R(k) are determined simply by obtaining the GDOP values from
the receiver and assuming there is no correlation between the fixes of the navigation axes (which is
how the GDOP is provided). This is obtained with the position fix
Determining the values for Q(k) depends on the noise level of the sensors implemented which can be
obtained from the manufacturer or through experimentation.
The principle tuning parameters which need to be addressed are the variances of velocity and
attitude. A large variance in the velocity terms will imply greater reliance on the GNSS velocity
fixes. Furthermore, the greater the accuracy of the velocity estimates, the greater the dampening on
the attitude errors.
If there are no attitude fixes then only the velocity information can contain the drift in attitude and
how quickly and accurately this can happen depends on the variance on the attitude.
Navigation System Design
Eduardo Nebot
Slide 222
Real Time Algorithm Issues
There are two points of concern in a real time implementation of an inertially aided GNSS navigation
system
• Processing power
• Data Latency
Data latency is extremely important, especially with the use of GNSS sensors, where it is common to
find position latencies in the order of tens of milliseconds and that of velocity reaching over a second.
Standard pseudorange solutions require the least computational processing. RTK technology requires
the most processing power. In addition if attitude information is required this also requires additional
processing.
When the latency of the solution is low the position information can be propagated using
velocity data and a constant velocity model. This is quite sufficient for low speed vehicles.
The problem is when latency of the GNNS is high, and the vehicle dynamics are fast. A GNSS
solution with a latency of 50ms will have an error of 0.8m for a vehicle traveling at 60km/hr.
Navigation System Design
Eduardo Nebot
Slide 223
Real Time Algorithm Issues
If the vehicle is moving with constant velocity and moving in a straight line then the position can be
simply propagated forward. However, any deviation from a straight line will lead to incorrect
estimates when the GNSS fix is fused with the current inertial state.
What is required is to store the inertial data and process the estimates at the time that the GNSS fix
should have occurred, and then propagate the inertial solution through the backlog of data.
Update
Pred 4
Pred5
Observation
Prediction 1
Navigation System Design
Prediction 2
Eduardo Nebot
Prediction 3
Slide 224
Real Time Algorithm Issues
Another problem is that in some cases position and velocity GNSS data do not have equal latencies.
This is the case when GNSS receiver provide velocity and position with independent measurements
then this requires alternative methods ( Uncorrelated ! )
For example, in RTK systems a doppler approach is used to obtain velocity while the determination
of the phase ambiguity is used for position measurement. In such systems the latency of the velocity
data can sometimes be over 1s, and hence the position and velocity determination occurs at different
points in time.
Some GNSS systems return position with time stamp to the proper position but the velocity
evaluated after integrating the doppler signal for a period of time. Obviously this velocity does not
correspond to the same time stamp.
Other GNSS manufacturers propagate the velocity through the position data and hence both the
position and velocity output occur at the same time with the same latencies. This approach produces
correlation between the position and velocity data, which is not ideal for navigation systems.
Navigation System Design
Eduardo Nebot
Slide 225
Hardware Issues
Land navigation usually does not require extremely high sampling of inertial measurement sensors
since the maximum rotation and acceleration frequencies are not high (with obvious consideration to
vibration).
The navigation functions required for the aided INS strapdown system are usually partitioned
between the fast guidance unit and the navigation processor, as shown in the Figure
INS
Platform
DGPS
Receiver
Gyro and
Accelerations
at 200Hz
Position and
Velocity at 10 Hz
Accelerations, 10 Hz
Fast Guidance
Unit
(Prediction)
Transformation Matrix, 10 Hz
Navigation
Unit
*Kalman
Filter
(Estimation)
Bias and Misalignment, Position
and Velocity Corrections
Position and Velocity
200Hz
Navigation System Design
Eduardo Nebot
Position and Velocity
10 Hz
Slide 226
Navigation Algorithm Implementation
1. Whilst the vehicle is stationary read in all acceleration, rotation rate and tilt sensor data from the
inertial unit and provide the average of these sensor readings.
2. Once the averaging process has been completed, perform calibration of the inertial unit to
determine biases and to obtain the initial Cnb matrix. At this stage the GPS position fix is used to
determine the initial Cng matrix.
3. The navigation system then proceeds onto the inertial navigation system with the initial position
as determined by the GPS receiver and the initial attitude as obtained from the alignment stage.
4. As the vehicle moves the acceleration and gyro values are read in and the biases removed,
Equation Cnb is updated, and the acceleration in the navigation frame is computed. These values
are then integrated to provide the position and heading of the vehicle.
5. If no GPS fix is available then return to Step 4 ( Continue in Prediction )
6. Use the gating function to determine if there are any high frequency faults. If high frequency faults
exist then only send out the current state values as determined by the inertial system and return
to Step 4. If the validation check has passed then fuse the GPS data.
Navigation System Design
Eduardo Nebot
Slide 227
Aiding Using Constraint: Vehicle Model
Navigation System Design
Eduardo Nebot
Slide 228
Aiding Using a Vehicle Model
In this section we present a new type of aiding information that does not come from external sensors.
This alternative method is based on the application of constraints on the inertial equations if the
motion of the vehicle is itself constrained in some way.
The method is presented for a land vehicle application but it is applicable to any system where the
presence of constrains can be used to bound the error growth of inertial units. The filter structure
illustrated in the Figure.
Vehicle
Constraints
IMU
Velocity
Observations
Acceleration and
Rotation Rates
Non-Linear
Kalman Filter
Position, Velocity and
Attitude Estimates
Navigation System Design
Eduardo Nebot
Slide 229
Aiding Using a Vehicle Model: Basic Idea
The general three dimensional inertial equations are derived from Newton’s laws and are thus
applicable to all forms of motion.
The inertial equations on board a train apply equally as well to an aerobatic aircraft. However, the
motion of the train is clearly different to that of the aircraft, in particular, the train is
constrained to move along the rails.
This fact can be used as a “virtual observation”, constraining the error growth along the lateral and
vertical directions.
The motion of a general land vehicle (under normal road conditions ) can also use constrained motion
equations. If the vehicle undergoes excessive slip or movement in the vertical direction off the ground,
then extra modelling is required, however, the use of constraints as virtual observations is still valid.
Navigation System Design
Eduardo Nebot
Slide 230
General Three-Dimensional Motion
Assume the vehicle motion can be described by the state equation,
ẋ = f(x,u)
(218)
¤T
£
¤T
£
where the vehicle state vector x = VTn , PTn , γ, β, θ , and the measurements u= fTb , ω Tb . Using
the kinematic relationship between ω b and the rates of changes of the Euler angles, and assuming that
the rate of rotation of the earth is negligible, the state equations for vehicle motion can be written as
V̇ n = Cnbfb
(219)
Ṗ n = Vn
(220)
ωy sin θ + ωz cos θ
cos β
β̇ = ωy cos θ − ωz sin θ
γ̇ =
(221)
(222)
θ̇ = ωx + (ωy sin θ + ωz cos θ) tan β
(223)
These equations enable the computation of the state x given x(0) and measurements fb and ω b. The
rate of error growth can be reduced if the velocity of the vehicle and the Euler angles β and θ can be
estimated directly. It will be shown that this is indeed possible for a vehicle moving on a surface.
Navigation System Design
Eduardo Nebot
Slide 231
Motion of a Vehicle on a Surface
Motion of a wheeled vehicle on a surface is governed by two non-holonomic constraints.
When the vehicle doesn’t jump off the ground and does not slide on the ground, the velocity of the
vehicle in the plane perpendicular to the forward direction (x-axis) is zero. Under ideal conditions,
there is no side slip along the direction of the rear axle (y-axis) and no motion normal to the road
surface (z-axis), so the constraints are
Vy = 0
(224)
Vz = 0
(225)
We know that these constrain are violated under slide slip during cornering and vibrations caused by
the engine and suspension system. Additional models can be included to address these issues. As a
first approximation however, it is possible to model the constraint violations as follows:
Vy − ηy = 0
(226)
Vz − ηz = 0
(227)
where ηy and ηz are Gaussian, white noise sources with zero mean and variance σy2 and σz2. The
strength of the noise can be chosen to reflect the extent of the expected constraint violations.
Navigation System Design
Eduardo Nebot
Slide 232
Motion of a Vehicle on a Surface
We need to relate the velocities in the body frame Vb = [Vx, Vy , Vz ]T to Vn,
Vb = [Cnb]T Vn
it is possible to write constraint Equations
Vy − ηy = 0
(228)
Vz − ηz = 0
(229)
as a function of the vehicle state x and a noise vector w= [ηy , ηz ]T ,


 
η
V
 y  = M +  y ,
(230)
Vz
ηz


VN (sin θ sin β cos γ + cos θ sin γ) + VE (cos θ cos γ + sin θ sin β sin γ) + VD (sin θ cos β)


M=
VN (sin θ sin γ + cos θ sin β cos γ) + VE (cos θ sin β sin γ − sin θ cos γ) + VD (cos θ cos β)
The navigation frame implemented here is North (N), East (E) and Down (D). It is now required to
obtain the best estimate for the state vector x modeled by the state Equations 219-223 from a series
of measurements fb and ω b, subjected to the constraint Equation 230.
Navigation System Design
Eduardo Nebot
Slide 233
Estimation of the Vehicle State Subject to Constraints
The state equation, obtained by the discretisation of Equations 219-223, is
x̂(k|k − 1) = f(x̂(k − 1|k − 1), u(k)),
(231)
and the discrete time version of the constraint equation obtained from Equation 230
z(k) = H(x(k)) + v(k).
(232)
The estimation of the state vector x subjected to stochastic constraints can be done in the framework
of an extended Kalman filter.
The Equation 232 is treated as an observation where the “observation” at each time instant k
is in fact identical to zero. The Kalman filter algorithm proceeds recursively in three stages:
Prediction, Observation and Update.
Navigation System Design
Eduardo Nebot
Slide 234
Prediction: The state and covariances are predicted as follows
x̂(k|k − 1) = F(x̂(k − 1|k − 1), u(k)),
(233)
P(k|k − 1) = ∇Fx(k)P(k − 1|k − 1)∇FTx (k) + Q(k).
(234)
The term ∇Fx(k) is the Jacobian of the current non-linear state transition matrix F(k) with respect
to the predicted state x(k|k − 1).
Observation: Following the prediction, it is assumed that an observation z(k) that is identical to
zero is made. An innovation is then calculated as follows
ν(k) = z(k) − ẑ(k|k − 1)
(235)
where z(k) is in fact set to zero. An associated innovation covariance is computed using
S(k) = ∇Hx(k)P(k|k − 1)∇HTx (k) + R(k)
Navigation System Design
Eduardo Nebot
(236)
Slide 235
Update: The state estimate and corresponding state estimate covariance are then updated
according to
x̂(k|k) = x̂(k|k − 1) + W(k)ν(k)
(237)
where ν(k) is the nonlinear innovation,
ν(k) = z(k) − H(x̂(k|k − 1)).
(238)
The gain and innovation covariance matrices are
W(k) = P(k|k − 1)∇HTx (k)S−1(k)
(239)
where the term ∇Hx(k) is the Jacobian of the current observation model with respect to the
estimated state x(k|k).
The updated covariance matrix is
P(k|k) = (I − W(k)∇Hx(k))P(k|k − 1)(I − W(k)∇Hx(k))T +
W(k)R(k)W(k).
Navigation System Design
Eduardo Nebot
(240)
Slide 236
Observability of the States
Although the extended Kalman filter algorithm obtains estimates of the state x, not all of them are
observable. For example, the estimation of the vehicle position, Pn, requires direct integrations and
therefore is not observable. Intuitively, forward velocity is the direct integral of the
measured forward acceleration during motion along straight lines, therefore is not observable.
Clearly an analysis is required to establish the conditions for observability.
Equations to relate the vehicle motion to the measurements from the inertial unit:
V̇f − fx + g sin β = 0,
(241)
Vf ωz − fy − g sin θ cos β = 0,
(242)
Vf ωy + fz + g cos θ cos β = 0.
(243)
Conclusions from the above equations:
• when the forward acceleration is zero the roll (θ) and pitch (β) can be directly computed from the
inertial measurements
• if one of the angular velocities ωy or ωz is not zero, the forward velocity can also be computed
directly
Navigation System Design
Eduardo Nebot
Slide 237
Experimental results using Constraints
POSITION
200
NORTH (m)
150
<−−−Constrained data
<−−−IMU/GPS
100
50
<−−−−−
0
Raw data
−50
−300
Navigation System Design
−250
−200
−150
−100
EAST (m)
Eduardo Nebot
−50
0
50
Slide 238
Multiple Aiding of an Inertial System
This section will discuss the aiding of an inertial unit with three forms of external observations:
• position and velocity derived from GPS
• speed from a drive shaft encoder vehicle model constraints
In this case we are incorporating all the information available into the filter. Furthermore, the
constraint algorithms contain the growth of the attitude error when there are no GPS fixes, thus
sustaining the accuracy of the inertial unit.
The speed information provided by the encoder data makes the forward velocity observable in all
conditions
A linear information filter is used with the inertial error model developed as a process model. A
direct feedback structure is implemented making the algorithm very efficient. It also allows for easy
and efficient process of asynchronous sensory information.
Navigation System Design
Eduardo Nebot
Slide 239
Multiple Aiding of an Inertial System
Estimated Errors of Position,
Velocity and Attitude
Position, Velocity
and Attitude
INS
+
Observed Error
+
-
GNSS
Observed Error
Position and
Velocity
Observations
Velocity
Observations
Navigation System Design
Eduardo Nebot
Kalman
Filter
Vehicle
constraints +
wheel encoder
Slide 240
Implementation of the INS Algorithm using the Information Filter
Prediction: The prediction stage is implemented using:
ŷ(k|k − 1) = L(k|k − 1)ŷ(k − 1|k − 1) + B(k)u(k),
(244)
where L(k|k − 1) = Y(k − 1|k − 1)F(k)Y−1(k − 1|k − 1).
and
Y(k|k − 1) = [F(k)Y−1(k − 1|k − 1)FT (k) + Q(k)]−1.
(245)
Observation: When an observation from the aiding sensor or constraint is made, the observation
vector generated is the observed error of the inertial system,
z(k) = zinertial (k) − zaiding (k).
Navigation System Design
Eduardo Nebot
Slide 241
Implementation of the INS Algorithm using the Information Filter
At each sampling of the inertial unit, a constraint observation is made, that is, Vy = 0 and Vz = 0.
The speed of the vehicle along the body x-axis which is obtained from the speed encoder Vx. This
observation, which is now in the body frame, is converted to the North (N), East (E), Down (D)
frame using Cnb. That is,


V (k)
 x


velocity
n
zV
(k) = Cb  Vy (k) 


Vz (k)

V (k) cos β cos γ
 x

=  Vx(k) cos β sin γ

−Vx(k) sin β
(246)



.

(247)
Thus the observation vector is
velocity
z(k) = zinertial
(k)
−
z
(k).
V
V
Navigation System Design
Eduardo Nebot
Slide 242
Implementation of the INS Algorithm using the Information Filter
The observation model is given by
Hvelocity
V
³
=
and the observation covariance matrix is
´
(248)
03×3 I3×3 03×3


σx2 0
Rvelocity
V
0




2
=  0 σy 0  .


0 0 σz2
(249)
Since the velocity vector is transformed from the body frame to the navigation frame, the observation
noise covariance needs to be transformed as well and is done so by
R̄
Navigation System Design
velocity
= CnbRvelocity CnbT
Eduardo Nebot
(250)
Slide 243
When the position and velocity are obtained from the GNSS receiver the observation vector is


inertial
GN SS
zP
(k) − zP
(k)

z(k) = 
(251)
inertial
GN SS
zV
(k) − zV
(k)
The observation model is

H
GN SS
and the observation noise matrix is
(k) = 

I3×3 03×3 03×3
03×3 I3×3 03×3
,
(252)








GN SS
R
(k) = 






σP2 N
0
0
0
0
0
0
σP2 E
0
0
0
0
0
0
σP2 D
0
0
0
0
0
0
0
0
σV2 N
0
0
0
0
σV2 E
0
0
0
0
0
0
σV2 D













(253)
where the individual variances are obtained from the GDOP values.
Navigation System Design
Eduardo Nebot
Slide 244
Update: Once the observations are formed, the information state vector is generated
i(k) = HT (k)R−1(k)z(k).
(254)
The amount of certainty associated with this observation is in the form of the information
observation matrix,
I(k) = HT (k)R−1(k)H(k)
(255)
The estimate then proceeds according to
ŷ(k|k) = ŷ(k|k − 1) + i(k)
(256)
Y(k|k) = Y(k|k − 1) + I(k).
(257)
and if there is more than one sensor at this time stamp:
ŷ(k|k) = ŷ(k|k − 1) +
X
ij (k)
X
Y(k|k) = Y(k|k − 1) +
Ij (k).
(258)
(259)
where j is the number of sensors providing information at time k.
Navigation System Design
Eduardo Nebot
Slide 245
Experimental Results using Multiple Information: Vehicle path
POSITION
300
250
NORTH (m)
200
150
100
50
0
−50
−100
Navigation System Design
0
100
200
300
EAST (m)
Eduardo Nebot
400
500
600
700
Slide 246
Experimental Results using Multiple Information: Velocity error
Velocity Error
1.4
North Error (m/s)
1.2
1
0.8
0.6
0.4
| Constrained Inertial
0.2
0
| Constrained + GPS
0
20
40
60
80
100
Time (s)
120
140
160
180
200
East Error (m/s)
2
1.5
1
0.5
| Constrained Inertial
0
0
20
Navigation System Design
40
60
80
100
Time (s)
120
Eduardo Nebot
140
160
| Constrained + GPS
180
200
Slide 247
Experimental Results using Multiple Information: Position error
Position Error
6
North Error (m)
5
| Constrained Inertial
4
3
2
| Constrained + GPS
1
0
0
20
40
60
80
100
Time (s)
120
140
160
180
200
35
East Error (m)
30
25
20
| Constrained Inertial
15
10
| Constrained + GPS
5
0
0
20
Navigation System Design
40
60
80
100
Time (s)
120
Eduardo Nebot
140
160
180
200
Slide 248
Tightly Coupled GPS/Ins
The methods discussed so far are also known as “loosely coupled” implementations since there is no
feedback to the aiding sensor/navigation system.
If feedback is provided to the aiding source a tighter configuration is obtained which in turn improves
system integrity. The Figure illustrates what is known as a “tightly coupled” configuration.
IMU
Acceleration and
Rotation Rates
Non-Linear Filter
External Aiding
Sensor
Position, Velocities and
Attitude Estimates
Raw Observations
Position, Velocity and
Attitude Estimates
Navigation System Design
Eduardo Nebot
Slide 249
Tightly Coupled GPS/INS: Advantages / Disadvantages
The loosely coupled configurations are highly modular in accuracy and cost. The system’s
designer can implement the model of choice along with the desired INS in whatever navigation
structure is preferred. Any aiding sensor can then be added to the navigation system. For
example one GPS range to a satellite can be incorporated.
Although the tightly coupled configuration is more robust than the loosely coupled configuration, it is
more expensive to implement and more difficult to develop.
Furthermore, if a different aiding sensor is employed, the models and algorithms must change
substantially.
Navigation System Design
Eduardo Nebot
Slide 250
Tightly Coupled GPS/INS: Advantages / Disadvantages
The majority of implementations have been loosely coupled. This is due to companies specialising in
inertial systems developing INS units with built in filtering techniques in a loosely coupled
configuration and in turn, GNSS companies focusing on delivering state of the art GNSS navigation
systems.
To implement a tightly coupled configuration requires close collaboration with GNSS companies,
since the aiding information from the navigation filter which is fed back to the GNSS sensor assists
with the satellite tracking algorithms.
These algorithms are kept secret since the speed of satellite reacquisition, the ability to locate and
track satellites after they have been lost, is what separates the quality of receivers between different
manufactures, and hence is a strong marketing tool.
Navigation System Design
Eduardo Nebot
Slide 251
REFERENCES
REFERENCES
References
[1] X. Kong. Development of a Non-Linear Psi-Angle Model for Large Misalignment Errors and its Application in INS Alignment and Calibration. In IEEE
International Conference on Robotics and Automation, pages 1430–1435, May 1999.
[2] P. Maybeck. Stochastic Models, Estimation and Control, volume 1. Academic Press, 1982.
[3] Durrant-Whyte H Nebot E. Initial Alignment and Calibration of Low Cost Inertial Navigation Units for Land Vehicle Applications. In Journal of Robotic
System, volume 16(2), pages 81–92, February 1999.
[4] Sukkarieh S. Aided Inertial Navigation Systems for Autonomous Land Vehicles. PhD thesis, Department of Mechanical and Mechatronic Engineering,
University of Sydney, 2000.
Navigation System Design
Eduardo Nebot
Slide 251