Cannon Launched Projectile Tracking Problem

Fundamentals of Kalman Filtering: A Practical Approach

9-1

Cannon Launched Projectile Tracking Problem Overview

Problem viewed in Cartesian coordinates Extended Cartesian Kalman lter Problem viewed in polar coordinates Extended polar Kalman lter Using linear decoupled polynomial Kalman lters Using linear coupled polynomial Kalman lters Robustness comparison of extended and linear coupled polynomial Kalman lters

Fundamentals of Kalman Filtering: A Practical Approach

9-2

Problem Viewed in Cartesian Coordinates

Fundamentals of Kalman Filtering: A Practical Approach

9-3

Radar Tracking Cannon Launched Projectile

y (x , y ) T T r g

! x Cannon Radar (x , y ) R R

Fundamentals of Kalman Filtering: A Practical Approach

9-4

Relevant Equations

Acceleration of projectile
xT = 0
yT = -g

Range and angle from radar to projectile

y -y ! = tan-1 xT - xR
T R

r = (xT - xR)2 + (yT - yR)2

Actual location of projectile in terms of radar parameters

xT = rcos! + xR
yT = rsin! + yR

Fundamentals of Kalman Filtering: A Practical Approach

9-5

Using Raw Radar Measurements to Estimate Projectile Position and Velocity

Recall
xT = rcos! + xR
yT = rsin! + yR

Estimated location of projectile in terms of raw radar measurements

xT = r* cos! + xR
yT = r* sin! + yR
*

Estimated velocity of projectile in terms of raw radar measurements

xTk = yTk = xTk - xTk-1 Ts yTk - yTk-1 Ts

Fundamentals of Kalman Filtering: A Practical Approach

9-6

Using Raw Measurements to Estimate Trajectory Appears to be Satisfactory

100000 3000 F/S, 45 Deg Launch !r=100 Ft, !"=.01 R

80000 Actual 60000

40000

20000

Estimated

0 0 50000 100000 150000 200000 Downrange (Ft) 250000 300000

Fundamentals of Kalman Filtering: A Practical Approach

9-7

Using Raw Measurements Yields Terrible Downrange Velocity Estimates

5000 3000 F/S, 45 Deg Launch !r=100 Ft, !"=.01 R Actual

4000

3000

2000

1000 Estimated 0 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9-8

Using Raw Measurements Also Yields Very Poor Altitude Velocity Estimates
3000 F/S, 45 Deg Launch !r=100 Ft, !"=.01 R

2000

-2000 Estimate -4000 Actual

-6000 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9-9

Error in Estimate of Downrange is Often Greater Than 1000 Ft

2000 3000 F/S, 45 Deg Launch !r=100 Ft, !"=.01 R 1000

-1000

-2000 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 10

Error in Estimate of Altitude is Often Greater Than 1000 Ft

3000 2000 1000 0 -1000 -2000 -3000 0 20 40 60 80 Time (Sec) 100 120 140 3000 F/S, 45 Deg Launch !r=100 Ft, !"=.01 R

Fundamentals of Kalman Filtering: A Practical Approach

9 - 11

Extended Cartesian Kalman Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 12

Setting up extended Kalman lter

Choice of states
xT x x= T yT yT

Model of real world

xT 0 1 xT = 0 0 yT 0 0 yT 0 0 0 0 1 0 0 0 0 0 xT 0 xT + 0 + yT 0 yT -g 0 us 0 us

Systems dynamics matrix

0 F= 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0

Continuous process noise matrix

0 0 0 0 Q(t) = 0 !s 0 0 0 0 0 0 0 0 0 !s
Fundamentals of Kalman Filtering: A Practical Approach
9 - 13

Deriving Fundamental Matrix

Recall
22 33 !(t) = I + Ft + F t + F t +... 2! 3!

Since
0 F2 = 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 = 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0

We get
1 !(t) = I + Ft = 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 + 0 0 0 0 0 1 0 0 0 0 0 0 0 0 t 1 0 0 !(t) = 0 0 0 t 1 0 0 0 0 1 0 0 0 t 1

Discrete fundamental matrix

0 !k = 0 0 0 Ts 1 0 0 0 0 1 0 0 0 Ts 1

Fundamentals of Kalman Filtering: A Practical Approach

9 - 14

Developing Measurement Matrix-1

Measurement equation is nonlinear
y -y ! = tan-1 xT - xR
T R

Linearized measurement equation

!!
*

!r*

"! "! "! "! "xT "xT "yT "yT "r "r "r "r "xT "xT "yT "yT

!xT !xT !yT !yT

v! vr

First row can be evaluated as

!! = !xT (xT - xR)*0 - (yT - yR)*1 -(yT - yR) 1 = 2 r2 (yT - yR) (xT - xR)2 1+ (xT - xR)2

!! =0 !xT

!! = !yT

(xT - xR)*1 - (yT - yR)*0 (xT - xR) 1 = r2 (yT - yR)2 (xT - xR)2 1+ (xT - xR)2

!! =0 !yT
Fundamentals of Kalman Filtering: A Practical Approach
9 - 15

Developing Measurement Matrix-2

Since
r = (xT - xR)2 + (yT - yR)2

Second row can be evaluated as

-1/2 !r 1 2 (x - x ) = [(xT - xR) + (yT - yR)2 ] 2(xT - xR) = T r R !xT 2

!r =0 !xT
-1/2 !r 1 2 (y - y ) = [(xT - xR) + (yT - yR)2 ] 2(yT - yR) = T r R !yT 2

!r =0 !yT

Measurement matrix
!! !! !! !! !xT !xT !yT !yT !r !r !r !r !xT !xT !yT !yT

H=

H=

-(yT - yR) r2 xT - xR r

0 0

xT - xR r2 yT - yR r

0 0

-(yTk - y R) Hk = r2 k xTk - xR rk

0 0

xTk - xR r2 k yTk - yR rk

0 0
Fundamentals of Kalman Filtering: A Practical Approach
9 - 16

Other Important Matrices

Measurement noise matrix
R k = E(v k v T) k
Rk = !" 0 0 !2 r
2

Discrete process noise matrix

Ts

Qk =
0

!(")Q! (")dt
Ts

Qk =

1 0 0 0
0

! 1 0 0

0 0 1 0

0 0 ! 1

0 0 0 0 0 "s 0 0 0 0 0 0 0 0 0 "s

1 ! 0 0

0 1 0 0

0 0 1 !

0 0 d! 0 1
T3 !s s 3 T2 !s s 2 0 0 T2 !s s 2 Ts !s 0 0 0 0 T3 !s s 3 T2 !s s 2 0 0 T2 !s s 2 Ts !s

Ts

!2 "s !"s !"s 0 0 "s 0 0

0 0 !"s

0 0 "s d!
Qk =

Qk =

!2 "s !"s

Fundamentals of Kalman Filtering: A Practical Approach

9 - 17

Discrete G Matrix
Filter equation
x = Fx + Gu + w

x k = !k x k-1 + G k u k-1 + K k (z k - H !k x k-1 - H G k u k-1)

If u constant between sampling instants discrete G matrix from

Ts

Gk =
0

