Chapter 11 Sensor and Navigation Systems

11.1 Low-Pass and Notch Filtering

11.2 Fixed Gain Observer Design
11.3 Kalman Filter Design
11.4 Nonlinear Passive Observer Design
11.5 Integration Filters for IMU and Global Navigation Satellite Systems

Navigation: is the science of directing a craft by determining its

position, course, and distance traveled
In some cases velocity and acceleration are determined as well. This is usually
done by using a satellite navigation system combined with motion sensors such
as accelerometers and gyros. The most advanced navigation system for marine
applications is the inertial navigation system (INS).

Navigation is derived from the Latin navis, ship,

and agere, to drive

It originally denoted the art of ship driving, including

steering and setting the sails.
1
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Chapter 11 Sensor and Navigation Systems
Wave filtering is one of the most important issues to take into account when designing
ship control systems.
It is important that only the slowly varying disturbances are counteracted by the
steering and propulsion systems; the oscillatory wave-frequency (WF) motion due to
the waves (1st-order wave-induced forces) should be prevented from entering the
feedback loop (Balchen 1976).
Definition 11.1 (Wave Filtering) Wave filtering can be defined as the reconstruction of the
low-frequency (LF) motion components from noisy measurements of position, heading and
in some cases velocity and acceleration by means of a state observer or a filter.

Remark: If a state observer is applied,

estimates of the WF motion components
can also be computed.

2
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1 Low-Pass and Notch Filtering
For wave periods in the interval 5s < T < 20s, the dominating wave frequency (modal
frequency) f of a wave spectrum will be in the range:

0. 05 Hz ! f 0 ! 0. 2 Hz # S(w)

The circular frequency = 2f

corresponding to periods T > 5s is: Swell
and tidal Developing
waves sea
! 0 ! 1. 3 rad/s #

w

Waves can be accurately described by 1st- and 2nd-order linear wave theory:

1st-order wave-induced forces (WF forces) produce large oscillations about a mean wave
force. WF forces are represented as a wave spectrum.
Compensated for by using wave filtering in the state estimator

2nd-order wave-induced forces or mean wave (drift) forces are slowly varying forces.
Compensated for by using integral action in the control law
3
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1 Low-Pass and Notch Filtering
A feedback control system will For a large oil tanker, the crossover frequency c can
typically move the bandwidth be as low as a 0.01 rad/s, while smaller vessels like
b of the vessel up to 0.1 rad/s cargo ships and the Mariner class vessel, are close to
which still is below the wave 0.05 rad/s.
spectrum.
The wave disturbances will
typically be inside the
bandwidth of the servos and
actuators of the vessel. Hence,
the wave disturbances must be
filtered out before feedback is
applied in order to avoid
unnecessary control action. !c !0

LF vessel motion
WF motion

4
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1 Low-Pass and Notch Filtering
For a ship moving at forward speed U > 0, there will be a shift in the wave spectrum
peak frequency 0.

The shifted frequency is referred to as the frequency of encounter e and it depends on

ship speed U, modal wave frequency and wave direction .

! 20
!e !U, ! 0 , "" ! ! 0 ! g U cos "

5
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1.1 Low-Pass Filtering
For sea states where the encounter frequency e is much higher than the bandwidth b of
the control system,
!b ! !e

a LP-filter can be used to filter out the 1st-order wave-induced forces. This is typically the
case for large vessels such as oil tankers.
For smaller vessels a LP filter in cascade with a notch filter is quite common to use.

LP and notch filters in series with the control system.

6
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1.1 Low-Pass Filtering
Autopilot measurement equation:
y!s" ! hship !s"!!s" " h wave!s"w!s" Total motion, LF + WF
"!s" "w !s"
where LF motion

y(s) is the compass measurement

w(s) is zero-mean Gaussian white noise WF motion

!!s"is the rudder input. 0

!!s"is the LF motion

0 50 100 150
! w !s" is the WF motion time

Linear theory:

!!s"
Consequently, the feedback control law should be a function of and not y(s) in
!!s"
order to avoid 1st-order wave-induced rudder motions.

Kw s K!1"T 3s"
h wave !s" ! s 2"2!" 0s"" 20 h ship !s" ! s!1"T 1s"!1"T 2s"

7
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1.1 Low-Pass Filtering
A first-order low-pass filter with time constant Tf can be designed according to:
1 1
hlp !s" ! 1"T fs !b # Tf # !e (rad/s"
This filter will suppress disturbances over the frequency 1/Tf.
This criterion is hard to satisfy for smaller vessels.

Higher-order low-pass filters can be designed by using a Butterworth filter:

hlp !s" ! 1
!n ! 1"h lp !s" ! 1
p!s" 1 " s/! f
where p(s) is found by !2f
!n ! 2"h lp !s" ! 2 ; " ! sin!45 o "
solving the Butterworth s " 2"!f s " ! 2f
polynomial: !2f
!n ! 3"h lp !s" ! 2 # 1 ; " ! sin!30 o "
2n
s " 2"!f s " ! f 1 " s/!f
2

p!s"p!!s" ! 1 " !s/j!f " 2

!2f
!n ! 4"h lp !s" ! ! s 2 " 2" i ! f s " !2f
; " 1 ! sin!22. 5 o ", " 2 ! sin!67. 5 o "
i!1

8
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1.1 Low-Pass Filtering

wave
disturbance

!b !0
bandwidth of
closed-loop
system low-pass
filter

A higher-order low-pass filter implies better disturbance suppression to the price of

additional phase lag

9
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.1.2 Cascaded Low-Pass and Notch
Filtering
For smaller craft the bandwidth of the controller can be close to or within the range of the
wave spectrum. This problem can be handled by using a low-pass filter in cascade with a
notch filter:

! !s" " hlp !s"hn!s"y!s"

! frequency range of
wave disturbance
where
s 2"2!" n s"" 2n
hn !s" ! !s"" n " 2
For a vessel moving at forward
speed U the optimal notch
frequency will be:

!n ! !e
but notch filtering also introduces
additional phase lag!
therefore use Kalman filtering or a
linear/nonlinear observer

10 !n
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2 Fixed Gain Observer Design
The simplest state estimator is designed as a fixed gain observer where the ultimate goal
of the observer is to reconstruct the unmeasured state vector x from the measurements
u and y of a dynamical system.

11
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.1 Observability
Definition 11.2 (Observability) Consider the linear time-invariant system:

x! " Ax # Bu #
y" Hx #
The state and output matrices (A, H) must satisfy the observability condition to ensure that
the state x can be reconstructed from the output y and the input u. The observability
condition requires that the matrix:

O ! !H! | A! H ! | . . . | "A! # n!1 H! $

must be of full column rank such that (at least) a left inverse exists.

If the observability matrix is nonsingular, the poles of the error dynamics can be placed in
O
the left half-plane by using the Matlab function:
k = place(A,h,p)
where p = [p_1,...,p_n] is a vector describing the desired locations of the observer error
poles (must be distinct).

12
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.2 Luenberger Observer
An alternative to conventional filtering of wave disturbances is to apply a state estimator
(observer).
A state estimator can be designed to separate the LF components of the motion from the
noisy measurements by using a model of the ship and the wave disturbances (WF model).

x! ! Ax " Bu " Ew # Observer goal: reconstruct the unmeasured state

plant vector x from the measurements u and y
y ! Hx " v #

x"! # Ax! $ Bu $ "!y, " #observer

(copy of
# Hx! #
dynamics)

! " !!y, y! is an injection

where
term to be constructed such that:

x! ! x as t ! !
13
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.2 Luenberger Observer
Luenberger Observer (Luenberger, 1964)
Assume that w = v = 0. Defining the estimation error as:

x! ! x ! x"
then error dynamics takes the form:

x"! # Ax! ! "!y, " #

A fixed-gain (Luenberger) observer is found by choosing the injection term as:

!!y, " ! K", " ! y ! ! Hx# # K ! constant

Hence, the error dynamics become:

x"! # !A ! KH"x! #

Asymptotical convergence of x to zero can be obtained for a constant K if

the system (A,H) is observable.

14
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.3 Case Study: Luenberger Observer
for Heading Autopilots using only Compass
Measurements
Example 11.1 (Nomoto ship model exposed to wind, waves and ocean currents)
Let a 1st-order Nomoto model describe the LF motion of a ship:
!! " r #
r! " ! 1 r # K !" ! b" # wr #
T T
b! " ! 1 b # wb #
Tb
where b is the rudder offset (counteracts slowly varying moments on the ship
due to wave drift forces, LF wind and ocean currents).
A linear wave model can be used to model the wave response:
!! w " " w # Kw s
h!s" ! s 2"2!" 0s"" 20
"! w " !#20 ! w ! 2$#0 " w # K www #
The process noise terms, wr,wb, and ww are modeled as white noise processes.
The compass measurement equation can be expressed by the sum:
y ! ! " !w " v
where v represents zero-mean Gaussian measurement noise.
15 ! w and " w
Notice that the yaw rate r nor the wave states are measured.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.3 Case Study: Luenberger Observer
for Heading Autopilots using only Compass
Measurements
Example 11.1 (Nomoto ship model exposed to wind, waves and ocean currents, cont.)
State-space model:
0 1 0 0 0 0
x ! !!w , " w, ", r, b! ! !! 20 !2"! 0 0 0 0 0
A! 0 0 0 1 0 , b! 0 #
w ! !ww , wr, wb! !
0 0 0 ! 1T ! KT K
T
u!! 0 0 0 0 ! T1b 0

