Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical

13 - 1Fundamentals of Kalman Filtering:A Practical Approach

Linearized Kalman Filtering


Linearized Kalman FilteringOverview

• Theoretical equations• Falling object revisited

- Getting feel for sensitivities- Designing linearized Kalman filter

• Cannon launched projectile revisited- Getting feel for sensitivities- Designing linearized Kalman filter


Theoretical Equations


Theoretical Equations - 1

Nonlinear model of the real worldx = f(x) + w

Process noise matrixQ = E(wwT)

Nonlinear measurement equationz = h(x) + v

Measurement noise matrixR = E(vvT)

Suppose we have nominal trajectory informationxNOM = f(xNOM)

Error equation!x = x - x NOM


Theoretical Equations - 2Linearized differential equation for error states

Measurement error equation!z = z - z NOM

Linearized error measurement equation

Systems dynamics and measurement matrices

x=xNOM

x=xNOM


Theoretical Equations - 3

Discrete measurement equation!zk = H!xk + v k

Discrete fundamental matrix !k = I + FTs +

F2Ts2

2! + F3Ts

3

3! + ...

Sometimes!k " I + FTs

Still use Riccati equations to compute filter gainsMk = !kPk -1!k

T + Qk

Kk = MkHT(HMkHT + Rk)-1

Pk = (I - K kH)Mk

Filter!xk = !!xk -1 + K k(!zk - H!!xk -1) xk = !xk + x NOM

To get state estimate


Falling Object Revisited


Radar Tracking Object With Unknown Drag

g

6000 Ft/Sec

200,000 Ft

(Known)

Drag

(Unknown)


Equations Representing Real World

Acceleration acting on objectx = Drag - g =

Qpg

! - g

Qp = .5!x2

! = .0034e-x/22000

Rewriting differential equationx =

Qpg

! - g =

.5g"x2

! - g =

.0034ge-x/22000x2

2! - g


FORTRAN Simulation for Actual and NominalTrajectories of Falling Object-1

IMPLICIT REAL*8 (A-H)IMPLICIT REAL*8 (O-Z)X=200000.XD=-6000.BETA=500.XNOM=200000.XDNOM=-6000.BETANOM=800.OPEN(1,STATUS='UNKNOWN',FILE='DATFIL')TS=.1T=0.S=0.H=.001WHILE(X>=0.)

XOLD=XXDOLD=XDXNOMOLD=XNOMXDNOMOLD=XDNOMXDD=.0034*32.2*XD*XD*EXP(-X/22000.)/(2.*BETA)-32.2XDDNOM=.0034*32.2*XDNOM*XDNOM*EXP(-XNOM/22000.)/

1 (2.*BETANOM)-32.2 X=X+H*XD

XD=XD+H*XDDXNOM=XNOM+H*XDNOMXDNOM=XDNOM+H*XDDNOM

T=T+HXDD=.0034*32.2*XD*XD*EXP(-X/22000.)/(2.*BETA)-32.2XDDNOM=.0034*32.2*XDNOM*XDNOM*EXP(-XNOM/22000.)/

1 (2.*BETANOM)-32.2 X=.5*(XOLD+X+H*XD)

XD=.5*(XDOLD+XD+H*XDD)XNOM=.5*(XNOMOLD+XNOM+H*XDNOM)XDNOM=.5*(XDNOMOLD+XDNOM+H*XDDNOM)

S=S+H

Initial conditions for actual trajectoryInitial conditions for nominal trajectory

Integrating actual andnominal differentialequations withsecond-orderRunge-Kuttatechnique


FORTRAN Simulation for Actual and NominalTrajectories of Falling Object-2

IF(S>=(TS-.00001))THENS=0.DELXH=X-XNOMDELXDH=XD-XDNOMDELBETAH=BETA-BETANOMXH=XNOM+DELXHXDH=XDNOM+DELXDHBETAH=BETANOM+DELBETAHWRITE(9,*)T,DELXH,DELXDH,DELBETAH,X,XH,

1 XD,XDHWRITE(1,*)T,DELXH,DELXDH,DELBETAH,X,XH,

1 XD,XDH ENDIF

END DO PAUSE

CLOSE(1)END

Compute differencesBetween actual andNominal trajectories

Estimated states


Object Slows Up Significantly as it Races Towardsthe Ground (Nominal Case)

200000

150000

100000

50000

0403020100

Time (Sec)

-6000

