• Nie Znaleziono Wyników

Application of the Kalkman Filter in marine navigation

N/A
N/A
Protected

Academic year: 2021

Share "Application of the Kalkman Filter in marine navigation"

Copied!
10
0
0

Pełen tekst

(1)

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

(2)

APPLICATION OF THE KAI}IAN FILTER IN MARINE NAVIGATION

Pràf. jr. J.A. Spaans., Department of Maritime

Technology,,

section

ilydronautics, 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) z2

where k1 is' the wèightfactor for

z1.

Minimizing the variance 2

of as function of k1 will give

1 1

-

2

z1 + 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 standard

deviations 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 positioning

north and east east oriented

(3)

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 -

A0

x(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 0

When 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 + o

I

2.X2 2X2 o

io

2X2 2X2

I

x y x y

is assumed that we do not only gather

state vector' from observations but 'also

-2-F t2 + 0 2X2

I

2X2 ), i: ax ay

The 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 '

(4)

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 ½At2

At At)

and

Wk 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 V

k) - 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. Kalman

filter developed by the section Hydronautics of

Delft University in

cooperation 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 0

0'

0

'x

y 1sAt2, 0 0

0'

0

(16)

0 0 1 0 0 0 0

*

At 0 0 a 0 0 0 1 0 0 0 At 0 0 w

dr

0 0 0 0

I

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 0

At.

(5)

tan h

-When A1

a2

Uxy When

alsO

"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.

(6)

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 and

z2.

The Kalman filter program can be used in that case for a whole range of positioning systems. The positioning program. computes the observed position from

linearised 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 1

Wk+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)

(7)

-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 o

i

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,k

and 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)t

Pifi,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

1

where

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,k

h3

arc tan (

) + in - dric.ic

y

k,k

h4

sin h'3, +

vk,k cos h3

-

dukk

(8)

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

(9)

.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 IAA

0--- 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 ,qr4

0--- r.tIrro RAC

(10)

II]

.11 IIIC I - I LIIC niiio orfUi) I ' I

I.,---

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 6

fig. 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

Cytaty

Powiązane dokumenty

 Współczynnik kompresji w standardzie MJPEG jest taki sam jak dla JPEG i wynosi około 10:1 – 15:1, natomiast dla systemu MPEG współczynnik kompresji wynosi około 30:1;

Wojtowicza jest też opracowany indeks polsko-łacińsko-grecki i indeks łacińsko-polski nazw własnych autorów cyto­ wanych oraz indeks polsko-łacińsko-grecki i

Tel est le monde de Mateo Principe, protagoniste du roman Fuffa de l’écrivain italien Ales- sandro Milito, qui ressemble à ce monde virtuel, cette forme récente d’es- capisme

It was proven that for relatively simple object and small number of particles Hybrid Kalman filters could work better than traditional Bootstrap Particle Filter.. In

[5] and Zhao and Faltinsen [2] are consistent with the present method at small deadrise angles: w i t h decreasing the deadrise angle, the discrepancy between the two solutions

In the present work, optimal vision signal estimation for an autonomous robot is presented, where vision information is obtained by appropriate shaping of the visual

The EnSSKF forecast, the model without assimilation, the EnSSKF hindcast and the measurements are shown in Figures 12, 13, and 14 for the salinity, the north velocity and the

Zestawienie sum ienne, z wyzyskaniem wszelkich szczegółów, po­ trójnego w ydania „Sztuki rym otw órczej“ (rozdział II), wykazuje, iż mię­.. IJ jest identy­