Linearized Kalman Filtering - Israel Association for...
Transcript of Linearized Kalman Filtering - Israel Association for...
![Page 1: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/1.jpg)
13 - 1Fundamentals of Kalman Filtering:A Practical Approach
Linearized Kalman Filtering
![Page 2: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/2.jpg)
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
![Page 3: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/3.jpg)
13 - 3Fundamentals of Kalman Filtering:A Practical Approach
Theoretical Equations
![Page 4: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/4.jpg)
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
![Page 5: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/5.jpg)
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
![Page 6: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/6.jpg)
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
![Page 7: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/7.jpg)
13 - 7Fundamentals of Kalman Filtering:A Practical Approach
Falling Object Revisited
![Page 8: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/8.jpg)
13 - 8Fundamentals of Kalman Filtering:A Practical Approach
Radar Tracking Object With Unknown Drag
g
6000 Ft/Sec
200,000 Ft
(Known)
Drag
(Unknown)
![Page 9: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/9.jpg)
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
![Page 10: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/10.jpg)
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
![Page 11: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/11.jpg)
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
![Page 12: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/12.jpg)
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
![Page 13: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/13.jpg)
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
![Page 14: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/14.jpg)
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
![Page 15: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/15.jpg)
13 - 15Fundamentals of Kalman Filtering:A Practical Approach
Developing A Linearized Kalman Filter For FallingObject Problem
![Page 16: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/16.jpg)
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
![Page 17: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/17.jpg)
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
![Page 18: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/18.jpg)
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
![Page 19: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/19.jpg)
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
![Page 20: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/20.jpg)
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
![Page 21: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/21.jpg)
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
![Page 22: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/22.jpg)
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
)
![Page 23: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/23.jpg)
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
![Page 24: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/24.jpg)
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
![Page 25: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/25.jpg)
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
![Page 26: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/26.jpg)
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
![Page 27: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/27.jpg)
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
![Page 28: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/28.jpg)
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
![Page 29: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/29.jpg)
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
![Page 30: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/30.jpg)
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
![Page 31: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/31.jpg)
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
![Page 32: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/32.jpg)
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
![Page 33: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/33.jpg)
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
![Page 34: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/34.jpg)
13 - 34Fundamentals of Kalman Filtering:A Practical Approach
Cannon Launched Projectile Revisited
![Page 35: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/35.jpg)
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
![Page 36: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/36.jpg)
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
![Page 37: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/37.jpg)
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
![Page 38: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/38.jpg)
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
![Page 39: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/39.jpg)
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
![Page 40: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/40.jpg)
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
![Page 41: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/41.jpg)
13 - 41Fundamentals of Kalman Filtering:A Practical Approach
Linearized Cartesian Kalman Filter
![Page 42: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/42.jpg)
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! +...
![Page 43: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/43.jpg)
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
![Page 44: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/44.jpg)
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
![Page 45: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/45.jpg)
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
![Page 46: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/46.jpg)
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
![Page 47: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/47.jpg)
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
![Page 48: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/48.jpg)
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
![Page 49: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/49.jpg)
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
![Page 50: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/50.jpg)
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
![Page 51: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/51.jpg)
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
![Page 52: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/52.jpg)
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
![Page 53: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/53.jpg)
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
![Page 54: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/54.jpg)
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
![Page 55: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/55.jpg)
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
![Page 56: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/56.jpg)
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
![Page 57: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/57.jpg)
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
![Page 58: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/58.jpg)
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
![Page 59: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/59.jpg)
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
![Page 60: Linearized Kalman Filtering - Israel Association for …iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1… · · 2017-03-16Fundamentals of Kalman Filtering: 13 - 2 A Practical](https://reader035.fdocuments.in/reader035/viewer/2022062504/5b06ddb47f8b9a5c308d8a75/html5/thumbnails/60.jpg)
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