-5000

-4000

-3000

-2000

-1000

Altitude

Velocity


Differences Between the Actual and NominalBallistic Coefficient Can Yield Significant

Differences in Altitude

14000

12000

10000

8000

6000

4000

2000

0

403020100

Time (Sec)

!ACT

=500 Lb/Ft2

!NOM

=500 Lb/Ft2

!NOM

=800 Lb/Ft2

!NOM

=1100 Lb/Ft2

!NOM

=600 Lb/Ft2


Differences Between the Actual and NominalBallistic Coefficient Can Yield Significant

Differences in Velocity

1000

800

600

400

200

0

403020100

Time (Sec)

!ACT

=500 Lb/Ft2

!NOM

=500 Lb/Ft2

!NOM

=800 Lb/Ft2

!NOM

=1100 Lb/Ft2

!NOM

=600 Lb/Ft2


Developing A Linearized Kalman Filter For FallingObject Problem


Developing A Linearized Kalman Filter-1

Error states

!x =

!x

!x

!!

=

x - xNOM

x - xNOM

! - !NOM

Model of real worldx =

.0034ge-x/22000x2

2! - g

! = us

Error model equation



Systems dynamics matrix

F =

!x

!x

!x

!x

!x

!!

!x

!x

!x

!x

!x

!!

!!

!x

!!

!x

!!

!! x=x NOM

Continuous process noise matrix

Q = E(ww T) Q(t) =

0 0 0

0 0 0

0 0 !s

Evaluate first row of partial derivatives

x = x

!x

!x = 0

!x

!x = 1

!x

!! = 0



Evaluate second row of partial derivatives

x = .0034ge-x/22000x2

2! - g

!x

!x =

-.0034e-x/22000x2g

2!(22000) =

-"gx2

44000!

!x

!x =

2*.0034e-x/22000xg

2! =

"gx

!

!x

!! =

-.0034e-x/22000x2g

2!2

= -"gx2

2!2

Evaluate third row of partial derivatives

! = us

!!

!x = 0

!!

!x = 0

!!

!! = 0


Developing A Linearized Kalman Filter-4Systems dynamics matrix is evaluated along nominal trajectory

F =

0 1 0

-!NOMgxNOM2

44000"NOM

!NOMgxNOM

"NOM

-!NOMgxNOM2

2"NOM2

0 0 0

where !NOM = .0034e-xNOM/22000

Two term Taylor series approximation for fundamental matrix!(t) " I + Ft

If we rewrite systems dynamics matrix as

F(t) = 0 1 0

f21 f22 f23

0 0 0

where

f21 = -!NOMgxNOM

2

44000"NOM

f22 = !NOMxNOMg

"NOM

f23 = -!NOMgxNOM

2

2"NOM2



Then

!(t) " I + Ft =1 0 0

0 1 0

0 0 1

+ 0 1 0

f21 f22 f23

0 0 0

t = 1 t 0

f21t 1+f22t f23t

0 0 1

And discrete fundamental matrix becomes

!k "

1 Ts 0

f21Ts 1+f22Ts f23Ts

0 0 1

Error measurement equation

!x* = 1 0 0

!x

!x

!!

+ v

Therefore measurement matrix is

H = 1 0 0


Developing A Linearized Kalman Filter-6Discrete measurement noise matrix is a scalar

Rk = E(vkvkT) Rk = !v

2


Q = E(ww T) Q(t) =

0 0 0

0 0 0

0 0 !s

Discrete process noise matrix

Qk = !(")Q!T(")d"

0

Ts

Qk = !s

1 " 0

f21" 1+f22" f23"

0 0 10

Ts

0 0 0

0 0 0

0 0 1

1 f21" 0

" 1+f22" 0

0 f23" 0

d"

Qk = !s

0 0 0

0 f23

2"2 f23"

0 f23" 10

Ts

d" Qk = !s

0 0 0

0 f23

2 Ts3

3 f23

Ts2

2

0 f23

Ts2

2 Ts


Developing A Linearized Kalman Filter-7Linearized Kalman filtering equation

!xk

!xk

!!k

= "

!xk-1

!xk-1

!!k-1

+

K1k

K2k

K3k

(!xk* - H"

!xk-1

!xk-1

!!k-1

)