!(")G d "

Recall
xT 0 1 xT = 0 0 yT 0 0 yT 0 0 0 0 1 0 0 0 0 0 xT 0 xT + 0 + yT 0 yT -g 0 us 0 us
0 G = Gu = 0 0 -g

Therefore
0
Ts

Gk =
0

1 0 0 0

! 1 0 0

0 0 1 0

0 0 ! 1

0 0 0 d! = 0 gT2 -g - s 2 -gTs
Fundamentals of Kalman Filtering: A Practical Approach
9 - 18

Calculating Projected States

Recall
x k = !k x k-1 + G k u k-1 + K k (z k - H !k x k-1 - H G k u k-1)

Since fundamental matrix exact projected state is

x k = !k x k -1 + G k uk -1

Substitution yields
0 1 = 0 0 0 Ts 1 0 0 0 0 1 0 0 0 0 x k -1 + Ts gT2 1 - s 2 -gTs

xk

Multiplying out terms yields

xTk = xTk-1 + Ts xTk-1 xTk = xTk-1 yTk = yTk-1 + Ts yTk-1 -.5gT2 s yTk = yTk-1 - gTs

Fundamentals of Kalman Filtering: A Practical Approach

9 - 19

Extended Kalman Filtering Equations

Recall
x k = !k x k-1 + G k u k-1 + K k (z k - H !k x k-1 - H G k u k-1)

Nonlinear Calculate projected measurements

! k = tan-1 yTk-1 - yR xTk-1 - xR

rk = (xTk-1 - xR)2 + (yTk-1 - yR)2

Filtering equations
xTk = xTk + K 11 k(! k - ! k) + K 12 k(r* - rk ) k xTk = xTk + K 21 k(! k - ! k ) + K 22 k(r* - rk ) k yTk = yTk +
* K 31 k(! k * * *

xTk = xTk-1 + Ts xTk-1

Where

xTk = xTk-1 yTk = yTk-1 + Ts yTk-1 -.5gT2 s yTk = yTk-1 - gTs

- ! k) +

K 32 k(r* k

- rk )

yTk = yTk + K 41 k(! k - ! k ) + K 42 k(r* - rk ) k

Note that linearized measurement is not use here but is only used in the Riccati equation
Fundamentals of Kalman Filtering: A Practical Approach
9 - 20

MATLAB Cartesian Extended Kalman Filter-1

TS=1.; ORDER=4; PHIS=0.; SIGTH=.01; SIGR=100.; VT=3000.; GAMDEG=45.; G=32.2; XT=0.; YT=0.; XTD=VT*cos(GAMDEG/57.3); YTD=VT*sin(GAMDEG/57.3); XR=100000.; YR=0.; T=0.; S=0.; H=.001; PHI=[1 TS 0 0;0 1 0 0;0 0 1 TS;0 0 0 1]; P=[1000.^2 0 0 0;0 100.^2 0 0;0 0 1000.^2 0;0 0 0 100.^2]; IDNP=eye(ORDER); Q=zeros(ORDER,ORDER); TS2=TS*TS; TS3=TS2*TS; Q(1,1)=PHIS*TS3/3.; Q(1,2)=PHIS*TS2/2.; Q(2,1)=Q(1,2); Q(2,2)=PHIS*TS; Q(3,3)=Q(1,1); Q(3,4)=Q(1,2); Q(4,3)=Q(3,4); Q(4,4)=Q(2,2); PHIT=PHI'; RMAT=[SIGTH^2 0;0 SIGR^2]; XTH=XT+1000.; XTDH=XTD-100.; YTH=YT-1000.; YTDH=YTD+100.; count=0;

Actual initial states

Fundamental and initial covariance matrices

Discrete process noise matrix

Measurement noise matrix

Initial state estimates

Fundamentals of Kalman Filtering: A Practical Approach
9 - 21

MATLAB Cartesian Extended Kalman Filter-2

while YT>=0. XTOLD=XT; XTDOLD=XTD; YTOLD=YT; YTDOLD=YTD; XTDD=0.; YTDD=-G; XT=XT+H*XTD; XTD=XTD+H*XTDD; YT=YT+H*YTD; YTD=YTD+H*YTDD; T=T+H; XTDD=0.; YTDD=-G; XT=.5*(XTOLD+XT+H*XTD); XTD=.5*(XTDOLD+XTD+H*XTDD); YT=.5*(YTOLD+YT+H*YTD); YTD=.5*(YTDOLD+YTD+H*YTDD); S=S+H; if S>=(TS-.00001) S=0.; XTB=XTH+TS*XTDH; XTDB=XTDH; YTB=YTH+TS*YTDH-.5*G*TS*TS; YTDB=YTDH-G*TS; RTB=sqrt((XTB-XR)^2+(YTB-YR)^2); HMAT(1,1)=-(YTB-YR)/RTB^2; HMAT(1,2)=0.; HMAT(1,3)=(XTB-XR)/RTB^2; HMAT(1,4)=0.; HMAT(2,1)=(XTB-XR)/RTB; HMAT(2,2)=0.; HMAT(2,3)=(YTB-YR)/RTB; HMAT(2,4)=0.; HT=HMAT'; PHIP=PHI*P; PHIPPHIT=PHIP*PHIT; M=PHIPPHIT+Q; HM=HMAT*M; HMHT=HM*HT; HMHTR=HMHT+RMAT; HMHTRINV=inv(HMHTR); MHT=M*HT; K=MHT*HMHTRINV; KH=K*HMAT; IKH=IDNP-KH; P=IKH*M;

Second-order Runge-Kutta integration of projectile differential equations

Linearized measurement matrix

Riccati equations
Fundamentals of Kalman Filtering: A Practical Approach
9 - 22

MATLAB Cartesian Extended Kalman Filter-3

THETNOISE=SIGTH*randn; RTNOISE=SIGR*randn; THET=atan2((YT-YR),(XT-XR)); RT=sqrt((XT-XR)^2+(YT-YR)^2); THETMEAS=THET+THETNOISE; RTMEAS=RT+RTNOISE; THETB=atan2((YTB-YR),(XTB-XR)); RTB=sqrt((XTB-XR)^2+(YTB-YR)^2); RES1=THETMEAS-THETB; RES2=RTMEAS-RTB; XTH=XTB+K(1,1)*RES1+K(1,2)*RES2; XTDH=XTDB+K(2,1)*RES1+K(2,2)*RES2; YTH=YTB+K(3,1)*RES1+K(3,2)*RES2; YTDH=YTDB+K(4,1)*RES1+K(4,2)*RES2; ERRX=XT-XTH; SP11=sqrt(P(1,1)); ERRXD=XTD-XTDH; SP22=sqrt(P(2,2)); ERRY=YT-YTH; SP33=sqrt(P(3,3)); ERRYD=YTD-YTDH; SP44=sqrt(P(4,4)); SP11P=-SP11; SP22P=-SP22; SP33P=-SP33; SP44P=-SP44; count=count+1; ArrayT(count)=T; ArrayXT(count)=XT; ArrayXTH(count)=XTH; ArrayYT(count)=YT; ArrayYTH(count)=YTH; ArrayXTD(count)=XTD; ArrayXTDH(count)=XTDH; ArrayYTD(count)=YTD; ArrayYTDH(count)=YTDH; ArrayERRX(count)=ERRX; ArraySP11(count)=SP11; ArraySP11P(count)=SP11P; ArrayERRXD(count)=ERRXD; ArraySP22(count)=SP22; ArraySP22P(count)=SP22P; ArrayERRY(count)=ERRY; ArraySP33(count)=SP33; ArraySP33P(count)=SP33P; ArrayERRYD(count)=ERRYD; ArraySP44(count)=SP44; ArraySP44P(count)=SP44P;

Noisy radar measurements Extended Kalman lter

Saving data as arrays for plotting and writing to les

end

end

Fundamentals of Kalman Filtering: A Practical Approach

9 - 23

Filtering Dramatically Reduces Position Error Over Using Raw Measurements

1500 1000 500 0 -500 -1000 No Filter -1500 0 20 40 60 80 Time (Sec) 100 120 140 Extended Kalman Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 24

Extended Cartesian Kalman Filters Projectiles Downrange Estimates Appear to Agree With Theory
Simulation 150 Theory 100 50 0 -50 -100 Theory -150 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 25

Extended Cartesian Kalman Filters Projectiles Downrange Velocity Estimates Appear to Agree With Theory
20

10

Theory

-10 Simulation -20 0 20 40

Theory

60 80 Time (Sec)

100

120

140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 26

Extended Cartesian Kalman Filters Projectiles Altitude Estimates Appear to Agree With Theory
600 400 200 0 -200 -400 -600 0 20 40 60 80 Time (Sec) 100 120 140 Theory Simulation Theory

Fundamentals of Kalman Filtering: A Practical Approach

9 - 27

Extended Cartesian Kalman Filters Projectiles Altitude Velocity Estimates Appear to Agree With Theory
20 Simulation

10

Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 28

Errors in Estimate of Velocity Increase With Increasing Process Noise

100 !s=1000 !s=100 !s=10 !s=0

80

60

40

20

0 0 20 40 60 80 Time (Sec) 100 120 140

T3 !s s 3 T2 !s s 2 0 0

T2!s s 2 Ts!s 0 0

0 0 T3 !s s 3 T2 !s s 2

0 0 T2 !s s 2 Ts!s
Fundamentals of Kalman Filtering: A Practical Approach
9 - 29

Qk =

Polar Coordinate System

Fundamentals of Kalman Filtering: A Practical Approach

9 - 30

Develop Polar Equations

Recall
y -y ! = tan-1 xT - xR
T R

r = (xT - xR)2 + (yT - yR)2

Taking rst derivative of angle

!= 1 yT - yR 1 + x -x T R
2

xT - xR yT - yT - yR xT xT - xR 2

Simplication yields
!= xT - xR yT - yT - yR xT r2

Take rst derivative of range

r=1 2 xT - xR 2 + yT - yR 2
-1/2

2 xT - xR xT + 2 yT - yR yT

Which simplies to
r= xT - xR xT + yT - y R yT r
Fundamentals of Kalman Filtering: A Practical Approach
9 - 31

Developing Angular Acceleration Formula

Taking second derivative of angle
!= r2 xTyT + xT - xR yT - xTyT - yT - yR xT - xT - xR yT - yT - yR yT 2rr r4

Which simplies to
!= xT - xR yT - yT - yR xT - 2rr! r2

Recognizing that
xT = 0
yT = -g

cos ! =
sin ! =

xT - xR r
yT - yR r

We get
!= -g cos ! - 2r! r
Fundamentals of Kalman Filtering: A Practical Approach
9 - 32

Developing Range Acceleration Formula

Taking second derivative of range
r= r xTxT + xT - xR xT + yTyT + yT - yR yT - xT - xR xT + yT - yR yT r r2

Which simplies to
r2! - gr sin ! r= r
2

Fundamentals of Kalman Filtering: A Practical Approach

9 - 33

Differential Equation Summary

Polar differential equations
!= -g cos ! - 2r! r
2

Cartesian differential equations

xT = 0
yT = -g

r2! - gr sin ! r= r

Polar initial conditions

!(0) = tan-1 yT(0) - yR xT(0) - xR

Cartesian initial conditions

xT(0), yT(0), xT(0), yT(0)

r(0) =

(xT(0) - xR)2 + (yT(0) - yR)2

xT(0) - xR yT(0) - yT(0) - yR xT(0) r(0)2

!(0) =

r(0) =

xT(0) - xR xT(0) + yT(0) - yR yT(0) r(0)

Fundamentals of Kalman Filtering: A Practical Approach

9 - 34

Going From Polar to Cartesian

y (x , y ) T T r g

! x Cannon Radar (x , y ) R R

Obtaining position
xT = r cos ! + xR
yT = r sin ! + yR

Obtaining velocity
xT = r cos ! - r ! sin !
yT = r sin ! + r ! cos !
Fundamentals of Kalman Filtering: A Practical Approach
9 - 35

FORTRAN Simulation That Compares Polar and Cartesian Differential Equations-1

IMPLICIT REAL*8(A-H,O-Z) VT=3000. GAMDEG=45. G=32.2 TS=1. XT=0. YT=0. XTD=VT*COS(GAMDEG/57.3) YTD=VT*SIN(GAMDEG/57.3) XR=100000. YR=0. OPEN(1,STATUS='UNKNOWN',FILE='DATFIL') T=0. S=0. H=.001 THET=ATAN2((YT-YR),(XT-XR)) RT=SQRT((XT-XR)**2+(YT-YR)**2) THETD=((XT-XR)*YTD-(YT-YR)*XTD)/RT**2 RTD=((XT-XR)*XTD+(YT-YR)*YTD)/RT WHILE(YT>=0.) XTOLD=XT XTDOLD=XTD YTOLD=YT YTDOLD=YTD THETOLD=THET THETDOLD=THETD RTOLD=RT RTDOLD=RTD XTDD=0. YTDD=-G THETDD=(-G*COS(THET)-2.*RTD*THETD)/RT RTDD=((RT*THETD)**2-G*RT*SIN(THET))/RT

Cartesian initial conditions

Polar initial conditions

Second-order Runge-Kutta integration

Fundamentals of Kalman Filtering: A Practical Approach

9 - 36

FORTRAN Simulation That Compares Polar and Cartesian Differential Equations-2

XT=XT+H*XTD XTD=XTD+H*XTDD YT=YT+H*YTD YTD=YTD+H*YTDD THET=THET+H*THETD THETD=THETD+H*THETDD RT=RT+H*RTD RTD=RTD+H*RTDD T=T+H XTDD=0. YTDD=-G THETDD=(-G*COS(THET)-2.*RTD*THETD)/RT RTDD=((RT*THETD)**2-G*RT*SIN(THET))/RT XT=.5*(XTOLD+XT+H*XTD) XTD=.5*(XTDOLD+XTD+H*XTDD) YT=.5*(YTOLD+YT+H*YTD) YTD=.5*(YTDOLD+YTD+H*YTDD) THET=.5*(THETOLD+THET+H*THETD) THETD=.5*(THETDOLD+THETD+H*THETDD) RT=.5*(RTOLD+RT+H*RTD) RTD=.5*(RTDOLD+RTD+H*RTDD) S=S+H IF(S>=(TS-.00001))THEN S=0. XTH=RT*COS(THET)+XR YTH=RT*SIN(THET)+YR XTDH=RTD*COS(THET)-RT*SIN(THET)*THETD YTDH=RTD*SIN(THET)+RT*COS(THET)*THETD WRITE(9,*)T,XT,XTH,YT,YTH,XTD,XTDH,YTD,YTDH WRITE(1,*)T,XT,XTH,YT,YTH,XTD,XTDH,YTD,YTDH ENDIF END DO PAUSE CLOSE(1) END

Compare polar and Cartesian

Fundamentals of Kalman Filtering: A Practical Approach

9 - 37

Polar and Cartesian Differential Equations are Identical in Position

80000

60000

40000

Polar and Cartesian

20000

0 0 50000 100000 150000 200000 Downrange, xT (Ft) 250000 300000

Fundamentals of Kalman Filtering: A Practical Approach

9 - 38

Polar and Cartesian Differential Equations are Identical in Velocity

2000

1000

xT Dot (Polar and Cartesian)

-1000

yT Dot (Polar and Cartesian)

-2000 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 39

Extended Polar Kalman Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 40

Developing Polar Extended Kalman Filter Equations

Our model of the real world is now nonlinear
!= -g cos ! - 2r! r
2

r2! - gr sin ! r= r

Linearized model of real world assuming zero process noise

"! "! !! !! !r !r = "! "! "r "! "r "! "! "! "! "! "r "! "r "! "! "r "! "r "r "r "r "r "! "r "! "r "r "r "r "r !! !! !r !r "r "! "r "! "r "r "r "r = 0 0 "! "! 1 "! "! 0 0 "! "r 0 0 "! "r 1 !! !! !r !r

Systems dynamics matrix

Fundamentals of Kalman Filtering: A Practical Approach
9 - 41

Developing Fundamental Matrix-1

Evaluating partial derivatives yields
0 gsin! r F= 0 -gcos! 0 2r! 1 -2r r 0 gcos!+ 2!r r2 0 !
2

0 -2! r 1 0

Two term Taylor series expansion

0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 + 0 1 gsin" r t 0 -gcos"t t -2r t r 0 2r"t 0 gcos"+ 2"r t r2 0 " t
2

0 -2" t r t 0

!(t) = I + F t =

Fundamentals of Kalman Filtering: A Practical Approach

9 - 42

Developing Fundamental Matrix-2

Simplication yields
1 gsin" r t 0 -gcos"t t 1 - 2r t r 0 2r"t 0 gcos"+ 2"r t r2 1 " t
2

0 -2" t r t 1

!(t) =

Or in discrete form

1 gsin" r Ts 0 -gcos"Ts

Ts 1 - 2r Ts r 0 2r"Ts

0 gcos"+ 2"r Ts r2 1 " Ts

2

0 -2" Ts r Ts 1

!k =

Fundamentals of Kalman Filtering: A Practical Approach

9 - 43

Measurement Equation is Linear in Polar System

Measurement equation
! !
*

r*

= 1 0 0 0 0 0 1 0

! r r

v! vr

Measurement matrix
H= 1 0 0 0 0 0 1 0

Discrete measurement noise matrix

R k = E(v k v T) k
Rk = !" 0 0 !2 r
2

We now are able to solve Riccati equations

Fundamentals of Kalman Filtering: A Practical Approach

9 - 44

Additional Equations For Comparing Polar and Cartesian Extended Kalman Filters-1
Recall we can derive Cartesian quantities from polar quantities
xT = r cos ! + xR
yT = r sin ! + yR
xT = r cos ! - r ! sin !
yT = r sin ! + r ! cos !

Using chain rule from calculus

!xT =
!xT =

"xT "!
"xT "!

!! +

"xT "!
"xT "!

!! +

"xT "x !r + T !r "r "r

"xT "x !r + T !r "r "r

!! +

!! +

!yT =
!yT =

"yT "!
"yT "!

!! +

"yT "!
"yT "!

!! +

"yT "y !r + T !r "r "r

"yT "y !r + T !r "r "r

!! +

!! +

Fundamentals of Kalman Filtering: A Practical Approach

9 - 45

Additional Equations For Comparing Polar and Cartesian Extended Kalman Filters-2
Evaluating partial derivatives yields
!xT = -rsin! !! + cos! !r
!xT = (-rsin!-r!cos!) !! - rsin! !! - !sin! !r + cos! !r

!yT = rcos! !! + sin! !r

!yT = (rcos!-r!sin!) !! + rcos! !! + !cos! !r + sin! !r

Or in state space form

-rsin! !xT !xT -rsin!-r!cos! = !yT rcos! !yT rcos! 0 -rsin! 0 rcos! cos! -!sin! sin! !cos! 0 cos! 0 sin! !! !! !r !r

A Relationship between covariance matrices

P CART = AP POLA T
Fundamentals of Kalman Filtering: A Practical Approach
9 - 46

Extended Polar Kalman Filter

Filtering equations
! k = ! k + K1 1k(! k - ! k ) + K1 2k(r* - rk ) k
*

! k = ! k + K 2 1k(! k - ! k ) + K2 2k(r* - rk ) k
rk= rk + K3 1k(! k - ! k ) + K 3 2k(r* - rk) k rk = rk + K 4 1k(! k - ! k) + K 4 2k(r* - rk ) k
* *

Where barred quantities are obtained by numerical integration (more computationally expensive)

Fundamentals of Kalman Filtering: A Practical Approach

9 - 47

True BASIC Polar Extended Kalman Filter-1

OPTION NOLET REM UNSAVE "DATFIL" REM UNSAVE "COVFIL" REM UNSAVE "COVFIL2" OPEN #1:NAME "DATFIL",ACCESS OUTPUT,CREATE NEW, ORGANIZATION TEXT OPEN #2:NAME "COVFIL",ACCESS OUTPUT,CREATE NEW, ORGANIZATION TEXT OPEN #3:NAME "COVFIL2",ACCESS OUTPUT,CREATE NEW, ORGANIZATION TEXT SET #1: MARGIN 1000 SET #2: MARGIN 1000 SET #3: MARGIN 1000 DIM P(4,4),M(4,4),GAIN(4,2),PHI(4,4),PHIT(4,4),PHIP(4,4) DIM Q(4,4),HMAT(2,4),HM(2,4),MHT(4,2),PHIPPHIT(4,4),AT(4,4) DIM RMAT(2,2),HMHTR(2,2),HMHTRINV(2,2),A(4,4),PNEW(4,4) DIM HMHT(2,2),HT(4,2),KH(4,4),IDNP(4,4),IKH(4,4),FTS(4,4) DIM AP(4,4) SIGTH=.01 SIGR=100. TS=1. G=32.2 VT=3000. GAMDEG=45. XT=0. YT=0. XTD=VT*COS(GAMDEG/57.3) YTD=VT*SIN(GAMDEG/57.3) XR=100000. YR=0. XTH=XT+1000. YTH=YT-1000. XTDH=XTD-100. YTDH=YTD+100. CALL ATAN2((YT-YR), (XT-XR)+.001, TH) R=SQR((XT-XR)^2+(YT-YR)^2) THD=((XT-XR)*YTD-(YT-YR)*XTD)/R^2 RD=((XT-XR)*XTD+(YT-YR)*YTD)/R CALL ATAN2((YTH-YR), (XTH-XR)+.001, THH) RH=SQR((XTH-XR)^2+(YTH-YR)^2) THDH=((XTH-XR)*YTDH-(YTH-YR)*XTDH)/RH^2 RDH=((XTH-XR)*XTDH+(YTH-YR)*YTDH)/RH ORDER=4 TF=100. T=0. S=0. H=.001 HP=.001

Initial conditions for Cartesian equations

Original initial state estimates for Cartesian Initial conditions for polar equations Initial state estimates for polar lter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 48

True BASIC Polar Extended Kalman Filter-2

MAT PHI=ZER(ORDER,ORDER) MAT P=ZER(ORDER,ORDER) MAT IDNP=IDN(ORDER,ORDER) MAT Q=ZER(ORDER,ORDER) MAT FTS=ZER(ORDER,ORDER) MAT A=ZER(ORDER,ORDER) P(1,1)=(TH-THH)^2 P(2,2)=(THD-THDH)^2 P(3,3)=(R-RH)^2 P(4,4)=(RD-RDH)^2 RMAT(1,1)=SIGTH^2 RMAT(1,2)=0. RMAT(2,1)=0. RMAT(2,2)=SIGR^2 HMAT(1,1)=1. HMAT(1,2)=0. HMAT(1,3)=0. HMAT(1,4)=0. HMAT(2,1)=0. HMAT(2,2)=0. HMAT(2,3)=1. HMAT(2,4)=0. DO WHILE YT>=0. THOLD=TH THDOLD=THD ROLD=R RDOLD=RD THDD=(-G*R*COS(TH)-2.*THD*R*RD)/R^2 RDD=(R*R*THD*THD-G*R*SIN(TH))/R TH=TH+H*THD THD=THD+H*THDD R=R+H*RD RD=RD+H*RDD T=T+H THDD=(-G*R*COS(TH)-2.*THD*R*RD)/R^2 RDD=(R*R*THD*THD-G*R*SIN(TH))/R TH=.5*(THOLD+TH+H*THD) THD=.5*(THDOLD+THD+H*THDD) R=.5*(ROLD+R+H*RD) RD=.5*(RDOLD+RD+H*RDD) S=S+H

Initial covariance matrix

Measurement noise matrix

Measurement matrix

Second-order Runge-Kutta integration of polar differential equations

Fundamentals of Kalman Filtering: A Practical Approach

9 - 49

True BASIC Polar Extended Kalman Filter-3

IF S>=(TS-.00001) THEN S=0. FTS(1,2)=1.*TS FTS(2,1)=G*SIN(THH)*TS/RH FTS(2,2)=-2.*RDH*TS/RH FTS(2,3)=(G*COS(THH)+2.*THDH*RDH)*TS/RH^2 FTS(2,4)=-2.*THDH*TS/RH FTS(3,4)=1.*TS FTS(4,1)=-G*COS(THH)*TS FTS(4,2)=2.*RH*THDH*TS FTS(4,3)=(THDH^2)*TS MAT PHI=FTS+IDNP MAT HT=TRN(HMAT) MAT PHIT=TRN(PHI) MAT PHIP=PHI*P MAT PHIPPHIT=PHIP*PHIT MAT M=PHIPPHIT+Q MAT HM=HMAT*M MAT HMHT=HM*HT MAT HMHTR=HMHT+RMAT MAT HMHTRINV=INV(HMHTR) MAT MHT=M*HT MAT GAIN=MHT*HMHTRINV MAT KH=GAIN*HMAT MAT IKH=IDNP-KH MAT P=IKH*M CALL GAUSS(THNOISE,SIGTH) CALL GAUSS(RNOISE,SIGR) CALL PROJECT(T,TS,THH,THDH,RH,RDH,THB,THDB,RB,RDB,HP) RES1=TH+THNOISE-THB RES2=R+RNOISE-RB THH=THB+GAIN(1,1)*RES1+GAIN(1,2)*RES2 THDH=THDB+GAIN(2,1)*RES1+GAIN(2,2)*RES2 RH=RB+GAIN(3,1)*RES1+GAIN(3,2)*RES2 RDH=RDB+GAIN(4,1)*RES1+GAIN(4,2)*RES2

Fundamental matrix

Riccati equations

Propagate estimates

Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 50

True BASIC Polar Extended Kalman Filter-4

ERRTH=TH-THH SP11=SQR(P(1,1)) ERRTHD=THD-THDH SP22=SQR(P(2,2)) ERRR=R-RH SP33=SQR(P(3,3)) ERRRD=RD-RDH SP44=SQR(P(4,4)) XT=R*COS(TH)+XR YT=R*SIN(TH)+YR XTD=RD*COS(TH)-R*THD*SIN(TH) YTD=RD*SIN(TH)+R*THD*COS(TH) XTH=RH*COS(THH)+XR YTH=RH*SIN(THH)+YR XTDH=RDH*COS(THH)-RH*THDH*SIN(THH) YTDH=RDH*SIN(THH)+RH*THDH*COS(THH) A(1,1)=-RH*SIN(THH) A(1,3)=COS(THH) A(2,1)=-RDH*SIN(THH)-RH*THDH*COS(THH) A(2,2)=-RH*SIN(THH) A(2,3)=-THDH*SIN(THH) A(2,4)=COS(THH) A(3,1)=RH*COS(THH) A(3,3)=SIN(THH) A(4,1)=RDH*COS(THH)-RH*SIN(THH)*THDH A(4,2)=RH*COS(THH) A(4,3)=THDH*COS(THH) A(4,4)=SIN(THH) MAT AT=TRN(A) MAT AP=A*P MAT PNEW=AP*AT ERRXT=XT-XTH SP11P=SQR(PNEW(1,1)) ERRXTD=XTD-XTDH SP22P=SQR(PNEW(2,2)) ERRYT=YT-YTH SP33P=SQR(PNEW(3,3)) ERRYTD=YTD-YTDH SP44P=SQR(PNEW(4,4)) PRINT T,R,RH,RD,RDH,TH,THH,THD,THDH PRINT #1:T,R,RH,RD,RDH,TH,THH,THD,THDH PRINT #2:T,ERRTH,SP11,-SP11,ERRTHD,SP22,-SP22,ERRR,SP33,-SP33,ERRRD,SP44,-SP44 PRINT #3:T,ERRXT,SP11P,-SP11P,ERRXTD,SP22P,-SP22P,ERRYT,SP33P,-SP33P,ERRYTD,SP44P,-

Polar errors in estimates

Actual Cartesian states

Estimated Cartesian states

Transformation matrix

Cartesian covariance matrix

Cartesian errors in estimates

SP44P END IF LOOP CLOSE #1 CLOSE #2 CLOSE #3 END

Fundamentals of Kalman Filtering: A Practical Approach

9 - 51

True BASIC Polar Extended Kalman Filter-5

SUB ATAN2 (Y, X, Z) IF X < 0 THEN IF Y < 0 THEN LET Z = ATN(Y / X) - 3.14159 ELSE LET Z = ATN(Y / X) + 3.14159 END IF ELSE LET Z = ATN(Y / X) END IF END SUB SUB PROJECT(TP,TS,THP,THDP,RP,RDP,THH,THDH,RH,RDH,HP) T=0. G=32.2 TH=THP THD=THDP R=RP RD=RDP H=HP DO WHILE T<=(TS-.0001) THDD=(-G*R*COS(TH)-2.*THD*R*RD)/R^2 RDD=(R*R*THD*THD-G*R*SIN(TH))/R THD=THD+H*THDD TH=TH+H*THD RD=RD+H*RDD R=R+H*RD T=T+H LOOP RH=R RDH=RD THH=TH THDH=THD END SUB SUB GAUSS(X,SIG) LET X=RND+RND+RND+RND+RND+RND-3 LET X=1.414*X*SIG END SUB

Arc tangent which is good in all quadrants

Propagate polar states ahead one sampling interval using Euler integration

Gaussian noise routine

Fundamentals of Kalman Filtering: A Practical Approach
9 - 52

Polar and Cartesian Extended Kalman Filters Yield Similar Results for Downrange Estimates
200 Simulation Polar Filter Theory 100

-100 Theory -200 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 53

Polar and Cartesian Extended Kalman Filters Yield Similar Results for Downrange Velocity Estimates
20 Simulation 10 Polar Filter Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 54

Polar and Cartesian Extended Kalman Filters Yield Similar Results for Altitude Estimates
600 Simulation 400 Theory 200 0 -200 -400 -600 0 20 40 60 80 Time (Sec) 100 120 140 Theory Polar Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 55

Polar and Cartesian Extended Kalman Filters Yield Similar Results for Altitude Velocity Estimates
20 Polar Filter 10

Simulation

Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 56

Error in the Estimate of Angle Indicates That Extended Polar Kalman Filter Appears to be Working Properly
0.010 Theory 0.005 Polar Filter Simulation

States
! ! r r

0.000

-0.005

Theory

-0.010 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 57

Error in the Estimate of Angle Rate Indicates That Extended Polar Kalman Filter Appears to be Working Properly
0.00010 Simulation Polar Filter

0.00005

Theory

0.00000

-0.00005 Theory

-0.00010 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 58

Error in the Estimate of Range Indicates That Extended Polar Kalman Filter Appears to be Working Properly
100 Polar Filter Theory 50

-50 Theory

-100

Simulation

-150 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 59

Error in the Estimate of Range Rate Indicates That Extended Polar Kalman Filter Appears to be Working Properly
20 Polar Filter

10

Theory

-10 Simulation -20 0 20 40 60 80 Time (Sec) 100

Theory

120

140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 60

Using Linear Decoupled Polynomial Kalman Filters

Fundamentals of Kalman Filtering: A Practical Approach

9 - 61

Making Measurement Noise Appear to be Linearly Related to States-1

In Cartesian frame model of real world is linear but measurements are nonlinear Recall
xT = rcos! + xR
yT = rsin! + yR

Find total differential from calculus

!xT = !yT = "xT "x !r + T !! = cos!!r - rsin!!! "r "! "yT "y !r + T !! = sin!!r + rcos!!! "r "!

Square both equations

!x2 = cos2 !!r2 - 2rsin!cos!!r!! + r2sin2 !!! T
2 2

!y2 = sin2!!r2 + 2rsin!cos!!r!! + r2 cos2 !!! T

Fundamentals of Kalman Filtering: A Practical Approach

9 - 62

Making Measurement Noise Appear to be Linearly Related to States-2

Taking expectations of both sides assuming range and angle are not correlated
E !x2 = cos2 !E !r2 + r2sin2 !E !! T E !y2 = sin2!E !r2 + r2 cos2 !E !! T
2 2

Since
!2 T = E !x2 x T
!2 T = E !y2 y T

!2 = E !r2 r
!" = E !"
2 2

We can say
!2 T = cos2 "!2 + r2 sin2 "!2 x r " !2 T = sin2 "!2 + r2 cos2 "!2 y r "

*We are pretending noise is on x and y rather than r and

Fundamentals of Kalman Filtering: A Practical Approach

9 - 63

Important Matrices in Downrange Channel-1

Model of real world
xT = 0 1 xT 0 0 xT + 0 xT us

Process noise matrix

Q= 0 0 0 !s

Systems dynamics matrix

F= 0 1 0 0

For this F we have already seen that

!(t) = 0 t 0 1

Discrete fundamental matrix

!k = 0 Ts 0 1

Fundamentals of Kalman Filtering: A Practical Approach

9 - 64

Important Matrices in Downrange Channel-2

Discrete process noise matrix can be found from
Ts

Qk =
0

!(")Q! (")dt

We have already seen that

Qk = !s T3 s 3 T2 s 2 T2 s 2 Ts

Due to our use of pseudonoise measurement equation is linear

x* = 1 0 T xT + v x xT

Measurement matrix
H= 1 0

Measurement noise matrix is a scalar

Rk = !2T x
!2 T = cos2 "!2 + r2 sin2 "!2 x r "

From radar
Fundamentals of Kalman Filtering: A Practical Approach
9 - 65

Downrange Linear Polynomial Kalman Filter

Recall
x k = !k x k-1 + K k (z k - H !k x k-1)

Substituting matrices yields

xTk xTk = 1 Ts 0 1 xTk-1 xTk-1 + K1k K2k x* k - 1 0 T 1 Ts 0 1 xTk-1 xTk-1

Multiplying terms out yields

xTk = xTk-1 + TsxTk-1 + K1 k(x* k - xTk-1 - TsxTk-1) T xTk = xTk-1 + K 2 k(x* k - xTk-1 - Ts xTk-1) T

Fundamentals of Kalman Filtering: A Practical Approach

9 - 66

Important Matrices in Altitude Channel

Model of real world
x = Fx + Gu + w

yT = 0 1 yT 0 0

yT + 0 + 0 yT -g us

Disturbance or control vector

G= 0 -g

Finding discrete disturbance vector

Ts Ts

Gk =
0

!Gd" =
0

0 " 0 1

0 d" = -g

-.5T 2g s -Tsg

Measurement equation
y* = 1 0 T yT + v y yT

Measurement noise matrix is scalar

Rk = !2T y
!2 T = sin2 "!2 + r2 cos2 "!2 y r "

*Matrices for Riccati equations are identical in both channels except for measurement noise

Fundamentals of Kalman Filtering: A Practical Approach

9 - 67

Altitude Linear Polynomial Kalman Filter

Recall
x k = !k x k-1 + G k u k-1 + K k (z k - H !k x k-1 - H G k u k-1)

Substituting matrices yields

yTk yTk = 1 Ts 0 1 yTk-1 yTk-1 .5gT2 s gTs + K1 k K2 k y* k - 1 0 T 1 Ts 0 1 yTk-1 yTk-1 + 1 0 .5gT2 s gTs

Multiplying out terms

yTk = yTk-1 + TsyTk-1 -.5gT2 + K 1k(x*k - xTk-1 - Ts xTk-1 +.5gT2 ) s s T yTk = yTk-1 -gTs + K2 k(x* k - xTk-1 - TsxTk-1+.5gT2) s T

Fundamentals of Kalman Filtering: A Practical Approach

9 - 68

MATLAB Version of Two Decoupled Polynomial Linear Kalman Filters for Tracking Projectile-1
TS=1.; ORDER=2; PHIS=0.; SIGTH=.01; SIGR=100.; VT=3000.; GAMDEG=45.; G=32.2; XT=0.; YT=0.; XTD=VT*cos(GAMDEG/57.3); YTD=VT*sin(GAMDEG/57.3); XR=100000.; YR=0.; T=0.; S=0.; H=.001; PHI=zeros(ORDER,ORDER); P=zeros(ORDER,ORDER); IDNP=eye(ORDER); Q=zeros(ORDER,ORDER); PHI(1,1)=1.; PHI(1,2)=TS; PHI(2,2)=1.; HMAT(1,1)=1.; HMAT(1,2)=0.; PHIT=PHI'; HT=HMAT'; Q(1,1)=PHIS*TS*TS*TS/3.; Q(1,2)=PHIS*TS*TS/2.; Q(2,1)=Q(1,2); Q(2,2)=PHIS*TS; P(1,1)=1000.^2; P(2,2)=100.^2; PY(1,1)=1000.^2; PY(2,2)=100.^2; XTH=XT+1000.; XTDH=XTD-100.; YTH=YT-1000.; YTDH=YTD-100.; count=0;

Initial conditions on projectile

Fundamental matrix for both channels Measurement matrix for both channels Process noise matrix for both channels

Initial covariance matrices Initial state estimates

Fundamentals of Kalman Filtering: A Practical Approach
9 - 69

MATLAB Version of Two Decoupled Polynomial Linear Kalman Filters for Tracking Projectile-2
while YT>=0. XTOLD=XT; XTDOLD=XTD; YTOLD=YT; YTDOLD=YTD; XTDD=0.; YTDD=-G; XT=XT+H*XTD; XTD=XTD+H*XTDD; YT=YT+H*YTD; YTD=YTD+H*YTDD; T=T+H; XTDD=0.; YTDD=-G; XT=.5*(XTOLD+XT+H*XTD); XTD=.5*(XTDOLD+XTD+H*XTDD); YT=.5*(YTOLD+YT+H*YTD); YTD=.5*(YTDOLD+YTD+H*YTDD); S=S+H; if S>=(TS-.00001) S=0.; THETH=atan2((YTH-YR),(XTH-XR)); RTH=sqrt((XTH-XR)^2+(YTH-YR)^2); RMAT(1,1)=(cos(THETH)*SIGR)^2+(RTH*sin(THETH)*SIGTH)^2; PHIP=PHI*P; PHIPPHIT=PHIP*PHIT; M=PHIPPHIT+Q; HM=HMAT*M; HMHT=HM*HT; HMHTR=HMHT+RMAT; HMHTRINV(1,1)=1./HMHTR(1,1); MHT=M*HT; K=MHT*HMHTRINV; KH=K*HMAT; IKH=IDNP-KH; P=IKH*M; THETNOISE=SIGTH*randn; RTNOISE=SIGR*randn; THET=atan2((YT-YR),(XT-XR)); RT=sqrt((XT-XR)^2+(YT-YR)^2); THETMEAS=THET+THETNOISE; RTMEAS=RT+RTNOISE; XTMEAS=RTMEAS*cos(THETMEAS)+XR; RES1=XTMEAS-XTH-TS*XTDH; XTH=XTH+TS*XTDH+K(1,1)*RES1; XTDH=XTDH+K(2,1)*RES1;

Second-order Runge-Kutta integration for projectile equations

Downrange R

Riccati equations in downrange channel Actual noisy measurements of range and angle Downrange lter
Fundamentals of Kalman Filtering: A Practical Approach
9 - 70

MATLAB Version of Two Decoupled Polynomial Linear Kalman Filters for Tracking Projectile-3
RMATY(1,1)=(sin(THETH)*SIGR)^2+(RTH*cos(THETH)*SIGTH)^2; PHIPY=PHI*PY; PHIPPHITY=PHIPY*PHIT; MY=PHIPPHITY+Q; HMY=HMAT*MY; HMHTY=HMY*HT; HMHTRY=HMHTY+RMATY; HMHTRINVY(1,1)=1./HMHTRY(1,1); MHTY=MY*HT; KY=MHTY*HMHTRINVY; KHY=KY*HMAT; IKHY=IDNP-KHY; PY=IKHY*MY; YTMEAS=RTMEAS*sin(THETMEAS)+YR; RES2=YTMEAS-YTH-TS*YTDH+.5*TS*TS*G; YTH=YTH+TS*YTDH-.5*TS*TS*G+KY(1,1)*RES2; YTDH=YTDH-TS*G+KY(2,1)*RES2; ERRX=XT-XTH; SP11=sqrt(P(1,1)); ERRXD=XTD-XTDH; SP22=sqrt(P(2,2)); ERRY=YT-YTH; SP11Y=sqrt(PY(1,1)); ERRYD=YTD-YTDH; SP22Y=sqrt(PY(2,2)); SP11P=-SP11; SP22P=-SP22; SP11YP=-SP11Y; SP22YP=-SP22Y; count=count+1; ArrayT(count)=T; ArrayERRX(count)=ERRX; ArrayERRXD(count)=ERRXD; ArrayERRY(count)=ERRY; ArrayERRYD(count)=ERRYD; ArraySP11(count)=SP11; ArraySP11P(count)=SP11P; ArraySP22(count)=SP22; ArraySP22P(count)=SP22P; ArraySP11Y(count)=SP11Y; ArraySP11YP(count)=SP11YP; ArraySP22Y(count)=SP22Y; ArraySP22YP(count)=SP22YP; end end

Altitude R

Riccati equations in altitude channel

Altitude lter

Collect data for plotting and writing to les

Fundamentals of Kalman Filtering: A Practical Approach

9 - 71

Linear Decoupled Kalman Filter Downrange Error in the Estimate of Position is Larger Than That of Extended Kalman Filter
200 Theory Linear Decoupled Filters

100

-100 Theory

-200 0

Simulation

20

40

60 80 Time (Sec)

100

120

140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 72

Linear Decoupled Kalman Filter Downrange Error in the Estimate of Velocity is Larger Than That of Extended Kalman Filter
20 Simulation Linear Decoupled Filters

10 Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 73

Linear Decoupled Kalman Filter Altitude Error in the Estimate of Position is Larger Than That of Extended Kalman Filter
600 400 200 0 -200 -400 -600 0 20 40 60 80 Time (Sec) 100 120 140 Theory Linear Decoupled Filters Theory

Simulation

Fundamentals of Kalman Filtering: A Practical Approach

9 - 74

Linear Decoupled Kalman Filter Altitude Error in the Estimate of Velocity is Larger Than That of Extended Kalman Filter
20 Simulation 10 Linear Decoupled Filters

Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 75

Using Linear Coupled Polynomial Kalman Filters

Fundamentals of Kalman Filtering: A Practical Approach

9 - 76

Measurement Noise Matrix For Linear Coupled Polynomial Kalman Filter -1

In Cartesian frame model of real world is linear but measurements are nonlinear Recall
xT = rcos! + xR
yT = rsin! + yR

Find total differential from calculus

!xT = !yT = "xT "x !r + T !! = cos!!r - rsin!!! "r "! "yT "y !r + T !! = sin!!r + rcos!!! "r "!

Square both equations

!x2 = cos2 !!r2 - 2rsin!cos!!r!! + r2sin2 !!! T
2 2

!y2 = sin2!!r2 + 2rsin!cos!!r!! + r2 cos2 !!! T

Fundamentals of Kalman Filtering: A Practical Approach

9 - 77

Measurement Noise Matrix For Linear Coupled Polynomial Kalman Filter -2

We can also nd
!xT!yT = sin!cos!!r2 + rsin2 !!r!! + rcos2 !!r!!- r2 sin!cos!!!
2

Neglected before

Taking expectations
E !x2 = cos2 !E !r2 + r2sin2 !E !! T E !y2 = sin2!E !r2 + r2 cos2 !E !! T
2 2

E !xT!yT = sin!cos!E !r2 - r2 sin!cos!E !!

New
!2 T = E !x2 x T

Therefore
!2 T = cos2 "!2 + r2 sin2 "!2 x r " !2 T = sin2 "!2 + r2 cos2 "!2 y r " !2 T y T = sin"cos"!2 x r r2 sin"cos"!2 "

Where

!2 T y

=E

!y2 T

!2 = E !r2 r
!" = E !"
2 2

!2 T y T x

= E !xT!yT

Or
Rk = cos2 !"2 + r2 sin2 !"! r sin!cos!"2 - r2 sin!cos!"2 r !
2

sin!cos!"2 - r2sin!cos!"! r sin2!"2 + r2 cos2 !"2 r !

Fundamentals of Kalman Filtering: A Practical Approach
9 - 78

Important Matrices For Linear Coupled Polynomial Kalman Filter-1

Measurement equation
x* T y* T = 1 0 0 0 0 0 1 0 xT xT v + x yT vy yT

Measurement matrix
H= 1 0 0 0 0 0 1 0

Model of real world

xT 0 1 xT = 0 0 yT 0 0 yT 0 0 0 0 1 0 0 0 0 0 xT 0 xT + 0 + yT 0 yT -g 0 us 0 us

Systems dynamics matrix

0 F= 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0

Fundamentals of Kalman Filtering: A Practical Approach

9 - 79

Important Matrices For Linear Coupled Polynomial Kalman Filter-2

Fundamental matrix
0 !(t) = 0 0 0 t 1 0 0 0 0 1 0 0 0 t 1

Discrete fundamental matrix

0 !k = 0 0 0 Ts 1 0 0 0 0 1 0 0 0 Ts 1

Continuous process noise matrix

0 0 0 0
Q = E(ww T)

Q(t) =

0 !s 0 0 0 0 0 0 0 0 0 !s

Discrete process noise matrix can be derived from

Ts

Qk =
0

!(")Q! (")dt
Fundamentals of Kalman Filtering: A Practical Approach
9 - 80

Important Matrices For Linear Coupled Polynomial Kalman Filter-3

Discrete process noise matrix
T3 !s s 3 T2 !s s 2 0 0 T2!s s 2 Ts!s 0 0 0 0 T3!s s 3 T2 !s s 2 0 0 T2 !s s 2 Ts!s

Qk =

Continuous control vector

0 G= 0 0 -g

Deriving discrete control vector

Ts Ts

Gk =
0

!Gd" =

0 0 0 0
0

" 1 0 0

0 0 1 0

0 0 " 1

0 0 d" = 0 -g

0 0 -.5gT2 s -gTs
Fundamentals of Kalman Filtering: A Practical Approach
9 - 81

Linear Coupled Polynomial Kalman Filter-1

Recall
x k = !k x k-1 + G k u k-1 + K k (z k - H !k x k-1 - H G k u k-1)

Substitution yields
xTk xTk yTk yTk K 1 1k K2 1k K3 1k K4 1k K1 2k K2 2k K3 2k K4 2k = 0 0 0 0 Ts 1 0 0 0 0 1 0 0 0 Ts 1 xTk-1 xTk-1 yTk-1 yTk-1 0 0 Ts 1 xTk-1 xTk-1 yTk-1 yTk-1 - 1 0 0 0 0 0 1 0 + 0 0 -.5gT2 s -gTs 0 0 -.5gT2 s -gTs +

1 0 0 0 0 0 1 0

0 0 0 0

Ts 1 0 0

0 0 1 0

Fundamentals of Kalman Filtering: A Practical Approach

9 - 82

Linear Coupled Polynomial Kalman Filter-2

Multiplying out terms
RES1 = x* k - xTk-1 - TsxTk-1 T RES2 = y* k - yTk-1 - TsyTk-1 + .5gT2 s T

xTk = xTk-1 + Ts xTk-1 + K1 1kRES1 + K1 2kRES2 xTk = xTk-1 + K2 1kRES1 + K2 2kRES2 yTk = yTk-1 + Ts yTk-1 -.5gT2 + K3 1kRES1 + K3 2kRES2 s

yTk = yTk-1 -gTs + K4 1kRES1 + K4 2kRES2

Fundamentals of Kalman Filtering: A Practical Approach

9 - 83

MATLAB Version of Coupled Polynomial Linear Kalman Filter for Tracking Projectile-1
TS=1.; ORDER=4; PHIS=0.; SIGTH=.01; SIGR=100.; VT=3000.; GAMDEG=45.; G=32.2; XT=0.; YT=0.; XTD=VT*cos(GAMDEG/57.3); YTD=VT*sin(GAMDEG/57.3); XR=100000.; YR=0.; T=0.; S=0.; H=.001; PHI=zeros(ORDER,ORDER); P=zeros(ORDER,ORDER); IDNP=eye(ORDER); Q=zeros(ORDER,ORDER); PHI(1,1)=1.; PHI(1,2)=TS; PHI(2,2)=1.; PHI(3,3)=1.; PHI(3,4)=TS; PHI(4,4)=1.; HMAT(1,1)=1.; HMAT(1,2)=0.; HMAT(1,3)=0.; HMAT(1,4)=0.; HMAT(2,1)=0.; HMAT(2,2)=0.; HMAT(2,3)=1.; HMAT(2,4)=0.; PHIT=PHI'; HT=HMAT'; Q(1,1)=PHIS*TS*TS*TS/3.; Q(1,2)=PHIS*TS*TS/2.; Q(2,1)=Q(1,2); Q(2,2)=PHIS*TS; Q(3,3)=PHIS*TS*TS*TS/3.; Q(3,4)=PHIS*TS*TS/2.; Q(4,3)=Q(3,4); Q(4,4)=PHIS*TS;

Initial conditions on projectile

Fundamental matrix Measurement matrix

Process noise matrix

Fundamentals of Kalman Filtering: A Practical Approach
9 - 84

MATLAB Version of Coupled Polynomial Linear Kalman Filter for Tracking Projectile-2
P(1,1)=1000.^2; P(2,2)=100.^2; P(3,3)=1000.^2; P(4,4)=100.^2; XTH=XT+1000.; XTDH=XTD-100.; YTH=YT-1000.; YTDH=YTD+100.; count=0; while YT>=0.

Initial covariance matrix Initial lter state estimates

XTOLD=XT; XTDOLD=XTD; YTOLD=YT; YTDOLD=YTD; XTDD=0.; YTDD=-G; XT=XT+H*XTD; XTD=XTD+H*XTDD; YT=YT+H*YTD; YTD=YTD+H*YTDD; T=T+H; XTDD=0.; YTDD=-G; XT=.5*(XTOLD+XT+H*XTD); XTD=.5*(XTDOLD+XTD+H*XTDD); YT=.5*(YTOLD+YT+H*YTD); YTD=.5*(YTDOLD+YTD+H*YTDD); S=S+H; if S>=(TS-.00001) S=0.; THETH=atan2((YTH-YR),(XTH-XR)); RTH=sqrt((XTH-XR)^2+(YTH-YR)^2); RMAT(1,1)=(cos(THETH)*SIGR)^2+(RTH*sin(THETH)*SIGTH)^2; RMAT(2,2)=(sin(THETH)*SIGR)^2+(RTH*cos(THETH)*SIGTH)^2; RMAT(1,2)=sin(THETH)*cos(THETH)*(SIGR^2-(RTH*SIGTH)^2); RMAT(2,1)=RMAT(1,2); PHIP=PHI*P; PHIPPHIT=PHIP*PHIT; M=PHIPPHIT+Q; HM=HMAT*M; HMHT=HM*HT; HMHTR=HMHT+RMAT; HMHTRINV=inv(HMHTR); MHT=M*HT; MHT=M*HT; Fundamentals of Kalman Filtering: K=MHT*HMHTRINV;

Second-order Runge-Kutta integration for projectile equations

R matrix

Riccati equations

9 - 85

A Practical Approach

MATLAB Version of Coupled Polynomial Linear Kalman Filter for Tracking Projectile-3
KH=K*HMAT; IKH=IDNP-KH; P=IKH*M; THETNOISE=SIGTH*randn; RTNOISE=SIGR*randn; THET=atan2((YT-YR),(XT-XR)); RT=sqrt((XT-XR)^2+(YT-YR)^2); THETMEAS=THET+THETNOISE; RTMEAS=RT+RTNOISE; XTMEAS=RTMEAS*cos(THETMEAS)+XR; YTMEAS=RTMEAS*sin(THETMEAS)+YR; RES1=XTMEAS-XTH-TS*XTDH; RES2=YTMEAS-YTH-TS*YTDH+.5*TS*TS*G; XTH=XTH+TS*XTDH+K(1,1)*RES1+K(1,2)*RES2; XTDH=XTDH+K(2,1)*RES1+K(2,2)*RES2; YTH=YTH+TS*YTDH-.5*TS*TS*G+K(3,1)*RES1+K(3,2)*RES2; YTDH=YTDH-TS*G+K(4,1)*RES1+K(4,2)*RES2; ERRX=XT-XTH; SP11=sqrt(P(1,1)); ERRXD=XTD-XTDH; SP22=sqrt(P(2,2)); ERRY=YT-YTH; SP33=sqrt(P(3,3)); ERRYD=YTD-YTDH; SP44=sqrt(P(4,4)); SP11P=-SP11; SP22P=-SP22; SP33P=-SP33; SP44P=-SP44; count=count+1; ArrayT(count)=T; ArrayERRX(count)=ERRX; ArrayERRXD(count)=ERRXD; ArrayERRY(count)=ERRY; ArrayERRYD(count)=ERRYD; ArraySP11(count)=SP11; ArraySP11P(count)=SP11P; ArraySP22(count)=SP22; ArraySP22P(count)=SP22P; ArraySP33(count)=SP33; ArraySP33P(count)=SP33P; ArraySP44(count)=SP44; ArraySP44P(count)=SP44P; end end

Noisy radar measurements Pseudomeasurements Filter

Saving data for plotting and writing to les

Fundamentals of Kalman Filtering: A Practical Approach

9 - 86

Error in Estimate of Downrange is the Same for Both the Linear Coupled and Extended Kalman Filters
150 100 50 0 -50 -100 Simulation -150 0 20 40 60 80 Time (Sec) 100 120 140 Theory Theory Linear Coupled Filter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 87

Error in Estimate of Downrange Velocity is the Same for Both the Linear Coupled and Extended Kalman Filters
20 Simulation Theory 10 Linear Coupled Filter

-10 Theory -20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 88

Error in Estimate of Altitude is the Same for Both the Linear Coupled and Extended Kalman Filters
600 400 200 0 -200 -400 Theory -600 0 20 40 60 80 Time (Sec) 100 120 140 Linear Coupled Filter Simulation Theory

Fundamentals of Kalman Filtering: A Practical Approach

9 - 89

Error in Estimate of Altitude Velocity is the Same for Both the Linear Coupled and Extended Kalman Filters
20 Simulation 10 Linear Coupled Filter

Theory

-10

Theory

-20 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 90

Robustness Comparison of Extended and Linear Coupled Kalman Filters

Fundamentals of Kalman Filtering: A Practical Approach

9 - 91

We Will Conduct Experiments With Initialization Errors

Initial lter state estimates
xT(0) xT(0) xT(0) x (0) = T + yT(0) yT(0) yT(0) yT(0) 1000 -100 -1000 100

Until now
0 0 10002 0 0 0 0 1002

Initial covariance matrix

10002 P0 = 0 0 0 0 1002 0 0

Let us start by doubling nominal errors

xT(0) xT(0) xT(0) x (0) = T + yT(0) yT(0) yT(0) yT(0)

20002
2000 -200 -2000 200

0 2002 0 0

0 0 20002 0

0 0 0 2002
9 - 92

and

P0 =

0 0 0

Fundamentals of Kalman Filtering: A Practical Approach

Extended Kalman Filter Sensitivity to Initialization Errors

Fundamentals of Kalman Filtering: A Practical Approach

9 - 93

Extended Kalman Filter Appears to Yield Good Estimates Even When Initialization Errors are Twice as Large as Nominal
300000 250000 200000 150000 100000 Actual and Estimate 50000 0 0 20 40 60 80 Time (Sec) 100 120 140 Nominal Errors and Twice Nominal Errors

Fundamentals of Kalman Filtering: A Practical Approach

9 - 94

Estimates From Extended Kalman Filter Degrade Severely When Initialization Errors are Five Times Larger Than Nominal
300000 250000 200000 Estimate 150000 100000 50000 0 0
xT(0)

Five Times Nominal Errors

Actual

20

40

60 80 Time (Sec)

100

120

140

xT(0) xT(0) x (0) = T + yT(0) yT(0) yT(0) yT(0)

50002
5000 -500 -5000 500

0 5002 0 0

0 0 50002 0

0 0 0 5002
9 - 95

P0 =

0 0 0

Fundamentals of Kalman Filtering: A Practical Approach

Estimates From Extended Kalman Filter are Worthless When Initialization Errors are Ten Times Larger Than Nominal
400000 Ten Times Nominal Errors 300000

200000

100000 Actual 0 Estimate -100000 0

xT(0)

20

40

60 80 Time (Sec)

100

120

140

xT(0) 10000 xT(0) xT(0) = + -1000 yT(0) -10000 yT(0) yT(0) 1000 yT(0)

100002 P0 = 0 0 0

0 10002 0 0

0 0 100002 0

0 0 0 10002
9 - 96

Fundamentals of Kalman Filtering: A Practical Approach

Addition of Process Noise Enables Extended Kalman Filter to Better Estimate Downrange in Presence of Large Initialization Errors
400000 Ten Times Nominal Errors Estimated !s=1 Actual 200000

300000

100000 Estimated !s=10 0 Estimated !s=0 0 20 40 60 80 Time (Sec) 100 120 140

-100000

Fundamentals of Kalman Filtering: A Practical Approach

9 - 97

Making Process Noise Larger Further Improves Extended Kalman Filters Ability to Estimate Downrange
400000 Ten Times Nominal Errors 300000 Actual

200000

Estimated !s=1000

100000 Estimated !s=100 0 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 98

After an Initial Transient Period Simulation Results Agree With Covariance Matrix Predictions
1000 Theory 500 Ten Times Nominal Errors !s=1000

-500 Theory Simulation -1000 0 20 40 60 80 Time (Sec) 100 120 140

Fundamentals of Kalman Filtering: A Practical Approach

9 - 99

Linear Coupled Polynomial Kalman Filter Sensitivity to Initialization Errors

Fundamentals of Kalman Filtering: A Practical Approach

9 - 100

Linear Coupled Polynomial Kalman Filter Does Not Appear to be Sensitive to Large Initialization Errors
300000 250000 200000 150000 100000 50000 0 0 20 40 60 80 Time (Sec) 100 120 140 Actual and Estimated Ten Times Nominal Error Linear Coupled Kalman Filter !s=0

Fundamentals of Kalman Filtering: A Practical Approach

9 - 101

Linear Coupled Polynomial Kalman Filter Appears to be Working Correctly in Presence of Large Initialization Errors
200 Simulation Ten Times Nominal Error Linear Coupled Kalman Filter !s=0

100

Theory 0

-100 Theory -200 0 20 40 60 80 Time (Sec) 100 120 140

*Same performance as when initialization errors are small

Fundamentals of Kalman Filtering: A Practical Approach
9 - 102

Linear Coupled Kalman Filter Performance Appears to be Independent of Initialization Errors

Simulation 150 100 50 0 -50 -100 Theory -150 0 20 40 60 80 Time (Sec)
! P0 = 0 0 0 0 ! 0 0

Theory

Linear Coupled Filter Bad Initialization

100

120

140

xT(0)

0 0 ! 0

0 0 0 !
Fundamentals of Kalman Filtering: A Practical Approach
9 - 103

0 xT(0) = 0 0 yT(0) 0 yT(0)

Cannon Launched Projectile Tracking Problem Summary

For this problem there were no performance advantages in the polar coordinate system but there were computational disadvantages For this problem a linear coupled polynomial Kalman lter yielded similar performance Linear coupled polynomial Kalman lter was much less sensitive to initialization errors than extended Kalman lter

Fundamentals of Kalman Filtering: A Practical Approach

9 - 104