0 0 0
2"! 0 # 0 0
Kw
E! 0 0 0 , h ! !!0, 1, 1, 0, 0" #
0 1 0
0 0 1

Is the ship model exposed to environmental forces observable? YES

This implies that yaw rate, bias and waves states can be estimated using
a single compass measurement! see example on next page
16
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.2.3 Case Study: Luenberger Observer
for Heading Autopilots using only Compass
Measurements
Example 11.2 (Luenberger observer gains) It is straightforward to see that the autopilot
model with wave frequency, wind and current models is observable from the input to the
compass measurement y. Matlab

K ! 1; T!50; lambda ! 0.1; wo !1; Tb ! 1000;

A ! [ 0 1 0 0 0
-wo*wo -2*lambda*wo 0 0 0
0 0 0 1 0
0 0 0 -1/T -K/T
0 0 0 0 -1/Tb ]

h ! [0,1,1,0,0]

n ! rank(obsv(A,h))

rank!O" ! 5
results in n = 5 corresponding to . Hence, the system is observable.
The Luenberger filter gains can be computed by using:
k = place(A',h,[p1,p2,p3,p4,p5])
where p1,p2,p3,p4,p5 are the desired closed-loop poles of x"! # !A ! kh !x!
!
17
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3 Kalman Filter Design
An alternative solution to the pole-placement technique is to apply a Kalman filter
(Kalman 1960) to compute the estimator gain matrix K.
Kalman filtering (or optimal state estimation in sense of minimum variance) allows the
user to estimate the state x of a dynamic system from a noise-contaminated input-
output pair (u, y).

w is a zero-mean Gaussian white noise process

x! " Ax # Bu # Ew with covariance matrix Q = QT > 0
v is a zero-mean Gaussian white noise process
y ! Hx " v with covariance matrix R = RT > 0

If the system is observable, the

state vector x can be
reconstructed recursively
through the measurement
vector y and the control input
vector u
18
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.1 Discrete-Time Kalman Filter
Discrete-Time Kalman Filter Design
The discrete-time KF is defined in terms of the discretized system model:

x!k ! 1" " !x!k" ! !u!k" ! "w!k" #

y!k" # Hx!k" ! v!k" #
where

! ! exp!Ah" ! I " Ah " 1 !Ah" 2 ". . . " 1 !Ah" N #

2 N!
"1 "1
! ! A !! " I"B, " ! A !! " I"E #

The discretized system matrices can be computed in Matlab as:

[PHI,DELTA]!c2d(A,B,h)
[PHI,GAMMA]!c2d(A,E,h)

where PHI!!, DELTA!! and GAMMA!".

Notice that Euler integration implies choosing N ! 1, that is !!k" ! I " Ah

19
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.1 Discrete-Time Kalman Filter
Discrete-Time
Kalman Filter Design matrices Q!k"! Q ! !k" ! 0, R!k"! R ! !k" ! 0 (usually constant)
Algorithm

x !0" " x 0
The algorithm,
Initial conditions P!0" " E#!x!0" ! x# !0""!x!0" ! x# !0"" ! $ " P 0
requires that the
state estimation
error covariance
matrix P(k) = P(k)T> 0 Kalman gain matrix K!k" " P!k"H! !k" #H!k"P!k"H ! !k" $ R!k"$ !1
is computed on-line. State estimate update x# !k" " x !k" $ K!k" #y!k" ! H!k" x !k"$
Error covariance P" !k" " #I ! K!k"H!k"$ P!k" #I ! K!k"H!k"$ !
update $ K!k"R!k"K ! !k", P" !k" " P" !k" ! ! 0

State estimate x !k $ 1" " !!k"x# !k" $ #!k"u!k"

propagation
Error covariance P!k $ 1" " !!k"P" !k"! ! !k" $ $!k"Q!k"$ ! !k"
propagation
20
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.2 Continuous-Time Kalman Filter
Continuous-Time KF Algorithm Linear system model

x! " Ax # Bu # Ew #
! !
Design matrices Q!t"! Q !t" ! 0, R!t"! R !t" ! 0 (usually constant)
y " Hx # v #

x" !0" # x 0
Initial conditions P!0" # E#!x!0" ! x" !0""!x!0" ! x" !0"" ! $ # P 0

injection term

Kalman gain matrix K!t" # P!t"H! !t"R !1 !t"

copy of
State estimate x"$ !t" # A!t"x" !t" % B!t"u!t" % K!t"#y!t" ! H!t"x" !t"$
dynamics
propagation
Error covariance P" !t" # A!t"P!t" % P!t"A! !t" % E!t"Q!t"E! !t"
propagation ! P!t"H ! !t"R !1 !t"H!t"P!t", P!t" # P ! !t" ! 0

21
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.2 Continuous-Time Kalman Filter
Steady-State Kalman Filter
In the linear case it is computationally advantageous to use the steady-state solution of the KF.
This filter has the same structure as the fixed-gain observers. The only difference is the method
for computation of the filter gain matrix.

x! " Ax # Bu # Ew # System equations

y " Hx # v #

x"! # Ax" $ Bu $ K ! !y " Hx" "

Kalman filter with fixed gain
K ! ! P ! H ! R "1 Matlab: [k,P] = lqe(A,E,h,Q,R)
where P= PT > 0 is the positive definite solution of the algebraic matrix Riccati
equation:

AP ! ! P ! A ! ! EQE ! " P ! H ! R "1 HP ! " 0

This solution can only be applied for linear time-invariant (LTI) systems.

22
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.3 Extended Kalman Filter
Nonlinear system model
! !
Design matrices Q!k"! Q !k" " 0, R!k"! R !k" " 0 (usually constant)
x! ! f!x" " Bu " Ew #
y ! Hx " v #
x!0" ! x0
Initial conditions P!0" ! E#!x!0" ! x# !0""!x!0" ! x# !0"" ! $ ! P 0

injection term
! !
Kalman gain matrix K!k" ! P!k"H !k" #H!k"P!k"H !k" $ R!k"$ !1
State estimate update x# !k" ! x!k" $ K!k" #y!k" ! H!k"x!k"$
Error covariance P! !k" ! #I ! K!k"H!k"$ P!k" #I ! K!k"H!k"$ !
update $ K!k"R!k"K ! !k", P! !k" ! P! !k" ! " 0 Discretized approximate
model
!!x! !k",u!k"" ! x! !k" " h#f!x! !k"" " Bu!k"$
State estimate x!k $ 1" ! "!x! !k", u!k""
propagation
!f!x!k", u!k""
!!k" ! I "h
!x!k" x!k"!x! !k"
Error covariance P!k $ 1" ! !!k"P! !k"! ! !k" $ "!k"Q!k"" ! !k"
propagation "!k" ! hE

23
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.4 Corrector-Predictor Representation
for Nonlinear Observers
Example 11.3 (Corrector-
Predictor for Ship Navigation
using Two Measurement Rates)

When implementing nonlinear

observers, the corrector-
predictor representation of the
discrete time EKF can be used to
effectively handle slow
measurement rates, multiple
measurement rates and dead-
reckoning.

x"! # f!x! , u" $ "!y, " #

!!y, " ! K!y ! " #

K d ! hK
x! x!
Corrector x! !k" ! x" !k" " K d #y!k" ! y" !k"$
#
Predictor x!k " 1" ! x!x"!k" " hf!x! !k",u!k""
24
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.5 Case Study: Kalman Filter for
Heading Autopilots using only Compass
Measurements
The main sensor components for a heading controlled marine craft are:

Magnetic and/or gyroscopic compasses measuring

Yaw rate gyro measuring r

In many commercial systems only the compass is used for feedback control since the yaw rate
can be estimated quite well by a state estimator.

LF vessel model WF model

!! " r #
!! w " " w #
r#! " ! 1 r $ m
1 !"
wind $ " N " $ b $ w 2 #
T "! w " !# 20 ! w ! 2$# 0 " w # w 1 #
b! " w 3 #

Compass measurement Yaw moment (control input)

y ! ! " !w " v # ! N ! m K " ! N " " # m ! I z ! N r"
T

25
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.5 Case Study: Kalman Filter for
Heading Autopilots using only Compass
Measurements
The LF and WF models must be written in state-space form in order to use
the Kalman filter algorithm.

State-space model

x ! !! w , " w , ", r, b" ! # 0 1 0 0 0 0

u ! # wind " # N # !! 20 !2"! 0 0 0 0 0
w ! !w 1 , w 2 , w 3 " ! # A! 0 0 0 1 0 , b! 0 #
0 0 0 !1/T 1 1/m
0 0 0 0 0 0
x! ! Ax " bu " w #
y ! h! x " v # 0 0 0
1 0 0
E! 0 0 0 , h ! ! !0, 1, 1, 0, 0" #
0 1 0
0 0 1

26
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.5 Case Study: Kalman Filter for
Heading Autopilots using only Compass
Measurements

27
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.5 Case Study: Kalman Filter for
Heading Autopilots using only Compass
Measurements
True LF heading and
estimate.

True LF heading rate r

and estimate.

True WF component of
the heading w and
estimate.

28
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
History:
Dynamic positioning (DP) systems have been commercially available for marine craft since
the 1960's. The first DP systems were designed using conventional PID controllers in cascade
with low pass and/or notch filters to suppress the wave-induced motion.
From the middle of the 1970's more advanced filtering techniques were available thanks to
the Kalman filter (Kalman 1960). This motivated Balchen and coauthors to define wave
filtering in terms of linear optimal estimation theory; see Balchen et al. (1976, 1980a, 1980b).
A similar design technique has been proposed by Grimble et al. (1979, 1980a).

The Kalman Filter as a DP Observer:

In many cases, measurements of the vessel velocities are not available. Hence, estimates of
the velocities must be computed from noisy position and heading measurements through a
state observer. Unfortunately, the position and heading measurements are corrupted with
colored noise due to wind, waves and ocean currents as well as sensor noise. Only the slowly
varying disturbances should be counteracted by the propulsion system, whereas the
oscillatory motion due to the waves (1st-order wave-induced forces) should not enter the
feedback loop. This require wave filtering.

29
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
Navigation Systems
Several position measurement systems are commercially available, for instance:

Local Navigation Systems

Hydro-acoustic positioning reference (HPR)
Taut wire
Global Navigation Satellite Systems (GNSS)
Galileo EU
Navstar GPS USA
Global Orbiting Navigation Satellite System (GLONASS) - Russian

30
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
In many DP systems the wave filtering and state estimation problems are solved by using the
linear or Extended Kalman Filter (EKF). The major drawback of the EKF is that the kinematic
and kinetic equations of motions must be linearized about varying velocities and yaw angle,
for instance by using:

!" ! R!!"# #
M#" " C RB !#"# " Dexp!!"V rc "#r " d!V rc ,# rc " ! $ " $wind #

! 12 "AFc C X !! rc "V 2rc

d!V rc , ! cr " ! ! 12 "ALc C Y !! rc "V 2rc # Nonlinear damping
! 12 "ALc L oa C N !! rc "V 2rc ! N |r|r r|r

An alternative approach is to linearize the nonlinear damping model (small velocity

assumption) such that vessel parallel coordinates can be used (no need for look-up tables):

Dexp!!!V rc "!r ! d!V rc , " rc " " D! ! R ! !#"b #

31
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
Vessel kinematics and kinetics
!" ! R!!"# # LF model with linear
damping and nonlinear
M#" $ D# ! % $ R ! !!"b $ w3 # rotation matrix

where ! ! !N,E,!" ! , " ! !u,v,r"

1st-order wave response model

1st-order wave forces
!" ! Aw ! # Ew w1 #
$w ! Cw ! #

where ! !! 6 is the state vector, w1!! 3 is a vector of zero-mean Gaussian white noise,
and Aw ! ! 6!6, Ew ! ! 6!3 and Cw ! ! 3!6 are constant matrices of appropriate dimensions
Bias modeling (slowly varying disturbances)
2nd-order wave forces,
b! ! w2 ocean currents and
where w2 is a vector of zero-mean Gaussian white noise. wind forces
32
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
Measurement model
The position and yaw angle measurements are generated by using
the principle of linear superposition:
Total motion, LF + WF

LF motion
y ! ! " !w "v

WF motion

0
Resulting DP observer model
0 50 100 150
time
!" ! A w ! # E w w1 #
$" ! R!!"% # The nonlinear rotation matrices can
b" ! w2 !alternatively b" ! !T!1 b # w2 " # be removed by using vessel parallel
coordinates
M%" ! !D% # R ! !!"b #& # w3 #
y ! $ " C w! " v #
33
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
Linear vessel parallel (VP) Kalman filter design
Since the only nonlinear term in the system model is the rotation matrix R it is convenient
to use vessel parallel coordinates (assumes that the heading angle is constant):

!
! p ! R ! !!"! # !" p # R ! !!"!" $ R" !!"!
bp ! R ! !!"b # !
! R ! !!"R!!"% $ R" !!"R!!"! p
!
This gives the linear model: ! % $ R" !!"! #

zero for constant heading, and

!" ! A w ! " E w w 1 #
negligible for vessels with large
#" p ! $ # inertiathat is, slow rotational
velocity
b" p ! w 2 !alternatively b" p ! !T!1 bp " w 2 " #
M$" ! !D$ " bp " % " % wind " w 3 # When using this model and the
y ! # p " Cw! " v # linear KF, the heading angle
must change slowly. If not use
the EKF!
34
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
State-space model using VP coordinates

x! ! Ax " Bu " Ew #
y ! Hx " v #

Aw 0 6!3 0 6!3 0 6!3 0 6!p

0 3!6 0 3!3 0 3!3 I3!3 0 3!p
A! , B ! #
0 3!6 0 3!3 !T!1 0 3!3 0 3!p
0 3!6 0 3!3 M!1 !M!1 D M!1 B u

Ew 0 6!3 0 6!3
0 3!3 0 3!3 0 3!3
E! , H! C w I3!3 0 3!3 0 3!3 #
0 3!3 I3!3 0 3!3
0 3!3 0 3!3 M!1

35
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.3.6 Case Study: Kalman Filter for
Dynamic Positioning Systems using GNSS
and Compass Measurements
Linear Kalman filter using VP coordinates
The continuous-time filter equations for this system is given by:

x"! # Ax" $ Bu $PH! R !1 !y ! Hx" " #

K
P! ! AP " PA! " EQE! ! PH! R !1 HP #

The covariance matrices Q = QT and R = RT are design matrices:

The measurement covariance matrix can be chosen as R = diag{r1,r2,r3} where the
elements r1 and r2 are the covariance of the GNSS position measurements and r3 is the
compass covariance.
The matrix Q is usually chosen to be diagonal with positive tunable parameters.
These are usually found by trial and error.
Notice that only local exponential stability can be proven for VP coordinates.
Global exponential stability can, however, be obtained using a nonlinear passive observer
36
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4 Nonlinear Passive Observer Designs
Extended Kalman Filtering has been applied in a large number of ship applications,
see Fossen (1994) and references therein, Balchen, Jenssen and Slid (1976, 1980),
Grimble, Patton and Wise (1980), Katebi and Grimble (1989) to mention some.

Drawback: If the EKF is combined with a state feedback controller using the estimates of the
states global asymptotic/exponential stability cannot be guaranteed.

The EKF is only locally exponentially stable (LES) since the Ricatti equations are based on the
linearized model of the plant using the transition matrix.
Global exponential stability (GES) of an observer-controller system requires:

The existence of a nonlinear separation principle

A nonlinear globally stable observer

Conclusion: There is no guarantee for global stability when applying the EKF in cascade with
your favorite full state feedback controller.

37
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4 Nonlinear Passive Observer Designs
The drawbacks of the Kalman filter are:
It is difficult and time-consuming to tune the state estimator (stochastic system with
15 states and 120 covariance equations).

The main reason for this is that the numerous covariance tuning parameters may be
difficult to relate to physical quantities. This results in an ad hoc tuning procedure for
the process covariance matrix Q while the measurement covariance matrix R usually
is well defined in terms of sensor specifications.
only local results

This motivates the search for a fixed gain observer covering the whole state space (global
exponential stability)

Alternative Solution:
Fossen, T. I. and J. P. Strand (1999). Passive Nonlinear Observer Design for Ships Using Lyapunov
Methods: Experimental Results with a Supply Vessel, Automatica, Vol. 35, No. 1, pp. 3-16, January 1999.

38
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Dynamic positioning (DP) Model Position/heading (Earth)

Velocity (Body)

v (sway) q (pitch)
Y

p (roll) r (yaw)

u (surge)
w (heave)
X
Z
39
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Environmental models (wind, waves and ocean currents):

Stochastic bias models: Deterministic bias models:

b! ! T!1 b " w w! 0 b! ! T!1 b

b! " w b! " 0
white noise
Stochastic wave model: Deterministic wave model:

!" ! A w ! # B w w # w! 0 !" ! Aw ! #
$ w% Cw! # # w$ Cw! #

white noise
wi K wi s
(s) = 2
wi s + 2 !0i s + !02i 1st-order wave-induced forces

The passive nonlinear observer is derived in a deterministic setting but it can be

used in stochastic systems.

40
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Observer requirements:
y = + w + v
Total motion, LF + WF Observer must reconstruct
LF motion , w and from y

Only and are used for

feedback
WF motion

0 50 100 150
time
Position/heading (Earth)
Measurement Equation (GNSS+Compass)
= [x, y, z]T

y = + w + v measurement noise Velocity (Body)

PSD = [u, v, r]T
Low-frequency Wave frequency
motion from the motion generated by
ship model a wave spectrum
41 frequency
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
!
!" # A w !" # K 1 !$ o "y% # wave model
&"! # R!y 3 "'" # K 2 y% # kinematics
( (
b" # !T!1 b" # K 3 y% !alternatively b" ) K 3 y% " # bias
!
M'"! # !D'" # R ! !y 3 "b" # * # R !y 3 "K y% 4 # dynamics
y # &" $ C w !" # measurements
DGPS
Compass
y

y y
Goal: Bias estimator:
- wave drift loads
Wave estimator:
- 1st-order wave-induced motion
choose the K4 - currents K3 K2 K1
- wind
gains Ki such b x
R (y3) Cw
T

that the error -

dynamics is R (y3) Aw
T
T -1
h
passive and
w

GES t n h
- M -1 R (y3)

42
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements

Observer error dynamics (position and WF models):

!
!"! 0 # A0 !" 0 #B 0 R!y 3 "$" # K 0 !% o "y& # K 1 !!o " !"0 ! !#" , !"! "!
K 0 !!o" !
y # C 0 !" 0 # K2

Aw 0 6!3 0 6!3
A0 ! , B0 ! , C0 ! C w I 3!3
0 3!6 0 3!3 I 3!3

Observer error dynamics including velocity/bias:

!"! 0 # !A0 ! K 0 ""o #C 0 $!# 0 $B 0 R"y 3 !%# # !"# ! ! !$ #

! "! !K 3 y# ! # b" # b ! b$ #
b" # !T!1 b# ! K 3 y# "alternatively b&
! %" 0 ! % 0 !%$ 0 #
M%"! # !D%# $ R ! "y 3 !b# ! R "y 3 !K 4 y# #

43
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Forming two passive blocks H1 and H2:
M!"! # !D!" ! R! !y 3"z"

! z ! !R ! !y 3 "z"
ez n
M
-1
- -

D
T
H1
R R

z! ! K 4 y! ! b! z
C
x
B
en
! ! ! R!y 3 ""#

A
H2

x"! # Ax! " BR!y 3"#! # #! 0

x! "
z! # Cx! # b!

44
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4 Nonlinear Passive Observer Designs
Passivity is a property of engineering systems, most commonly used in electronic engineering
and control systems.

A passive component, may be either a component that consumes (but does not produce)
energy, or a component that is incapable of power gain. A component that is not passive is
called an active component.

An electronic circuit consisting entirely of passive components is called a passive circuit (and has
the same properties as a passive component).

A transfer functions h(s) must have phase greater than -90 in order to be passive.

Passivity is related to stability and Lyapunov analysis can be used to prove passivity/stability in
nonlinear systems while for linear systems the Kalman-Yakubovich-Popov (KYP) Lemma can be
used to prove stability.

45
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Definition 6.3 (Khalil 2002) A nonlinear system is said to be passive if there exists a
continuously differentiable positive definite function V(x) (called storage function) such
that:

u T y ! V!

Moreover, it is said to be

Lossless if u T y ! V"
u T y ! V! " u T !!u"
Input-feedforward passive if for some function j(u)
u T y ! V! " u T !!u"
Input strictly passive if and u T(u) > 0, for all u 0

u T y ! V! " yT !!y"
Output-feedback passive if for some function r(y)
u T y ! V! " yT !!y"
Output strictly passive if and y T(y) > 0 , for all y 0

u T y ! V! " !!x"
Strictly passive if for some positive definite function y(x)

46
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Proposition 11.1 (Strictly Passive Velocity Error Dynamics)
The mapping H1 is strictly passive.
Proof: Let,
S1 ! 1
2 !" ! M!"
be a positive definite storage function. Time differentiation of S1, yields:

S! 1 " ! 12 !"!!D # D ! "!" ! z" ! R!y 3 "!"

Using the fact that , yields:
! z ! !R ! !y 3 "z"
!z H1 !"
Hence:
!!z "# $S! 1 " 12 "# ! !D % D! ""#

This proves that H1 is strictly passive.

47
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Theorem 6.3 (Khalil 2002) The feedback connection of two time-invariant dynamical systems is
GAS if the origin of the nominal system (u = 0) is asymptotically stable and

both feedback components are strictly passive

both feedback components are output strictly passive and zero-state observable, or
one component is strictly passive and the other is output strictly passive and zero-state
observable

In addition the storage function for each component must be radially unbounded

1. The mapping is strictly passive (block H

! z ! "# 1)

2. Post-multiplication with the bounded transformation matrix R(y3) and pre-

multiplication by it's transpose will not affect the passivity properties.
! ! ! z"
3. Hence, it only remains to show that the mapping (block H2) is strictly passive

For linear systems passivity can easily be checked by applying the

Kalman-Yakubovich-Popov (KYP) Lemma.

48
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Emma 11.1 (Kalman-Yakubovich-Popov)
Let Z(s) = C(sI-A)-1B be an mm transfer function matrix, where A is Hurwitz, (A,B) is
controllable, and (A,C) is observable. Then Z(s) is strictly positive real (SPR) if and only if
there exist positive definite matrices P = PT and Q = QT such that:

PA ! A! P " !Q #
B !P " C #

Since H1 is strictly passive and H2, given by three matrices (A, B, C) according to

x"! # Ax! " B# ! # !! z!

H2
z! # Cx! #

can be made SPR by choosing the gain matrices Ki (i=1,,4) according to the KYP Lemma.
Hence, according to Lemma 6.4 (Khalil 2002), H2 is strictly passive since H2 is SPR
Interconnected system H1 and H2 is GAS
49
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Determination of the Observer Gains
The mapping H2 describes three decoupled systems in surge, sway, and yaw.

This suggests that the observer gain matrices should have a diagonal structure:

diag#K11 !! o1 ", K 12 !! o2 ", K13 !! o3 "$

K 1 !! o " ! #
diag#K14 !! o1 ", K 15 !! o3 ", K16 !! o3 "$ function of wave
K 2 ! diag#K21 , K22 , K 23 $ # frequencies in surge,
sway, and yaw.
K 3 ! diag#K31 , K32 , K 33 $ #
Opens for:
K 4 ! diag#K41 , K42 , K 43 $ #
-gain scheduling
-adaptive observer

50
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Three decoupled transfer functions:

z! !s"" H!s"# ! !s" ! H 0 !s"HB !s"# ! !s"

H0 !s" ! C0 #sI ! A0 ! K 0 !"0"C 0$ !1B 0

HB !s" ! K 4 " !sI ! T!1 "!1 K 3

Ho is determined by using pole placement:

s 2"2! i " oi s"" 2oi

h oi !s" !
s 3"!K 1!i"3" "K 2i "2! i " oi "s 2"!" 2oi "2! i " oi K 2i !K 1i " 2oi "s"" 2oi K 2i

s 2"2! i " oi s"" 2oi !

h di !s" ! K1i !!oi " ! !2!"ni ! # i " ! cioi #
s 2"2# ni " oi s"" 2oi !s"" ci "
K1!i"3" !!oi " ! 2!oi !" ni ! # i " #
The desired structure is
low-pass + notch K2i ! !ci #

51
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
The remaining gains K3 and K4 in HB is found by frequency shaping. The transfer functions
hi(s) must all have phase greater than -90 in order to be passive.

This is satisfied for:

1/Ti ! K3i /K 4i ! ! oi ! ! ci

H!s" ! H0 !s"HB !s"

K K
s" 1 " K 3i T i !1 s" K 3i
Ti
hBi !s" ! K4i 4i
" K 4i 4i
s" T1 s" T1
i i

s 2"2! i " oi s"" 2oi

h di !s" !
s 2"2# ni " oi s"" 2oi !s"" ci "

52
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements

The first experiments

were performed in
the GNC laboratory at
the Department of
Engineering
Cybernetics, NTNU
using CyberShip I
which is offshore
supply vessel scale
1:70.

53
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.1 Case Study: Passive Observer for Dynamic
Positioning using GNSS and Compass Measurements
Measured (gray) and estimated LF (solid) x-position deviation [m] Estimated bias in surge [kN]

Experimental results:
implemented and tested
onboard several ships and
rigs offshore.

Reduced commissioning
time: easy to tune
Measured (gray) and estimated LF (solid) y-position deviation [m] Estimated bias in sway [kN]

compared to the Extended

Kalman Filter.

Measured (gray), estimated LF (solid) and desired (dotted) heading [deg] Estimated bias in yaw [kNm]

I II III I II III

54
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements
Passivity-Based Pole Placement
The observer error dynamics can be reformulated as two subsystems for yaw angle/rudder
bias, and yaw rate. Fossen and Strand (1999) have shown that these systems forms a
passive interconnection if the observer gains are chosen according to:
hi(s) = hoi(s)hBi(s)

!2!0 !1 ! ""/!c 20 mean wave drift forces,

1st-order wave loads:
currents, wind
woi = 0.8976 (rad/s)
2! 0 !1 ! ""

Mag nitud e (dB )

0

k! !c -20
integral
action

K4 -40
notch low-pass
K5 -60
effect filter
90

45

! c ! ! 0 is the filter cut-off

Phas e (de g)
where 0

frequency and: -45

-90
0 ! 1/Tb ! K5 /K 4 ! ! 0 ! ! c -135
1/Ti K3i/K4i woi wci
-4 -3 -2 -1 0 1
10 10 10 10 10 10

55
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements
Example 11.7 (Passive Wave Filtering) Consider the Mariner class cargo ship with K=0.185 s-1
and T=T1+T2-T3 = 107.3 s (Strm-Tejsen 1965). The bias time constant is chosen to be rather
large, that is Tb = 100 s. The wave response model is modeled by a linear approximation to
! ! 0. 1 ! 0 ! 1. 2
the JONSWAP spectrum with and rad/s.

State-space model:

0 1 0 0 0 0
!1. 96 !0. 26 0 0 0 0
wave-frequency
A! 0 0 0 1 0 ,b ! 0 #
model
0 0 0 !0. 0093 !0. 0017 0. 0017
0 0 0 0 !0. 001 0
ship + bias models
0 0 0
0. 26 ! 0 0
E! 0 0 0 , h ! !!0, 1, 1, 0, 0" #
0 1 0
0 0 1

56
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements
Example 11.7 (Passive Wave Filtering, cont.) Using passivity as a tool for filter design with
cut-off frequency , yields:
! c ! 1. 1! 0

K1 !2!0 !1 ! ""/! c !1. 64

K2 2!0 !1 ! "" 1. 80 !0
k! K3 ! !c ! 1. 10 !0
K4 K4 K4
K5 K5 K5

! 0 ! 1. 2 ! c ! 1. 1! 0
!0
Notice that the notch effect at is
more than -20 dB for h3(s) and h4(s)
representing the state estimates .
!! and !r
We also see that high-frequency motion
!c
components above is low-pass
filtered. Finally, the transfer function
h2(s) representing reconstruction of the
!! w
WF motion filters out signals on the
outside of the wave response spectrum. Bode plot showing the wave filter transfer
functions and the JONSWAP spectrum.

57
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements

passive
MSS Toolbox Simulink example wave filter
wave
amplitude
psi_w
Pilot w y 3 D2R
30 R2D
input (deg)
Band-Limited
White Noise Linear 2nd-order psi_w psi_w
wave spectrum
D2R

psi_w
psi compass
R2D
psi_ship
3rd order psi_d Only
psi
LP-filter r_d PID tracking Compass
delta_c r psi_d, psi
controller delta_c r_ship rudder angle
dr/dt_d r and psi_ship
Reference model
delta Autopilot Wave Filter 1
delta
Course autopilot
Mariner class cargo ship
(2nd -order)

R2D
Autopilot demo using only compass measurements
Author: Thor I. Fossen r and r_ship
21-Mar-2003 13:23:10

58
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements

Passive Wave Filter Block 1

compass
-pi<y <pi y (rad)

rad to [-pi pi]

K5 K4 w_c -K- -K-
K5 K4 K3 K2 K1

psi_w
1

1 1 psi_w
s psi_w s

psi_w
1
s 1/T_b 2*lambda*w_o
rudder
bias

w_o^2
xi_w

1 r 1 psi
2 K 1/T y (rad) -pi<y <pi
s s
rudder
angle rad to [-pi pi] 2

psi
3
r

59
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.2 Case Study: Passive Observer for Heading
Autopilots using only Compass Measurements

60
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.3 Case Study: Passive Observer for Heading
Autopilots using both Compass and Rate Measurements
It is advantageous to integrate gyro and compass measurements in the observer. This
results in less variance and better accuracy of the state estimates.
One simple way to do this is to treat the gyro measurements as an input to the system
model:
!! " u gyro # b #
b! " wb #

where b denotes the gyro bias, wb is Gaussian white noise and ugyro is the rate gyro
measurement.

This model will give proper wave filtering of the state . However, the estimate of r is not
wave filtered, since this signal is taken directly from the gyro measurement ugyro. This can
be solved by filtering ugyro with a notch filter hnotch(s) and a low-pass filter to the cost of
some phase lag:
uf ! hnotch!s" h lp!s" u gyro

61
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.3 Case Study: Passive Observer for Heading
Autopilots using both Compass and Rate Measurements
The resulting model becomes:

!!" w # " ! w $ K1 # # WF model

!" w # !$ 20 !! w ! 2%$ 0 "! w $ K 2 # #
"
"!" # u f $ b! $ K3 # # yaw kinematics with gyro input
"
b! # ! 1 b! $ K4 # # gyro bias
Tb

" !"
where ! ! y ! " " w and Tb " 0

Notice that the gyro bias must be estimated on-line since it will vary with temperature and
possible scale factor/misalignment errors when mounted onboard the ship. This is a slowly
varying process so the gain K4 can be chosen quite small reflecting a large bias time
constant.
If passivity-based pole placement is used, K1,K2 and K3 become:

K1 ! !2!0 !1 ! ""/! c , K2 ! 2!0 !1 ! "", K 3 ! !c

62
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.3 Case Study: Passive Observer for Heading
Autopilots using both Compass and Rate Measurements
MSS Toolbox Simulink Example
1
compass
bias 1
-pi<y <pi y (rad)
bias s

rad to [-pi pi]

w_c -K- -K-
K4 1/T_b K3 K2 K1
K4
psi_w
1

1 1 psi_w
s psi_w s

psi_w

2*lambda*w_o

gyro filter w_o^2

xi_W

s2 +2*zeta_n*w_os+w_o*w_o w_c 1 psi

2 y (rad) -pi<y <pi
s2 +2*w_os+w_o^2 s+w_c s
rate gyro
Notch filter LP filter rad to [-pi pi] 2
psi
3

63
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.3 Case Study: Passive Observer for Heading
Autopilots using both Compass and Rate Measurements

The wave filter has been tested on a

scale model of MV Autoprestige of the
United European Car Carriers (UECC)

The maneuvering test were

performed in the Ocean
Basin at MARINTEK in
Trondheim April 2001.

64
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.4.3 Case Study: Passive Observer for Heading
Autopilots using both Compass and Rate Measurements

It is seen that the WF motion Significant wave height: Hs= 1.3 m (full scale)
components are quite well Frequency of encounter: = 1.07 rad/s
!e
removed from the estimate of Cruise speed: U = 2.3 m/s (model scale)
resulting in good course-keeping
capabilities. Course-keeping maneuver
145
y
140
We also notice that the estimate
!w is quite good, while r
of
135

could be slightly improved by 130

y= y +yw
changing the observer gains. 125
0 2 4 6 8 10 12
time (s)
xy-plot
40

30
y (m)

20

10

0
25 30 35 40 45 50 55 60
x (m)
65
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
Inertial Navigation Systems
An inertial navigation system (INS) consists of:

Inertial measurement unit (IMU)

Global reference system (GNSS)
Software (state observer) that computes position, velocity and attitude from the
measurements

When integrating the angular velocity (gyro) and linear acceleration (accelerometers) using
the strapdown equations, drift must be prevented. This is obtained by using a GNSS as
reference for position and the resulting system is known a strapdown inertial navigation
system.
In case of GNSS drop-outs the strapdown equations are integrated without corrections of the
observer (zero observer gains). Hence, the position and velocity will drift off until the GNSS
signals returns. The bias estimates are frozen during GNSS drop-outs since they require
position updates.

66
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
Inertial Navigation Systems (INS) and Global Navigation Satellite Systems (GNSS) are used in
manned and unmanned vehicles. Robust navigation is very important when designing
automatic/autonomous systems.

Why not buy an integrated INS and GNSS system?

High-quality systems are expensive
You cannot modify the SW (proprietary code). Non-trivial to add SW for failure detection,
fault tolerance etc. which is necessary for robust navigation and tight integration with the
autopilot.
Sensor fusion with other sensors is difficult (no access to Kalman filter source code)

Why use nonlinear observes instead of the well-proven extended Kalman filter (EKF)?
A small computational footprint is important in embedded systems with limited power. The
EKF uses hundreds of Riccati equations, which can be avoided.
Explicit stability requirements for semiglobal or global exponential stability (not available
when using the EKF). This gives robustness guarantees and tuning rules for the convergence
67 rate of the estimates.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
Inertial Measurement Systems
Today inertial measurement technology is available for commercial users thanks to a
significant reduction in price the last decade. As a consequence of this, low cost inertial
sensors can be integrated with satellite navigation system using a conventional Kalman
filter or a nonlinear state observer.

LN-200 (Courtesy Litton)

ISA (Inertial Sensor Assembly) - cluster of IMU (Inertial Measurement Unit) - consists of an
three gyros and three accelerometers that ISA, hardware to interface the ISA, and low level
measure angular velocity and linear software that performs down-sampling,
acceleration, respectively. temperature calibration, and vibration (sculling
and coning) compensation
68
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 MEMS
Microelectromechanical systems (MEMS) is the technology of very small
mechanical devices driven by electricity.

It merges at the nano-scale into nanoelectromechanical systems (NEMS) and

nanotechnology.

Today inertial measurement technology is available for commercial users thanks to a MEMS
technology and a significant reduction in price the last decades.

Low-cost inertial sensors can be integrated with a satellite navigation system or other
positioning systems using a conventional Kalman filter or a nonlinear state observer.

69
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Inertial Measurement Units (IMUs)

MicorStrain 3DM-GX3 -45

Attitude and Heading
Reference System (IMU)
with build-in EKF
~ $2,000.

70
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Low-Cost Motion Sensors
MTi Attitude and Heading
Reference System (IMU) with
build-in EKF

~ $2,000

Dimensions: 58mm x 58mm

71
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Inertial Measurement Units (IMUs)
ADIS16488 Tri-Axis Inertial Sensor
with Magnetometer and Pressure
sensor by Analog Devices
~ $2000
10 DOF IMU Mounted in a Cube:
3-axis, digital gyroscope
Tri-axis, 18 g digital accelerometer
Tri-axis, 2.5 Gauss digital magnetometer
Digital pressure sensor, 300 mbar to 1100 mbar
550 ms start-up time
Gyro drift 0.0014 deg/s

Factory-calibrated sensitivity, bias, and axial alignment

Operating temperature range: 40C to +85C
Gyro bias stability for some inertial sensors
Earth rotation
MEMS Units drift deg/s drift deg/h 0.0042 deg/s
Sensonor STIM 300 0.0001 deg/s 0.5 deg/h
Analog Devices ADIS 16488 0.0014 deg/s 5.1 deg/h
Analog Devices ADIS 16405 0.007 deg/s 25.2 deg/h
MicroStrain 3DM-GX3 -45 0.2 deg/s 720.0 deg/h

72
XSENS MTi 1.0 deg/s 3600.0 deg/h
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
~ $10000
 What is an Inertial Navigation System?
An instrument (electronic + sensors) which is using its initial state (position) and
internal motion sensors (gyroscopes + accelerometers) to measure and calculate
its subsequent positions in space with high accuracy, stability and update rate.
The integrated signals will drift. Hence,
bias
position the system must be aided by GNSS,
accelero-
meters hydroacoustic positioning reference (HPR)
systems or other reference systems.
bias
angle
gyros The sensors can be mounted on a gimbal
or a moving body (strapdown), which is
related to the North-East-Down positions
Inertial measurement by the navigation differential equations.
unit (IMU)

In case of GNSS drop-outs the strapdown equations are

integrated without corrections of the observer (zero observer
gains). This is referred to as dead-reckoning.
Hence, the position and velocity will drift off until the GNSS
signals returns. The bias estimates are frozen during GNSS
73
drop-outs since they require position updates.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Inertial Navigation Systems History
1944 German V2 combined two gyroscopes and a lateral accelerometer with a analog
computer to adjust the azimuth for the rocket in flight
1950s Atlas ICBM
1958 USS Nautilus to North Pole
1960 The Kalman Filter is invented
1961 Apollo program
1969 Commercial Navigation Boeing 747

Today gimbaled systems are replaced by strapdown INS

1980s Practical ring laser gyro systems and strapdown INS using fast computers
(Strapdown INS runs at 2 000 Hz)
1985 Development of fiber optic gyros (FOGs) starts
1990s Low-cost MEMS gyros and accelerometers
2006 The Mahoney, Hamel and Pflimlin nonlinear attitude observer can replace the
extended Kalman filter (EKF) and stability is proven
2012-2016 A nonlinear observer framework for strapdown INS (attitude and
translational motion) is developed at NTNU. Tailor-made for embedded computers with
limited computational capacity.

74
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 State Estimator (Observer) Inverse Problem
The ultimate goal of a state estimator or an observer is to reconstruct the unmeasured
state vector x from the measurements u and y of a dynamical system given by ordinary
differential equations (ODEs). This only works if the system is observable.

System described
Measured input u by differential eqs. Measured output y
x! " Ax # Bu # Ew
y ! Hx " v

Kalman filter or Estimated states x

observer

Some observations:
It is possible to estimate linear velocity and acceleration from a position sensor:
- Signal-based approach (no input u). This is equivalent to differentiating the measured position y
- Acceleration as input u and a double-integrator model improves the performance
- Alternatively the vehicle model can be used to compute acceleration.
Drawback: model parameters must be known
75
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Why should we use alternatives to the well-
celebrated Kalman Filter?
Since 1960 the Kalman filter, and nonlinear extensions thereof,
has been used to provide integrated navigation solutions based
on different types of measurements.

The Kalman filter is used in millions of applications and it

is the core algorithm of all modern navigation systems.

However, Rudolf Kalman (May 19, 1930 July 2, 2016)

Nonlinear observers provide explicit stability and

robustness guarantees that are typically not available
for nonlinear Kalman filter implementations.
The number of C-code lines can be significantly
reduced. This simplifies documentation and
maintenance.
Tuning and commissioning are less time consuming.
MEMS technology and faster computers make it
possible to replace gimbaled mechanical solutions
with strapdown navigation differential equations 50th anniversary of the Kalman filter in 2010
running at 100-2000 Hz.
Kalman, R. E. (1960). "Contributions to the theory of optimal control". Bol. Soc. Mat. Mexicana.
Kalman, R.E. (1960). "A new approach to linear filtering and prediction problems" (PDF). Journal of Basic Engineering. 82 (1): 3545
Kalman, R.E.; Bucy, R.S. (1961). "New Results in Linear Filtering and Prediction Theory". Journal of Basic Engineering. March 1961.
76
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
An IMU can be integrated with a satellite navigation system in a
state observer to obtain estimates of position and velocity in 6 DOF.
A stand-alone IMU solution, where
- acceleration measurements are integrated twice to obtain positions
- gyro measurements (angular rates) are integrated once to obtain attitude
will drift due to sensors biases, misalignments, temperature variations etc.
bias
position
accelero-
An INS state estimator aided by
meter
GNSS can be used to estimate the
bias
bias terms and thus give accurate
angle estimates of position and attitude
gyro

A low-cost IMU/GNSS strapdown integration technique can be

used for local navigation by neglecting the Earth rotation and
assuming that the GNSS signals are available all the time.
For vehicles and robots, the North-East-Down reference frame can
be assumed to be inertial even though the Earth is moving
relatively to the star-fixed reference frame.
77
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
Coordinate systems for local navigation:
R
BODY body-fixed frame
NED North-East-Down frame is approximated as
the inertial frame by neglecting the Earth rotation
and movement of the Earth in the solar system

Attitude can be described by a

3 x 3 matrix (9-elements) relating
a vector in BODY to a vector in NED

Consequently, a NED vector vi is

related to a BODY vector vb by
the rotation matrix according to

vn = Rvb where R SO(3)

O(3) is the group of all orthogonal

matrices, i.e. RRT = I3 and RT = R-1

SO(3) special orthogonal group.

The subgroup of the O(3) satisfying: GNSS/INS integration
78 det(R) = 1
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5 Integration Filter for IMU and Global
Navigation Satellite Systems
IMU Measurements:

a bimu ! R bn !!"!v" nm/n "- g n " " bbacc " w bacc # Linear acceleration
# bimu ! # bm/n " bbgyro " w bgyro # Angular velocity
mbimu ! R bn !!"mn " bbmag " w bmag # Magnetic field

bias

bias

79
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.1 Integration Filter for Position and
Linear Velocity
Integration of IMU and GNSS Position Measurements:

v! nm/n ! R bn !"" ! #a bimu ! bbacc ! w bacc $ !+ g n # ! " !!, ", #! !

Dynamics: Observer (copy of dynamics):

n
p! nm/n ! v nm/n Attitude measurements
# p"! m/n # v! nm/n $ K 1 y" 1 #
b
v"! m/n # R nb !#"#a bimu ! b! acc $ !
n
v! nm/n ! R nb !""#a bimu ! bbacc $ !+ g n # + g n $ K 2 y" 1 #
b
b! acc ! 0 # !b
b" acc # K 3 R nb !#" ! y" 1 #
IMU measurements injection
y 1 ! pnm/n # 1 # p! nm/n terms #

pngnss ! !N gnss , E gnss , Dgnss " ! y! 1 ! y1!y 1 ! pn ! p" n

GNSS measurements
v! b "Rbn!#"v! n
80
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.1 Integration Filter for Position and
Linear Velocity
Observer error dynamics:

n
p"! m/n !K 1 I 0 p" nm/n
n
v"! m/n # !K 2 0 !R nb !!" v" nm/n #
b
b"! acc !K 3 R nb !!" ! 0 b
0 b" acc
$
x# # A!!"x #

Property: A matrix K is said to commute with the rotation matrix R() if:
KR!!" ! R!!"K
Examples of K matrices satisfying this property are linear combinations:

K ! a1 R!!" " a2 I " a 3 k ! k

k ! !0, 0, 1" !

81
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.1 Integration Filter for Position and
Linear Velocity
Define the transformation:

x ! T!!"z # T!!" ! diag#Rnb !!", R nb!!", I#

T! !"" ! diag#R nb !""S!# bm/n ", R nb !""S!# bm/n ", 0$ ! 0 #
This assumption is OK for marine craft
Then
!K 1 I 0
! T!!" ! A!!"T!!"z #
A! !K 2 0 I
! Az # A ! T!!" ! A!!"T!!" # !K 3 0 0

if the observer gain matrices Ki (i=1,,3) commute with the rotation matrix R()

How to choose the observer gains using pole placement?

If the matrices Ki are chosen diagonal
K i ! diag!k i , k i , l i ", i ! 1, 2, 3 #
stability can be checked by computing the eigenvalues of A. Notice that the eigenvalues
of A and A() are equal.
82
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.1 Integration of IMU and GNSS
Position and Velocity Measurements
The observer can be extended to include n
p"! m/n # v! nm/n $ K 11 y" 1 $ K 21 y" 2 #
velocity measurements.
b
v"! m/n # R nb !#"#a bimu ! b! acc $ !+ g n $ K 12 y" 1 $ K 22 y" 2 #
n
The gains are again found by using
commutation of matrices. !b
b" acc # K 13 R nb !#" ! y" 1 $ K 23 R nb !#" ! y" 2 #
1 # p! nm/n #
2 # v! nm/n #
Error dynamics:
n
p"! m/n !K 11 I ! K 21 0 p" nm/n
n
v"! m/n # !K 12 !K 22 !R nb !!" v" nm/n
b
b"! acc !K 13 R nb !!" ! !K 23 R nb !!" ! b
0 b" acc
$
x# # A!!"x #

Choosing Kij diagonal so they commute with R(), yields:

! T! !!"AT!!"z
check eigenvalues of A
! Az #
83
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Nonlinear Attitude Estimation
The key component of strapdown INS

84
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Attitude Representations
Coordinate systems for local navigation:

BODY body-fixed frame

NED North-East-Down frame is approximated as
the inertial frame by neglecting the Earth rotation R
and movement of the Earth in the solar system

Attitude can be described by a

3 x 3 matrix (9-elements) relating
a vector in BODY to a vector in NED

Consequently, a NED vector vn is

related to a BODY vector vb by
the rotation matrix according to

vn = Rvb where R on SO(3)

O(3) is the group of all orthogonal

matrices, i.e. RRT = I3 and RT = R-1

SO(3) special orthogonal group.

The subgroup of the O(3) satisfying:
det(R) = 1
85
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Attitude from Reference Vectors
The main challenge in many navigation problems is the estimation of attitude, represented as
the rotation of a body-fixed coordinate frame with respect to some reference frame.

The attitude can be estimated by comparing a set of vectors measured in the BODY frame
using accelerometers, magnetometers or sun sensors with a set of reference vectors in a
second reference frame usually NED.

Micro-electro-mechanical system (MEMS) accelerometer technology

capable of tracking vibrations in industrial equipment at frequencies as high as
22 kHz. Analog Devices (2008)

Algorithms such as QUEST and TRIAD (Shuster and Oh, 1981) can be used to determine the
attitude algebraically from vector measurements if at least two pairs of nonparallel vectors
are available.

However, vector measurements are typically affected by noise. This

suggests that an observer based on a dynamic model and rate
gyroscopes should be used to obtain accurate attitude information
at high frequencies for accelerated vehicles.

Gyro measurements are subject to bias, which must be estimated along with the attitude.

M. D. Shuster and S. D. Oh (1981). Three-axis attitude determination from vector observations, Journal of Guidance, Control
86 and Dynamics, vol. 4, no. 1, pp. 7077.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Parameterizations on SO(3)
3-parameter representation (Euler Angles)

c!c" !s!c# " c!s"s# s!s# " c!c#s" !! " p # q sin! tan" # r cos ! tan" #
!
R nb !! nb " ! s!c" c!c# " s#s"s! !c!s# " s"s!c# # " " q cos ! ! r sin! #
sin! cos !
!s" c"s# c"c# #! " q #r , " " $90 o #
cos " cos "
- This representation is singular for 90 degrees pitch
- Only local exponential or asymptotically stable observers can be designed

4-parameter representation (Quaternions)

!! " ! 1 !" 1 p # " 2 q # " 3 r" #
2
1 ! 2!! 22 " ! 23 " 2!! 1 ! 2 ! ! 3 "" 2!! 1 ! 3 " ! 2 "" "! 1 " 1 !!p ! " 3 q # " 2 r" #
2
R nb !q" ! 2!! 1 ! 2 " ! 3 "" 1 ! 2!! 21 " ! 23 " 2!! 2 ! 3 ! ! 1 "" #
"! 2 " 1 !" 3 p # !q ! " 1 r" #
2!! 1 ! 3 ! ! 2 "" 2!! 2 ! 3 " ! 1 "" 1 ! 2!! 21 " ! 22 " 2
"! 3 " 1 !!" 2 p # " 1 q # !r" #
2
- Avoids the singularity by using one extra parameter
Kinematic constraint:
- Two equilibrium points corresponding to h = 1 or h = -1
- Almost-global or semiglobal exponential stability can be achieved ! 2 ! " 21 ! " 22 ! " 23 " 1 #

It is impossible to obtain global attitude for Euler angles.

Quaternions can be globally stabilized using discontinuous feedback.
87
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.2 Accelerometer and Compass Aided
Attitude Observer
The observers based on Salcudeans work assume that the attitude is available as an explicit
quaternion measurement. This typically requires a separate algebraic resolution of the
attitude usually a static accelerometer-to-attitude mapping.

The static acceleration solution gives inaccurate results for accelerated vehicles exhibiting
Coriolis and centripetal accelerations.

1991 Salcuedan First nonlinear Lyapunov-based attitude observer where the real part of the
quaternion estimation error is used as injection term. Exponential stability is proven.
1999 Vik, Shiriaev and Fossen The Salcuedan observer is extended to include gyro bias.
Convergence is proven by using Barbalats lemma. Local stability by linearization.
2003 Thienel and Sanner A gyro persistency-of-excitation (PE) condition for the
Vik et al. (1999) observer is given and exponential stability of the quaternion and bias
estimation errors follows.

S. Salcudean (1991). A Globally Convergent Angular Velocity Observer for Rigid Body Motion,
IEEE Transaction on Automatic Control TAC-36(12).
B. Vik and T. I. Fossen (2001). A nonlinear observer for GPS and INS integration,
Proc. IEEE Conference on Decision and Control, Orlando, FL, pp. 29562961.
J. K. Thienel and R. M. Sanner (2003). A coupled nonlinear spacecraft attitude controller and observer with an
unknown constant gyro bias and gyro noise, IEEE Transactions on Automatic Control, vol. 48, no. 11, pp. 20112015.
88
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.2 Accelerometer and Compass Aided
Attitude Observer
Static Mapping from Linear Accelerations to Roll and Pitch Angles:
0n 0b 0
a bimu ! R bn !!"g n v! m/n !#R bn !"" ! #a bimu ! bacc ! w bacc $ !+ g n #
!
ax 0 "g sin!!" This is a method for computation
ay ! R bn !!" 0 " g cos!!" sin!"" # of the static roll and pitch angles
using 3 linear accelerometers.
az g g cos!!" cos!""
- OK for marine craft but not
Taking the ratios: high-acceleration maneuvers
(need Coriolis-centripetal
corrections).
ay a x ! " sin!"", a 2y ! a 2z
a z ! tan!!", g 2
! cos 2 !"" #- The mapping needs bias
g
calibration/removal

implies that: a The special solution of the

! ! atan a yz # attitude observer when only the
roll and pitch angles are
" ! "atan ax # estimated (no compass
a 2y ! a 2z measurement) is referred to as a
vertical reference unit (VRU).
89
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.2 Accelerometer and Compass Aided
Attitude Observer
Quaternion-Based Attitude Observer:

1
!! !
q! ! Tq !q"" bb/n # Tq !q" ! 2
!I " S!!"
Gyro measurement equation:

! bm/n ! ! bimu ! bbgyro ! w bgyro #

Motion Reference Unit
(Courtesy Kongsberg Seatex)
Attitude Dynamics: Observer:
q! ! Tq !q" " bimu ! bbgyro ! w bgyro # q"! # Tq !q! "R nb !q" ! R nb !q! "
b
# bimu ! b! gyro $ K p $% sgn!!% " #
b R ! !q" "
b! gyro ! 0 #
%b
b! gyro # ! 1 K i $% sgn!!% " #
2

This observer is an extension of the Salcudean (1991) observer. Bias estimation was
first included by Vik, Shiriaev and Fossen (1999). The proof is also found in Vik and
Fossen (2001) using Lyapunov analysis and quaternion error dynamics.

90 Next: how to implement this?

Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.2 Accelerometer and Compass Aided
Attitude Observer
Key assumption: Roll and pitch angle mappings based on static acceleration.
Must be compensated for acceleration bias. Sensitive to Coriolis acceleration

IMU

S. Salcudean (1991). A Globally Convergent Angular Velocity Observer for Rigid Body Motion, IEEE Transaction on
Automatic Control TAC-36(12). (no gyro bias estimation)
B. Vik, A. Shiriaev and T. I. Fossen (1999). Nonlinear Observer Design for Integration of DGPS and INS, In "New
91 Directions in Nonlinear Observer Design" (H. Nijmeijer and T. I. Fossen, Eds.), Springer (with gyro bias estimation).
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 11.5.3 Attitude Observer using Acceleration
and Magnetic Field Directions
Normalized acceleration and magnetometer vector measurements in BODY
a bimu mbmag
v b1 ! , v b2 ! #
a bimu mbmag

Estimate of vector measurements in NED (reference vectors)

v! b1 ! R bn !q! "v n1 # v! b2 ! R bn !q! "v n2 # v n1 ! a n #

Inertial acceleration must be estimated while
v n2 ! m n #
the Inertial magnetic field is known

Hamel and Mahony (2006)

Mahony, Hamel and Pflimlin (2008) Grip, Fossen, Johansen and Saberi (2012)
n b
k i v b !v" b " ! ! v" b !v b " ! !b" gyro ! Proj(b! bgyro , K i # b !
! bmes ! !vex " 2 i i i i # mes
i!1
b
q#" ! Tq !q" " ! bimu ! b" gyro $ K p ! bmes #
#b
b" gyro ! ! 1 K i ! bmes #
2
H. F. Grip, T. I. Fossen, T. A. Johansen and A. Saberi (2012). Attitude estimation using biased gyro and vector
measurements with time-varying reference vectors. IEEE Transactions Automatic Control.
T. Hamel and R. Mahony (2006). Attitude estimation on SO(3) based on direct inertial measurements,
Proc. IEEE Int. Conf. Robotics Automation, Orlando, FL, pp. 21702175.
R. Mahony, T. Hamel, and J.-M. Pflimlin (2008). Nonlinear complementary filters on the Special Orthogonal Group,
92 IEEE Trans. Automat. Contr., vol. 53, no. 5, pp. 12031218.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen) Next: how to implement this?
 11.5.3 Attitude Observer using Acceleration
and Magnetic Field Directions
Uses direct measurements to update the observer but.
needs an inertial reference vector an

IMU

93
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Experimental Testing using UAV
Adaptive Flight Inc, Atlanta, USA

The adaptive Flight Hornet mini helicopter equipped with

the FCS-20 autopilot system. Autopilot system includes
IMU, GPS and EKF for navigation.

IMU: accelerometers and gyros (100 Hz)

magnetometers (10 Hz)
GPS: position and velocity (5 Hz)

Acceleration reference vector is found by differentiating

the GPS position twice (only for demonstration)
94
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 UAV Example: The Hornet Mini Helicopter

H. F. Grip, T. I. Fossen, T. A. Johansen and A. Saberi (2012). Attitude estimation using biased gyro and vector
95 measurements with time-varying reference vectors. IEEE Transactions Automatic Control.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Experimental Testing using Aircraft
Piper Cherokee 140 light fixed-wing aircraft

Hvard the pilot

The Piper Cherokee is equipped with:

IMU: XSens Mti, accelrometers, gyros and

magnetometers (100 Hz)
GNSS: u-Blox LEA-6H, position and Doppler-based
velocity measurements (5 Hz)

Mounted on a bulkhead within the tail of the aircraft.

The GNSS antenna is mounted on top of the
instrument panel.
96
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Traffic Pattern Maneuver

The third maneuver is a left-handed traffic

pattern flown from takeoff to landing

H. F. Grip, T. I. Fossen, T. A. Johansen and A. Saberi (2014). Globally Exponentially Stable Attitude and Gyro Bias
97 Estimation with Application to GNSS/INS Integration, Automatica, to appear.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Feedback Interconnection of the Attitude Observer
and Translation Motion Observer (TMO)
1 The attitude observer typically runs at 1000 Hz and it uses accelerometers and
magnetometers to compute one of the following:

Unit quaternions
Roll, pitch and yaw angles
Rotation matrix

2 The translational motion observer (TMO) estimates position, linear velocity and linear
acceleration (specific force). Typical update rate is 1-10 HZ for GNSS-aided systems.

GNSS

Specific force is the non-gravitational force divided by mass

98 Feedback interconnection of the attitude and TMO observers.

Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Nonlinear Attitude Estimation

Feedback interconnection where the specific force fn is

estimated. Important for highly accelerated vehicles.

Reference vectors (BODY): Euler angles

Magnetic field: mb Quaternions on the unit sphere
Acceleration/specific force: fb Rotation matrix on SO(3)
Attitude
Optical flow: vb
Observer

Reference vectors (NED):

Magnetic field: mn
Acceleration/specific force: fn -gn Static approximation for non-accelerated vehicles

99 GNSS speed: vn Specific force is the non-gravitational force divided by mass

Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Multiplicative and Generalized Extended
Kalman Filters (MEKF and GEKF)
A special form of the EKF constructed using a
multiplicative quaternion error is often used for
attitude estimation. This is referred to as the
multiplicative extended Kalman filter (MEKF).

Lefferts, E. J., Markley, F. L., and Shuster, M. D. (1982).

Kalman Filtering for Spacecraft Attitude Estimation,
Journal of Guidance, Control, and Dynamics, Vol. 5, No. 5,
pp. 417429. 614.

The MEKF has been modified to be frame

consistent when estimating the gyro bias. This
representation is called the geometric extended
Kalman filter (GEKF).

Michael Andrle, John L. Crassidis (2015).

Attitude Estimation Employing Common Frame Error
Representations. Journal of Guidance, Control and
Dynamics, Vol.38: 1614-1624.

The main drawback of the MEKF/GEKF solutions

are that they are computational expensive and that
only local stability results are available.

100
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Nonlinear Attitude Observer

Quaternion representation Rotation matrix representation

Injection term: Convert estimates to be on SO(3)

Injection term based on compass and specific force

measurements, alternatively other reference vectors.

Quaternions give a semiglobal result, while estimation of the 9 elements in the rotation matrix gives GES.

________________________________________________

Mahoney, R., Hamel , T. and Pflimlin, J.M. (2008). Nonlinear complementary filters on the special orthogonal group.
IEEE Trans. Automatic Control 53(5), 12032018
Grip, T.H., Fossen, T.I, Johansen, T.A. and Saberi, A.(2012). Attitude estimation using biased gyro and vector
measurements with time-varying reference vectors. IEEE Trans. Automatic Control 57(5), 13321338
Grip, H. F., T. I. Fossen, T. A. Johansen and A. Saberi (2015). Globally Exponentially Stable Attitude and Gyro Bias
Estimation with Application to GNSS/INS Integration. Automatica. Volume 51, Pages 158166.
Grip, T.H., Fossen, T.I, Johansen, T.A. and Saberi, A.(2013). Nonlinear Observer for GNSS-Aided Inertial Navigation with
101 Quaternion-Based Attitude Estimation. In Proc. of the American Control Conference, 272279
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 The Attitude Estimation Problem: Remarks
on Global Stability on SO(3)
Euler angle representation
It is impossible to obtain global results when using Euler angles due to the representation
singularity.

Unit Quaternion representation

The quaternion attitude observer results in semiglobal stability
(it is not global due to several equilibria).
However, it is possible to achieve global results through introduction of discontinuities
to avoid the topological obstruction and thus get one equilibrium point.

Rotation matrix representation

When estimating the 9 elements of the matrix R (over-parameterized estimation problem)
the origin is GES on R3x3.
The SO(3) property cannot be forced on the system structural limitation of SO(3) since this
kill the observer7convergence properties. Hence, the estimates are on R3x3 and not on SO(3).

BUT, the estimate of R on R3x3 converge to SO(3) asymptotically. Even better, we can convert the
estimates to SO(3) at each time sample to improve transient behavior.

102
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Core Algorithm for Nonlinear Strapdown
INS

103
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 Integrated Navigation
When integrating acceleration and angular rates bias
the solutions will drift due to measurement noise position
accelero-
and bias terms. An integrated navigation system meters
is a navigation system, which is aided by one or
more position reference (PosRef) systems such bias
as: angle
gyros
GNSS (GPS, Glonass, Galileo, BeiDou)
Hydro-acoustic positioning reference (HPR)
Inertial measurement
systems unit (IMU)
Machine vision (optical camera)
The resulting system is a feedback interconnection of two observers for attitude and translational motions

Dead reckoning is referred to as the case when the PosRef systems fail and position and linear velocity are
104 predicted using the observers without PosRef updates.
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)
 GES Attitude and TMO Observers
Rotation Matrix Representation

Attitude Observer Translational Motion Observer (NED)

Nonlinear Injection Term

Grip, H. F., T. I. Fossen, T. A. Johansen and A. Saberi. Attitude Estimation Using Biased Gyro and Vector Measurements
with Time-Varying Reference Vector. IEEE Transactions on Automatic Control, Vol. 57, No. 5, pp. 1332-1338, May 2012
105 Grip, H. F., T. I. Fossen, T. A. Johansen and A. Saberi. A Nonlinear Observer for Integration of GNSS and IMU Measurements with
GyroLecture Notes TTK Proc.
Bias Estimation, 4190 Guidance and Control
of the ACC12, of Vehicles
Montreal, (T. I.24-27
Canada, Fossen)
March 2012.
 Semiglobal Exponential Stable Attitude and TMO
Observers Unit Quaternion Representation
Translational Motion Observer (ECEF)

Attitude Observer

Nonlinear Injection Term

Grip, H. F., T. I. Fossen, T. A. Johansen and A. Saberi. Globally Exponentially Stable Attitude and Gyro Bias Estimation
with Application to GNSS/INS Integration. Automatica. Volume 51, January 2015, Pages 158166.
106 Grip, H. F., T. I. Fossen, T. A. Johansen and A. Saberi. Nonlinear Observer for GNSS-Aided Inertial Navigation with Quaternion-
Lecture
Based Notes
Attitude TTK 4190 Proc.
Estimation. Guidance and Control
of ACC13, of VehiclesDC,
Washington (T. I. Fossen)
17-19 June.
 Validation of INS/GNSS Nonlinear Observer
against EKF
EKF: Extended Kalman filter implementation

NLO-Mag: magnetometer is used as attitude

reference vector

NLO-Vel: assumes that heading and course to

coincide (avoid compass) when computing the
velocity reference vectors

Hansen, J. M., J. Rohc, M. ipo, T. A. Johansen and T. I. Fossen (2016). Validation and Experimental Testing of
107 Observers for Robust GNSS-Aided Inertial Navigation. In "Recent Advances in Robotic Systems. (G. Wang, Ed.),
InTech,
Lecture Vienna.
Notes TTK<Open Access: http://intechweb.org/>.
4190 Guidance and Control of Vehicles (T.ISBN 978-953-51-4767-1
I. Fossen)
 Validation of INS/GNSS Nonlinear
Observer against EKF

Hansen, J. M., J. Rohc, M. ipo, T. A. Johansen and T. I. Fossen (2016). Validation and Experimental Testing of
108 Observers for Robust GNSS-Aided Inertial Navigation. In "Recent Advances in Robotic Systems. (G. Wang, Ed.),
InTech,
Lecture Vienna.
Notes TTK<Open Access: http://intechweb.org/>.
4190 Guidance and Control of Vehicles (T.ISBN 978-953-51-4767-1
I. Fossen)
 Validation of INS/GNSS Nonlinear
Observer against EKF

Hansen, J. M., J. Rohc, M. ipo, T. A. Johansen and T. I. Fossen (2016). Validation and Experimental Testing of
109 Observers for Robust GNSS-Aided Inertial Navigation. In "Recent Advances in Robotic Systems. (G. Wang, Ed.),
InTech,
Lecture Vienna.
Notes TTK<Open Access: http://intechweb.org/>.
4190 Guidance and Control of Vehicles (T.ISBN 978-953-51-4767-1
I. Fossen)