MATLAB Simulation of Linearized Kalman Filter-1X=200000.;XD=-6000.;BETA=500.;XNOM=200000.;XDNOM=-6000.;BETANOM=500.;ORDER=3;TS=.1;TF=30.;Q33=300.*300./TF;T=0.;S=0.;H=.001;SIGNOISE=25.;RMAT(1,1)=SIGNOISE^2;XH(1,1)=X-XNOM;XH(2,1)=XD-XDNOM;XH(3,1)=-300.;PHI=zeros(ORDER,ORDER);P=zeros(ORDER,ORDER);IDNP=eye(ORDER);Q=zeros(ORDER,ORDER);P(1,1)=SIGNOISE*SIGNOISE;P(2,2)=20000.;P(3,3)=300.^2;HMAT=zeros(1,ORDER);HMAT(1,1)=1.;HT=HMAT';count=0;

Initial actual and nominal states

Initial filter error state estimates

Initial covariance matrix


MATLAB Simulation of Linearized Kalman Filter-2while X>=0.

XOLD=X;XDOLD=XD;XNOMOLD=XNOM;XDNOMOLD=XDNOM;XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;

X=X+H*XD;XD=XD+H*XDD;XNOM=XNOM+H*XDNOM;XDNOM=XDNOM+H*XDDNOM;

T=T+H;XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;

X=.5*(XOLD+X+H*XD);XD=.5*(XDOLD+XD+H*XDD);XNOM=.5*(XNOMOLD+XNOM+H*XDNOM);XDNOM=.5*(XDNOMOLD+XDNOM+H*XDDNOM);

S=S+H;if S>=(TS-.00001)

S=0.;RHONOM=.0034*exp(-XNOM/22000.);F21=-32.2*RHONOM*XDNOM*XDNOM/(2.*22000.*BETANOM);F22=RHONOM*32.2*XDNOM/BETANOM;F23=-RHONOM*32.2*XDNOM*XDNOM/(2.*BETANOM*BETANOM);PHI(1,1)=1.;PHI(1,2)=TS;PHI(2,1)=F21*TS;PHI(2,2)=1.+F22*TS;PHI(2,3)=F23*TS;PHI(3,3)=1.;Q(2,2)=F23*F23*Q33*TS*TS*TS/3.;Q(2,3)=F23*Q33*TS*TS/2.;Q(3,2)=Q(2,3);Q(3,3)=Q33*TS;PHIT=PHI';

Numerical integrationof actual and nominalequations of fallingobject

Fundamental matrix

Process noise matrix


MATLAB Simulation of Linearized Kalman Filter-3PHIP=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; XNOISE=SIGNOISE*randn;

DELX=X-XNOM;MEAS(1,1)=DELX+XNOISE;PHIXH=PHI*XH;HPHIXH=HMAT*PHIXH;RES=MEAS-HPHIXH;KRES=K*RES;XH=PHIXH+KRES;DELXH=XH(1,1);DELXDH=XH(2,1);DELBETAH=XH(3,1);XHH=XNOM+DELXH;XDH=XDNOM+DELXDH;BETAH=BETANOM+DELBETAH;ERRX=X-XHH;SP11=sqrt(P(1,1));ERRXD=XD-XDH;SP22=sqrt(P(2,2));ERRBETA=BETA-BETAH;SP33=sqrt(P(3,3));SP11P=-SP11;SP22P=-SP22;SP33P=-SP33;count=count+1;ArrayT(count)=T;ArrayBETA(count)=BETA;ArrayBETAH(count)=BETAH;ArrayERRBETA(count)=ERRBETA;ArraySP33(count)=SP33;ArraySP33P(count)=SP33P;

endend

Riccati equations

Filter in terms of error states

Convert estimated error states toestimated states

Actual and theoretical errors in estimates

Save data in arrays for plotting andwriting to files


Linearized Kalman Filter is Able to AccuratelyEstimate the Ballistic Coefficient After Only 10 Sec

*Of course if actual and nominal ballistic coefficients were equal wewould not have to estimate ballistic coefficient

800

600

400

200

0403020100

Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=500 Lb/Ft2Actual

Estimate


Linearized Kalman Filter Appears to be WorkingCorrectly

300

200

100

0

-100

-200

-300

403020100Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=500 Lb/Ft2

Simulation

Theory

Theory


Linearized Kalman Filter Accurately EstimatesBallistic Coefficient When There is a Slight Error in

the Nominal Trajectory800

600

400

200

0403020100

Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=600 Lb/Ft2Actual

Estimate


