Kalman Filtering

Originally developed for use in spacecraft navigation, the Kalman filter turns out to be useful for many applications. It is mainly used to estimate system states that can only be observed indirectly or inaccurately by the system itself.74Embedded Systems ProgrammingEsEmbedded Systems Programmingfunction kaLman (duration dt)function kaLman (duration, dt)7 KaLman filter simulation for a vehicle travelling along a roadINPUT9 duration length of simulation (seconds)9 dt step size (seconds)measnoise 10: i position measurement noise (feet)acceLnoise =0.2/% acceleration noise (feet/sec 2)a=[ dt:01:% transition matrixb= Lat 2/2i dt]: input matrixc=[1 01:% measurement matrixx= CO: 0]: initial state vectorxhat =x: initial state estimateSz measnoise 2. measurement error covarianceSw accelnaise 2 [dt 4/4 dt 3/2: dt 3/2 dt 2]: process noise covP= Sw: initial estimation covarianceInitialize arrays for Later plottingpos Li t true position arrayposhat =[1:% estimated position arrayposmeas []: measured position arrayvel =L]; 1 true velocity arrayve Lhat L]: estimated velocity arrayfor t =0. dt: durationd Use a constant commanded acceleration of 1 foot/sec 2Simulate the linear systemProcessNoise acce noise [(dt 2/2)*randn dt*randnJ;x a*x+b*u+ ProcessNoise:Simulate the noisy measurementMeasNoi sey =c*x+ MeasNoiseExtrapolate the most recent state estimate to the present timexhat=a★xhat+bForm the innovation vectorInn =y-c xhatCompute the covariance of the Innovationg Form the Ka lman Gain matrixKUpdate the state estimatexhat xhat K Inn.Z Compute the covariance of the estimation errorListing 1 cont inued on p76Embedded Systems ProgrammingEsEmbedded Systems ProgrammingP=a★P*a'-a*P*c"★inv(s)★c★P头a'+SMSave some parameters for plotting laterpos [: x(1)J;posmeas Lposmeas: yIshat Lposhati xhat(1)]vel [; X(2)J;velha [ve lhat: xhat(2)]dPlot the resultscLose aLLit=0: dtduration;oLot(t, pos, t, posmeas, t, poshat)ixLabeL( Time (sec)1)('Position (feet)1)title(' Figure 1 -Vehicle Position (True, Measured, and Estimated))figureplot(t, pos-posmeas, t, pos-poshat)xLabeL(Time (sec)');LabeL('Position Error (feet)')title(' figure 2- Position Measurement Error and position Est imation Error)plot(t, vel, t, velha)grid;xLabeL(I Time (sec))yLabeL('Veloci ty (feet/sec)')ititle('Figure 3-Velocity (True and estimated)')plot(t, vel-ve Lhat)gr1xLabeL( Time (sec)i)yLabeL('Veloci ty Error (feet/sec))title(' Figure 4-Velocity Estimation Error ')espEmbedded Systems Programming// The fol lowing code snippet assumes that the linear system has n states,m/ inputs, and r outputs. Therefore, the fol lowing variables are assumed to/ already be defined// A is an n by n matrix//b is an n by m matrixby n matrix//y is an r by 1 vectorFor further reading//u is an m by 1 vectornr by r matrix// Sw is an n by n matrix//P is an n byfloat AP[n][n]/ this is the matrix A*Pfloat CTOr]/ This is the matrix CTfloat APCTEn]Cr]/ This is the matrix A*P*CTfloat CPCr]En]/ This is the matrix c*pfloat CPCT[r]Cr]/ This is the matrix C*P*CTfloat CPCTSzEr ][r]This is theC*P★CT+Szfloat CPCtszInyr ]r]/ This is the matrix (c*P*CT+Sz)-1float KLn]r]/7 This is the Kalman gain.float Chater正[1/ This is the vector C*xhatfloat yCxhatLr]C1]/ This is the-C*xhatfloat Ky[nJL11/7 This is the vector K*(y-Cxhat)float AxhatCn][1]/ This is the vector A*xhatfloat buln卫1]/ This is the vector B*ufloat AxhatBuLnIC1J;/ This is the vector A*xhat+B*ufloat ATEn]Cn]// Ththe matrix afloat APATEn][n]// This is the matrix aP*ATfloat APATSwLnJEn]/ This is the matrix A*P*A+Swfloat CPAtLr][n]/ This is the matrix c为P★ATfloat SzInvCr]Cr]/ This is the matrix Sz-1float APCTSzInyEnlcr]/ This is the matrix A*P*CT*Sz1float APCTSzInVCPATEnJCn]// This is the matrix a为 P*CT*SZ-1+C木PkAT/7 The fol lowing sequence of function cal ls computes the K matrixMatrixMultiply((float*)A,(f loat*)P, n, n, n,(f loat* )AP)MatrixTran((float*)c(fLoat*)CT)MatrixMultiply((f loat* )AP,(float*)CT, n, n, rr(float*)APCT)MatrixMultiply((f loat*)C,(float)P,r, n, n,(f loat*)CP)MatrixMultiply((float*)CP, (f loat*)CT, r, n, r, (float*)CPCT)MatrixAddition (( f loat*)CPcT. (fLoat* )Sz, r. r, (float*)CPCTSz)MatrixInversion ((float*)CPCTSz, r, (float*)CPCTSzInv)MatrixMultiply(float*)APCT, (fLoat*)CPCTSzInv, n, r, r, (f loat*)K)// The following sequence of function calls updates the xhat vector.MatrixMultiply((float*),(f loat* )xhat, r, n,1,(fLoat)Cxhat)MatrixSubtraction((f loat* y, (float* )Cxhat, r, l,(float*)yCxhat)MatrixMultiply ((f loat* )k (f loat*)yCxhat, n, r, 1, (float*) Ky Cxhat)MatrixMultiply((f loat)A, (f loat*)xhat, n, n, 1,(float* )Axhat);MatrixMultiply((float*)B,(float* u, n, rrl,(float* )Bu);dition((float* )Axhat,(float)Bu, n, 1,(float)AxhatBu)MatrixAddition((f loat*)AxhatBu, (float*)Ky Cxhat, n, 1,(float*)xhat)/7 The following sequence of function calls updates the P matrix.MatrixTranspose(float*)A, n, n,(float*)AT)MatrixMultiply(float*AP,(float*)AT, n, n, n,(float*)APAT)MatrixAddition ((f loat* )APAT,(fLoat*)Sw,n, n, (float*)APATSw)MatrixTranspose((float*)APCT, n, r, (fLoat*)CPAT)MatrixInversion((f loat*)Sz, r, (f loat*)SzInv)MatrixMultiply((f loat*)APCT, (fLoat*)SzInv, n,r, r,(float* )APCTSzInv)MatrixMultiply((f loat*)APCTSzInv,(float*)CPAT,n, r, nr(float* )APCTSzInvCPAT)Matrixsubtraction ((float* )APatsw float*) APCTSz Invcpat. nn. (float*)P)Embedded Systems Programming79