Application Of the Kalan Filter
in marine navigation
Prof..ir.J.A. Spaans.
Rapportnr. 758-P
Congress of the International
Association of Institutes of
Navigation, February 1988,
Sydney, Austrálie.
Deift University of Technology Ship Hydromechanics Laboratory Mekelweg 2
2628 CD Deift The Netherlands Phone 015 - 7868 82
APPLICATION OF THE KAI}IAN FILTER IN MARINE NAVIGATION
Pràf. jr. J.A. Spaans., Department of Maritime
Technology,,
sectionilydronautics, Deift University of Technology..
The Kalman filter ha's found many applications in navigationsystems. Operators have often problems however in understanding what goes on in the algorithm and what the effect is of fIlter settings. After a general introduction this paper gives
a tutorial review of a simple and robust navigation filter of
which the performance can be understood and influenced by the
operator. The filter has been operational for several years. in many appIcations of offshore surveys and has proved its value by
the increase 'of precision and reliability . of the navigation
performance. 1. Introduction
A Kalman filter is. an optimal recursive algorithm that processes measurements to derive a "minimtim variance estimate" of the system
state using the. information of:
- system equations - system noise - observations - observation noise - initial conditions
Le.t us suppose a:s introduction' that 'a scalar quantity x is estimated by the system equations as z.1 with variance a whereas the 'observed value of x is z2 with variance a.. An' unbiased estimate of x will be
k1 z1 +
(1-k1) z2where k1 is' the wèightfactor for
z1.
Minimizing the variance 2of as function of k1 will give
1 1
-
2z1 + i z2.)
where a2 follows from
-The relation (3) can be visualized in figure 1, where it can be seen
that
a
is always smaller than each of the individual standarddeviations a and
02.
Similar expressions as above will be found for vector quantities form the Kalman filter concept.fig 1 Starxlard deviatiana of' t derived fran b independent thnervat.iena with stndard deviatims Uj end 0.2,.
2. The state vector
For the' design of a Kalman fil:ter it is necessary to vector containing elements which descrIbe the state sufficiently For simple navigation purposes using
system only, the state vector
'
( A 1N VE) is used, where (, A) is lat,. long, and (Vli, VE) the speedcomponents. In a local Cartesian grid with X-axis
( x y * ' )
+1
2 02 define a state of the system a positioningnorth and east east oriented
can be 'used, where the relation with lat. long, is given by
y(t)+Att
I
(t + at)
"° 180 M-I A(t + at) J -
A0x(t), + at *
r
with M' the local, radius of the meridian,, r the local, radius o,f the
parallel and (0,0) in .(o, Ao). . '
To improve the performance of this simple navigation system: the positioning system. is integrated with the gyro compass.. The drift angle dr - defined as the difference angle between groundcourse C and
gyr.o reading b with tan C - is added to the state vector
x' (x
y *
r dr) (7)When a dual axis (doppler) log- with readings u and v representing forward andy lateral speed through the water or over the ground - is
additionaly integrated in the system, the errors du and dv are added
to the state vector ' .
x =(xyxydrdudv)
(8)3. 'System equations
in the Kalman filter it information regarding the
from a prediction model. The model equations should rep'resent physical reality as close as poèsible; not modelled system behaviour is represented as system noise. The et of stochastic lineair differential equations is noted as
* (t) F x('t) + Cw(t) (9)
where x(t) is the state vector, w(t) the noise vector,, F the state transition matrix and 'C the noise transition matrix
The set of adjoined stochastic difference equations reads'
xic+i xic+
r
with 't and 1' ' 'Z'(r) C d r 0When a vessel proceed's with constant groundcourse and constant ground
speed with unknown zero men acccelerations, equation (9) reads in
the local grid
tk+i -
tic, exp (F.t) I + F tt + oI
2.X2 2X2 oio
2X2 2X2I
x y x yis assumed that we do not only gather
state vector' from observations but 'also
-2-F t2 + 0 2X2I
2X2 ), i: ax ayThe set of stochastic difference equations then reads:
('6) (10) (12) (13) x 1; 0
at
o x ½at2 0 y 0 1 0 At y + 0 ½At2 -' X 0 0 1 0 0 : ay ' (14) ' 0 0 0 1 0 At 'Only the first term on the right hand siide is used for prediction:.
,k
(15)
where the first index is the real time and the second index is the time where the last observations were processed in the algorithm.
The second term rwic on the right hand side of (14) is used to predict the uncertainty in the state vector, see section 4. The stochastic sequences ax and ay are taken as equal zero mean
stationary Markov sequences ak';
therefore
rt (½At2 ½At2At At)
andWk ak.
Integration of positioning system with gyro and log will give:
where w G - , the zero mean stochastic sequence of the drift, rate
and q is the zero mean stochastic sequence of the logdrift.
In all cases the algorithm has to 'be initialised by an initial state vector Xo,o either manually or with default values derived from
observed positions.
In a multipurpose navigation software package for different sensor
configurations, equations (16)
are used with options:- positioning only
- positioning with gyro.
- positioning with gyro and single axis log - positionIng with gyro and dual axis log
The rows and colununs which are not used 'for 'a certain option are deleted.
4. Precision of the predicted state vector
The precision of the predicted' state vector is indicated by the covàriance matrix Pk-fl.k which follows from (10) using Gauss'
propagation.
law for errors:Pk1.k + r Qk rt (17)
where Qk is the covariance matrix defined by
t. 2 2 2
Qk -
E(wic Vk) - diag (a ,
a ,a
)a W q
Depending On- the option, one, two or all three elements are used. Relation (17) is initialised with Po,o either manual Or by default
values. The values in Qk should indicate the growth in uncertainty of the' elements of the state vector. The perator has direct
influence
on this. by selecting the. "filtersettings". in the. Kalmanfilter developed by the section Hydronautics of
Delft University incooperation with Intersite Surveys B.V. in Haarlem Holland, the operator can choose between filtersettings C. to 9[lJ. in the zero
setting the filter is off and the "raw" observations are used for, navigation. x '
y
'1
0 0 1 At 0 0 At 0 0 0 00'
0'x
y 1sAt2, 0 00'
0(16)
0 0 1 0 0 0 0*
At 0 0 a 0 0 0 1 0 0 0 At 0 0 wdr
0 0 0 0I
0 0 dr o At 0 q du 0 0 0 0 0 1 0 clii o o At dv k+L 0 0 0 0 0 0 1 dv 0 0At.
tan h
-When A1a2
Uxy WhenalsO
"2
-The 95 % ellipse 2.45. In figure position transfer A1 -a2
then take tan
of z:
R-E.(vv)
.'2) k.k clxy k+I .k (20) A2- o2
a'2 0 then the standard ellipse is a circle. i'. found by multiplying both axes with a factor
2 the increase of the standard ellipse by the
over At is shown.
5.. The observation model
To be able to process the observations at time tit+i in the Kalman
filter three requirements have to be fulfilled.
(1) select the observations
z1, 'z2,
--- Zn
(ii) determine the relation between observation and state vector
z h(.x) + v h(x) + H(Ax)
+ V
8h where 'H
-('iii) determine the covariance matrix for the observation errors
(22) To keep the operator in feeling, with the performance. of the system the following procedure is used,. For the cycle time At of the system
- usually the cycle time' of the observed position update - the
maximum and minimum possible values of the increase n error of the predicted position are estimated, depending on operation, vessel and environmental conditions. These values are reduced to mean
accelerations over At and divided by three to reduce to standard
deviations.. (The increase in P by c and is then neglected).
The maximum value for Ca is substituted in Qk for filtersetting 1 and
the minimum value for setting 9'; for the other filtersettings values
of Ca are derived by lineair interpolation,. A similar procedure is used for
a.
The standard deviation of the logdrift is taken constant for all flltersettings for the specified At. The operator selects the filtersetting on his assessment of the prevailing conditions.,The upper left (2x2) matrix P of Pk+1,k:
a ,cl
F-a
provides the position standard ellipse with half axes and IA2
where A and A2 are the eigenvalues of P following from
A2 A.tr (F) + det (F) = 0 (19)
The rection of half axis 1 follows from the associated eigenvector:
figure 2 Increase of staixiard ellipse by
error In predict1ci
.1.
ad(i) Instead
of
processing the range,, range difference, direction or doppler observations in (22) it is recommended to process these observations in a separate program and use the obtained positon coordinates as observations z1 andz2.
The Kalman filter program can be used in that case for a whole range of positioning systems. The positioning program. computes the observed position fromlinearised equations with the first two elements from +1,kas
provisional coordinates. When tx is the. correction vector on the provisional coordinates, A is the design matrix, b s a vector
containing observed minus computed observations and r contains the observation errors, the linearized equations read
A*=b+r
(23)with a least squares solution (for more. than 2 observations)
(AAY' AJb
. (24)wher,e W' is the a-.priori error covariance matrix for the
observations. The residual Vector is t
A* - b
(25)The associated covariance. matrices are
Pi (A'.JA) and Pr W APi At
(26) (27)
With (25) and (27) .the observations are tested on g.ross errors by the random variable for each observation (4 or. more observations)
ri
wi
-
. (28)O'ri
which. should stay within the critical values -1.96 and 1.96 (95 %). Observations with wi outside that interval are rejected and the position is recalculated without .the rej.ec.ted observation(s).
To adapt the covariance matrix for the observations W 1 for
variable conditions, the variance factor s2 for each fix is computed _ tWf
n- 2
(29)where ti is the number of observations.
With a 4-LOP position fix the variance factor r2 is on-line adapted by
d+
--i (s -, a) (30)FOr a 3-LOP fix .the factor should be. i---, see [2].
The observation error covariance. inatri°°s on-line adapted with
-j
2 1Wk+1 o1v4-1 Wk
in each of the .defined options from section 3., the first two elements of z contain the position coordinates.
Fur.ther z3 is the. gyro reading, z4 the forward speed log reading
and
z5
the lateral speed log reading.ad(ii) hen the gyroreading is included:
z3 arctan ( ) + £ir -dr + v3 (32)
y
- 0,1,2 depending on signs and r
(31)
-5-From the upper left (2x2) matrix of PIci-1,1+i the position standard ellipse and/or 95-% ellipse can be derived, seefigure 3.
(36)
(41)
For the, Iogredings:
z4 u - x sin h +
'
cos - du+ v4
(33)
z5 v x cos h - ' sin h - dv + v5
(34)
where h arctan () + iir - dr (35)
y
For state vector (:8> the H matrix will then read;
l
0 0 0 0 0 0 oi
o 0 0 0 0 H o o -1 0 0 0 0 a a4 a43 l 0 0 0 a53 58 a55 0-i
az3
y
with 83.3 + 2 , 843 sin h +
'
(* cosh - ' sinh)/(*2 +and so on. Depending on the selected option associated rows and colujnms are used/deleted.
ad(iii) The cOvariance matrix R for the error
vector v of
the observation vector z contains the computed position error covariance matrix (26) in the left (2x2) upper corner. Depending on the option,the assumed variances for the logreading, forward speed reading and
lateral speed reading are placed on the
main
diagonal of R with zeroes else.6. The filter equations
After the computation of
tc+1,kand Pk1,k with (15), (17) and input of the
observations z with covariance matrix R, the optimal state vector is found in the iineair case by minimizing the following object function J by variation of x:
J .=½
:[ CX - k+1,k)tPifi,k (x -
kf1.k) +(zHx)tR1
(z-Hx)] (37)In the non-lineair case we fInd, see [2], [3];
+1,k+1 = k+1k + K(ziv.-i
- h(k1,k)
) (38)t
1where
K Pk+1'k H(H Pk+1,kH + R)
(39)
The covariance matrix for
+ii.is
Pk+1,k+1= (I - KH) Pk+1,k
(40)
The term K (zk+1
- h (xi4-1,k)
)is the correction on the predicted state
vector, where K is the Kalman gain and h
(*k+1.k)contains the predicted
observations:
k,k + t*1c,k ; h2 yi +
t
r k,kh3
arc tan (
) + in - dric.icy
k,kh4
sin h'3, +
vk,k cos h3
-dukk
figure 3 Predicted, observedned filtered positicn
with associated stawiard eLUjes.
-7-R(2X2)
kl.l.+i (2X2)
I,+I.kt
Pk+Ik(2X2)
When an optimal position has to be derived from two positioning
systems only, giving positions x1 and x2 with covariance matrices R1 and R2 the Kalman gain K, can be eliminated from (38), (39) and (40)
giving, see [2]: l
nl
ni
L
Lj
+L2
R (R1' x1 + R;1 x2). (43)
which are similar expressions as found earlier for the scalar case in (2.) and. (3).. When two independent positioning systems are used,
the combined result can be used as input in the observation' vector z of the navigation Kalman filter.
7. App1ication
(1) Navigating with a positioning system only the Kalman fIlter model' assumes constant speed and course and will therefore introduce
errors when speed and/or course is changed.
This can be seen in figure 4 where the filtered track lies outside the 'observed (adjuste'd) track in the curve.. The observed position is obtained in the' simulation frám three ranges, with standard deviations of 5 m each.
Integrating the gyrocompass in the system will improve the performance, see figure 5. Adding a dual . axis dopplerlog will
improve the system performance significantly which can be seen in
figure 6. Additional data from the rUn in figure 6 is given in
figure 7 [:1]. Figure 8 shows' the output of operational raw data and filtered data in skywave conditions for ilyperfix.
(ii) An important feature of the Kalman filter is, that it is not necessary to collect all observations for the position computation at one instant of time. This is of major importance. in systems where the observations are collected sequentially by one receiverchannel from different beacons, which is the case in single channel CPS
receivers sUch as the Rockwell Collins Navcore I, see [4] and in long baseline, under water accoustic navigation systems.
The section Hydronautics of Delf.t University developed in cooperation with Intersite Surveys BV in Haarlem Holland a
processing package where' after each cycle the position of the accoustic transducer is updated using one range measurement Z in the observation equation (22).
L.A. delleyer (1984)
Ka]iin Filter alicati in sea surveying, Graduatict, thesis, Dept. of taritIim Technology, secticn Navigatics; DeLft Urilveraity ofTedwlcgy
Spaans, l.A. (1987)
BetrciM,aarheid, nauc4ceurigheid an precisie van na gatiasetlodiekan retort 64(, ip I,dranechanics Laboratory DeJIt thiversity of Technology '
Aederstal B.D.D. arid wore .7.B. (1979)
C,tImal filtering, Prantice Hail 1919
(4) G. Krishnamrt.i, Sit. Harshberger, T.N. 5iIth
.0-0
fig. 4 True track, thaerved (edjusted) track awl filtered track using sitiaJng aysten cmly' in the KaIIn filter
IIRAC coCrIcuiAIpo.i
- 10111 10.00
-
ascooro IAA0--- r.ui'(ACD IIAC
fig. 5 Kaiiran filter perfonnance, using .psiticn1n aystu E gyro calçasa
IRUCO
I OIICIOCO I0(0 0---- IICIC000 bAd
ICAtC I', 1.000
fig. 6 Kalan- filter perfonnance using poaiticrd.ng syston, gyro caiaZS &xi dual, axis 10g.
IC IRcC
-.
JJ$I(5 ,qr40--- r.tIrro RAC
II]
.11 IIIC I - I LIIC niiio orfUi) I ' II.,---
e-:
U I - I -II I -I.
LIIC Ictc haL
I Corrected 4 Filtersetting 2 FlxtntcrVal 50 sec Speed ±4hn Transverse mercator Cent. mer.
/
4 'Intended track/
-
,/''Corrected'V
- LoneSIlP _Lanestip PosUioning: 1' flange syledis 2 I-lyperbotae hyperlix Gyro ye s. Log no 4. "flow" track filtered Irack 1' 5C11C I: ?OO IC I 1NTEflSUE SURVEYS TIlE NETUERIAHOS 6fig. 5ratia8iie1ii
filtar-perfonnmceôirin8, skyways cooditboos iasthg 1 Syledis r&ige wd 2 flypartixiiypotholae &xI- gyrocalçass.
--fig. 7 Results of slimi ticmru in --fig. 6 - stiard deviaticn cojrse - itxiard daviatiai velocity
0 .caarse error
V - veloclt.y àrror
- sq.iaro root. of trace of covarience matrix of the
filtered positbm
D idun for thaezved positicn
R - absolute sitim error
- ei.mg track sttboo error
cr - cross track ,oslL
I