Error in Estimate of Ballistic Coefficient is BetweenTheoretical Error Bounds When There is a Slight

Error in the Nominal Trajectory

-300

-200

-100

0

100

200

300

403020100Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=600 Lb/Ft2Simulation

Theory

Theory


Linearized Kalman Filter Underestimates BallisticCoefficient When There is 60% Error in the Nominal

Ballistic Coefficient800

600

400

200

0403020100

Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=800 Lb/Ft2Actual

Estimate


Error in Estimate of Ballistic Coefficient is NotBetween Theoretical Error Bounds With 60% Error

in the Nominal Ballistic Coefficient

-300

-200

-100

0

100

200

300

403020100Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=800 Lb/Ft2Simulation

Theory

Theory


There is Significant Steady-State error in theLinearized Kalman Filter’s Estimate of Ballistic

Coefficient When There is a 120% Error

800

600

400

200

0

-200

403020100Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=1100 Lb/Ft2

Actual

Estimate


With 120% Error, the Errors in the Estimates areCompletely Outside the Theoretical Error Bounds

800

600

400

200

0

-200

403020100Time (Sec)

βACT

=500 Lb/Ft2

βNOM

=1100 Lb/Ft2

Simulation

Theory

Theory


Cannon Launched Projectile Revisited


Radar Tracking Cannon Launched Projectile

x

y

Radar (x , y )RR

(x , y )TT

!

rg

Cannon

xT = 0

yT = -g

Acceleration on projectile Angle and range from radar to projectile! = tan-1 yT - yR

xT - xR

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


FORTRAN Simulation for Actual and NominalProjectile Trajectories-1

IMPLICIT REAL*8(A-H,O-Z)TS=1.VT=3000.VTERR=0.GAMDEG=45.GAMDEGERR=0.VTNOM=VT+VTERRGAMDEGNOM=GAMDEG+GAMDEGERRG=32.2XT=0.YT=0.XTD=VT*COS(GAMDEG/57.3)YTD=VT*SIN(GAMDEG/57.3)XTNOM=XTYTNOM=YTXTDNOM=VTNOM*COS(GAMDEGNOM/57.3)YTDNOM=VTNOM*SIN(GAMDEGNOM/57.3)XR=100000.YR=0.OPEN(1,STATUS='UNKNOWN',FILE='DATFIL')T=0.S=0.H=.001WHILE(YT>=0.)

XTOLD=XTXTDOLD=XTDYTOLD=YTYTDOLD=YTDXTNOMOLD=XTNOMXTDNOMOLD=XTDNOMYTNOMOLD=YTNOMYTDNOMOLD=YTDNOMXTDD=0.

YTDD=-GXTDDNOM=0.

YTDDNOM=-G XT=XT+H*XTD XTD=XTD+H*XTDD

YT=YT+H*YTDYTD=YTD+H*YTDD

Actual and nominal initial conditions

Integration of actual and nominaldifferential equations using the second-order Runge-Kutta technique


FORTRAN Simulation for Actual and NominalProjectile Trajectories-2

XTNOM=XTNOM+H*XTDNOM XTDNOM=XTDNOM+H*XTDDNOM

YTNOM=YTNOM+H*YTDNOMYTDNOM=YTDNOM+H*YTDDNOMT=T+HXTDD=0.

YTDD=-GXTDDNOM=0.

YTDDNOM=-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)

XTNOM=.5*(XTNOMOLD+XTNOM+H*XTDNOM) XTDNOM=.5*(XTDNOMOLD+XTDNOM+H*XTDDNOM)

YTNOM=.5*(YTNOMOLD+YTNOM+H*YTDNOM)YTDNOM=.5*(YTDNOMOLD+YTDNOM+H*YTDDNOM)S=S+HIF(S>=(TS-.00001))THEN

S=0.RTNOM=SQRT((XTNOM-XR)**2+(YTNOM-YR)**2)THET=ATAN2((YT-YR),(XT-XR))RT=SQRT((XT-XR)**2+(YT-YR)**2)THETNOM=ATAN2((YTNOM-YR),(XTNOM-XR))DELTHET=57.3*(THET-THETNOM)DELRT=RT-RTNOMDELXT=XT-XTNOMDELXTD=XTD-XTDNOMDELYT=YT-YTNOMDELYTD=YTD-YTDNOMWRITE(9,*)T,DELXT,DELXTD,DELYT,DELYTD,DELTHET,

