Upload
letu
View
234
Download
1
Embed Size (px)
Citation preview
13 - 1Fundamentals of Kalman Filtering:A Practical Approach
Linearized Kalman Filtering
13 - 2Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 3Fundamentals of Kalman Filtering:A Practical Approach
Theoretical Equations
13 - 4Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 5Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 6Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 7Fundamentals of Kalman Filtering:A Practical Approach
Falling Object Revisited
13 - 8Fundamentals of Kalman Filtering:A Practical Approach
Radar Tracking Object With Unknown Drag
g
6000 Ft/Sec
200,000 Ft
(Known)
Drag
(Unknown)
13 - 9Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 10Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 11Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 12Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 13Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 14Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 15Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter For FallingObject Problem
13 - 16Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 17Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter-2
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
13 - 18Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter-3
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
13 - 19Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 20Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter-5
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
13 - 21Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter-6Discrete measurement noise matrix is a scalar
Rk = E(vkvkT) Rk = !v
2
Continuous process noise matrix
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
13 - 22Fundamentals of Kalman Filtering:A Practical Approach
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
)
13 - 23Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 24Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 25Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 26Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 27Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 28Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 29Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 30Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 31Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 32Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 33Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 34Fundamentals of Kalman Filtering:A Practical Approach
Cannon Launched Projectile Revisited
13 - 35Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 36Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 37Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 38Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 39Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 40Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 41Fundamentals of Kalman Filtering:A Practical Approach
Linearized Cartesian Kalman Filter
13 - 42Fundamentals of Kalman Filtering:A Practical Approach
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! +...
13 - 43Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 44Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 45Fundamentals of Kalman Filtering:A Practical Approach
Developing a Linearized Cartesian Kalman Filter-4
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
13 - 46Fundamentals of Kalman Filtering:A Practical Approach
Developing a Linearized Cartesian Kalman Filter-5
Discrete measurement noise matrix
Rk = E(vkvkT) Rk =
!"
20
0 !r2
Continuous process noise matrix
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
13 - 47Fundamentals of Kalman Filtering:A Practical Approach
Developing a Linearized Cartesian Kalman Filter-6
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
13 - 48Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 49Fundamentals of Kalman Filtering:A Practical Approach
MATLAB Linearized Kalman Filter for TrackingCannon Launched Projectile-2
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
13 - 50Fundamentals of Kalman Filtering:A Practical Approach
MATLAB Linearized Kalman Filter for TrackingCannon Launched Projectile-3
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
13 - 51Fundamentals of Kalman Filtering:A Practical Approach
MATLAB Linearized Kalman Filter for TrackingCannon Launched Projectile-4
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
13 - 52Fundamentals of Kalman Filtering:A Practical Approach
MATLAB Linearized Kalman Filter for TrackingCannon Launched Projectile-5
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
13 - 53Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 54Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 55Fundamentals of Kalman Filtering:A Practical Approach
Altitude Errors in the Estimates are Within theTheoretical Bounds for Perfect Nominal Trajectory
-600
-400
-200
0
200
400
600
140120100806040200
Time (Sec)
Perfect Nominal Trajectory
Simulation
Theory
Theory
13 - 56Fundamentals of Kalman Filtering:A Practical Approach
Altitude Velocity Errors in the Estimates are Withinthe Theoretical Bounds for Perfect Nominal
Trajectory
-20
-10
0
10
20
140120100806040200
Time (Sec)
Perfect Nominal Trajectory
Simulation
Theory
Theory
13 - 57Fundamentals of Kalman Filtering:A Practical Approach
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
13 - 58Fundamentals of Kalman Filtering:A Practical Approach
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)
100 Ft/Sec Velocity Error
Simulation
Theory
Theory
13 - 59Fundamentals of Kalman Filtering:A Practical Approach
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)
200 Ft/Sec Velocity Error
Simulation
Theory
Theory
13 - 60Fundamentals of Kalman Filtering:A Practical Approach
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