1 DELRTWRITE(1,*)T,DELXT,DELXTD,DELYT,DELYTD,DELTHET,

1 DELRTENDIF

END DO PAUSE

CLOSE(1)END

Actual and nominalrange and angle

Differences between actual andnominal quantities


Differences Between the Actual and Nominal InitialProjectile Velocities Can Yield Significant

Downrange Differences

-20000

-15000

-10000

-5000

0

5000

140120100806040200

Time (Sec)

VTACT=3000 Ft/SecVNOM=3000 Ft/Sec

VNOM=3050 Ft/Sec

VNOM=3100 Ft/Sec

VNOM=3200 Ft/Sec


Differences Between the Actual and Nominal InitialProjectile Velocities Can Yield Significant Angle

Differences

-6

-4

-2

0

2

4

6

140120100806040200Time (Sec)

VTACT=3000 Ft/Sec

VNOM=3000 Ft/Sec

VNOM=3050 Ft/Sec

VNOM=3100 Ft/Sec

VNOM=3200 Ft/Sec


Differences Between the Actual and Nominal InitialProjectile Velocities Can Yield Significant Range

Differences

-20000

-15000

-10000

-5000

0

5000

140120100806040200Time (Sec)

VTACT=3000 Ft/SecVNOM=3000 Ft/Sec

VNOM=3050 Ft/Sec

VNOM=3100 Ft/Sec

VNOM=3200 Ft/Sec


Linearized Cartesian Kalman Filter


Developing a Linearized Cartesian Kalman Filter-1Error states for filter

!x =

!xT

!xT

!yT

!yT

=

xT-xTNOM

xT-xTNOM

yT-yTNOM

yT-yTNOM

Model of real world!xT

!xT

!yT

!yT

=

0 1 0 0

0 0 0 0

0 0 1 0

0 0 0 0

!xT

!xT

!yT

!yT

+

0

us

0

us

Systems dynamics matrix

F =

0 1 0 0

0 0 0 0

0 0 0 1

0 0 0 0

Taylor series approximation for fundamental matrix

!(t) = I + Ft + F2t2

2! + F

3t3

3! +...


Developing a Linearized Cartesian Kalman Filter-2Since

F2 =

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 1

0 0 0 0

=

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

Two term approximation is exact

!(t) = I + Ft =

1 0 0 0

0 1 0 0

0 0 1 0

0 0 0 1

+

0 1 0 0

0 0 0 0

0 0 0 1

0 0 0 0

t !(t) =

0 t 0 0

0 1 0 0

0 0 1 t

0 0 0 1

Discrete fundamental matrix

!k =

0 Ts 0 0

0 1 0 0

0 0 1 Ts

0 0 0 1

Error measurement equation

!!*

!r* =

"!

"xT

"!

"xT

"!

"yT

"!

"yT

"r

"xT

"r

"xT

"r

"yT

"r

"yT

!xT

!xT

!yT

!yT

+ v!vr


Developing a Linearized Cartesian Kalman Filter-3

First row of partials

! = tan-1 yT - yR

xT - xR

!!

!xT = 1

1+(yT - yR)2

(xT - xR)2

(xT - xR)*0 - (yT - yR)*1

(xT - xR)2 =

-(yT - yR)

r2

!!

!xT

= 0

!!

!yT = 1

1+(yT - yR)2

(xT - xR)2

(xT - xR)*1 - (yT - yR)*0

(xT - xR)2 =

(xT - xR)

r2

!!

!yT

= 0

Second row of partials

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

!r

!xT = 1

2 [(xT - xR)

2 + (yT - yR)2]

-1/22(xT - xR) =

(xT - xR)r

!r

!xT

= 0

!r

!yT = 1

2 [(xT - xR)

2 + (yT - yR)2]

-1/22(yT - yR) =

(yT - yR)r

!r

!yT

= 0



Since measurement matrix is

H =

!!

!xT

!!

!xT

!!

!yT

!!

!yT

!r

!xT

!r

!xT

!r

!yT

!r

!yT

We get

H =

-(yT - yR)

r20

xT - xR

r20

xT - xRr

0yT - yR

r0

Evaluate discrete matrix along nominal trajectory

Hk =

-(yTNOM - yR)

rNOM2

0xTNOM

- xR

rNOM2

0

xTNOM - xR

rNOM0

yTNOM - yR

rNOM0



Discrete measurement noise matrix

Rk = E(vkvkT) Rk =

!"

20

0 !r2


Q = E(ww T) Q(t) =

0 0 0 0

0 !s 0 0

0 0 0 0

0 0 0 !s

General formula for discrete process noise matrix

Qk = !(")Q!T(")dt

0

Ts

Substitution yields

Qk =

1 ! 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 0 ! 1

d!

0

Ts



After some algebra we get

Qk =

!2"s !"s 0 0

!"s "s 0 0

0 0 !2"s !"s

0 0 !"s "s

d!

0

Ts

Integration yields

Qk =

Ts3!s

3

Ts2!s

20 0

Ts2!s

2Ts!s 0 0

0 0Ts

3!s

3

Ts2!s

2

0 0Ts

2!s

2Ts!s

Linearized Kalman filter!xTk

!xTk

!yTk

!yTk

= !

!xTk-1

!xTk-1

!yTk-1

!yTk-1

+

K11kK12 k

K21kK22 k

K31kK32 k

K41kK42 k

!"

*

!r* - H!

!xTk-1

!xTk-1

!yTk-1

!yTk-1


MATLAB Linearized Kalman Filter for TrackingCannon Launched Projectile-1

TS=1.;ORDER=4;PHIS=0.;SIGTH=.01;SIGR=100.;VT=3000.;VTERR=0.;GAMDEG=45.;GAMDEGERR=0.;VTNOM=VT+VTERR;GAMDEGNOM=GAMDEG+GAMDEGERR;G=32.2;XT=0.;YT=0.;XTD=VT*cos(GAMDEG/57.3);YTD=VT*sin(GAMDEG/57.3);XTNOM=XT;YTNOM=YT;XTDNOM=VTNOM*cos(GAMDEGNOM/57.3);YTDNOM=VTNOM*sin(GAMDEGNOM/57.3);XR=100000.;YR=0.;T=0.;S=0.;H=.001;XH(1,1)=XT-XTNOM;XH(2,1)=XTD-XTDNOM;XH(3,1)=YT-YTNOM;XH(4,1)=YTD-YTDNOM;PHI=zeros(ORDER,ORDER);P=zeros(ORDER,ORDER);IDNP=eye(ORDER);Q=zeros(ORDER,ORDER);TS2=TS*TS;TS3=TS2*TS;

Actual and nominal initial conditions

Initial filter error states



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);PHI(1,1)=1.;PHI(1,2)=TS;PHI(2,2)=1.;PHI(3,3)=1.;PHI(3,4)=TS;PHI(4,4)=1.;PHIT=PHI';RMAT(1,1)=SIGTH 2̂;RMAT(1,2)=0.;RMAT(2,1)=0.;RMAT(2,2)=SIGR 2̂;P(1,1)=1000. 2̂;P(2,2)=100. 2̂;P(3,3)=1000. 2̂;P(4,4)=100. 2̂;count=0;while YT>=0.

XTOLD=XT;XTDOLD=XTD;YTOLD=YT;YTDOLD=YTD;XTNOMOLD=XTNOM;XTDNOMOLD=XTDNOM;YTNOMOLD=YTNOM;YTDNOMOLD=YTDNOM;XTDD=0.;

YTDD=-G;

Process noise matrix

Fundamental matrix

Measurement noise matrix

Initial covariance matrix



XTDDNOM=0.; YTDDNOM=-G; XT=XT+H*XTD; XTD=XTD+H*XTDD;

YT=YT+H*YTD;YTD=YTD+H*YTDD;XTNOM=XTNOM+H*XTDNOM;

XTDNOM=XTDNOM+H*XTDDNOM;YTNOM=YTNOM+H*YTDNOM;YTDNOM=YTDNOM+H*YTDDNOM;T=T+H;XTDD=0.;

YTDD=-G;XTDDNOM=0.;

YTDDNOM=-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);

XTNOM=.5*(XTNOMOLD+XTNOM+H*XTDNOM); XTDNOM=.5*(XTDNOMOLD+XTDNOM+H*XTDDNOM);

YTNOM=.5*(YTNOMOLD+YTNOM+H*YTDNOM);YTDNOM=.5*(YTDNOMOLD+YTDNOM+H*YTDDNOM);S=S+H;if S>=(TS-.00001)

S=0.;RTNOM=sqrt((XTNOM-XR) 2̂+(YTNOM-YR) 2̂);HMAT(1,1)=-(YTNOM-YR)/RTNOM 2̂;HMAT(1,2)=0.;HMAT(1,3)=(XTNOM-XR)/RTNOM 2̂;HMAT(1,4)=0.;HMAT(2,1)=(XTNOM-XR)/RTNOM;HMAT(2,2)=0.;HMAT(2,3)=(YTNOM-YR)/RTNOM;HMAT(2,4)=0.;HT=HMAT';

Integrate actual and nominaldifferential equations usingsecond-order Runge-Kuttatechnique

Measurement matrix



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;THETNOISE=SIGTH*randn;RTNOISE=SIGR*randn;THET=atan2((YT-YR),(XT-XR));RT=sqrt((XT-XR) 2̂+(YT-YR) 2̂);THETNOM=atan2((YTNOM-YR),(XTNOM-XR));RTNOM=sqrt((XTNOM-XR) 2̂+(YTNOM-YR) 2̂);DELTHET=THET-THETNOM;DELRT=RT-RTNOM;MEAS(1,1)=DELTHET+THETNOISE;MEAS(2,1)=DELRT+RTNOISE;PHIXH=PHI*XH;HPHIXH=HMAT*PHIXH;RES=MEAS-HPHIXH;KRES=K*RES;XH=PHIXH+KRES;XTH=XTNOM+XH(1,1);XTDH=XTDNOM+XH(2,1);YTH=YTNOM+XH(3,1);YTDH=YTDNOM+XH(4,1);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));

Riccati equations

Linearized filter in termsof error states

Convert error state estimates tostate estimates

Actual and theoretical errors in estimates



SP11P=-SP11;SP22P=-SP22;SP33P=-SP33;SP44P=-SP44;count=count+1;ArrayT(count)=T;ArrayXT(count)=XT;ArrayXTH(count)=XTH;ArrayXTD(count)=XTD;ArrayXTDH(count)=XTDH;ArrayYT(count)=YT;ArrayYTH(count)=YTH;ArrayYTD(count)=YTD;ArrayYTDH(count)=YTDH;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

endfigureplot(ArrayT,ArrayERRX,ArrayT,ArraySP11,ArrayT,ArraySP11P),gridxlabel('Time (Sec)')ylabel('Error in Estimate of x (Ft)')axis([0 140 -170 170])

Save data as arrays for plotting andwriting to files

Sample plot


Downrange Errors in the Estimates are Within theTheoretical Bounds for Perfect Nominal Trajectory

-150

-100

-50

0

50

100

150

140120100806040200

Time (Sec)

Perfect Nominal TrajectorySimulation

Theory

Theory


Downrange Velocity Errors in the Estimates areWithin the Theoretical Bounds for Perfect Nominal

Trajectory

-20

-10

0

10

20

140120100806040200

Time (Sec)

Perfect Nominal Trajectory

Simulation

Theory

Theory


Altitude Errors in the Estimates are Within theTheoretical Bounds for Perfect Nominal Trajectory

-600

-400

-200

0

200

400

600

140120100806040200

Time (Sec)


Simulation

Theory

Theory


Altitude Velocity Errors in the Estimates are Withinthe Theoretical Bounds for Perfect Nominal

Trajectory

-20

-10

0

10

20

140120100806040200

Time (Sec)


Simulation

Theory

Theory


Error in Estimate of Projectile Downrange isBetween Theoretical Error Bounds When There is a

1.7% Error in the Nominal Velocity

-200

-100

0

100

200

140120100806040200

Time (Sec)

50 Ft/Sec Velocity Error

Simulation

Theory

Theory


Error in Estimate of Projectile Downrange Starts toDrift Outside Theoretical Error Bounds When There

is a 3.3% Error in the Nominal Velocity

-200

-100

0

100

200

140120100806040200

Time (Sec)


Simulation

Theory

Theory


Error in Estimate of Projectile Downrange Has NoRelationship to Theoretical Error Bounds When

There is a 6.7% Error in the Nominal Velocity

-800

-600

-400

-200

0

200

400

600

800

140120100806040200

Time (Sec)


Simulation

Theory

Theory


Linearized Kalman FilteringSummary

• When nominal trajectories are accurate linearized Kalman filteryields excellent estimates

- Of course in this case filter may not be required• When nominal trajectories are inaccurate linearized Kalman filteryields estimates that deteriorate

- Under certain circumstances they may be worthless

Documents

Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical