// Kalman filter - discrete // Example of Don Simon article, the trajectory of a projectile // discretized qith period T. // variables: p - position, v - velocity, a - acceleration // The filter is applied 'a posteriori' to the signals, but // could be imbricated with the system simulation // // // // // // Linear model Xk+1 = A Xk + B u + w Yk = C Xk + z w and z are the model and measurement noises, both gaussian N(0,s).T=0.1 // sampling interval p0=5.0 // initial position v0=0.0 // initial velocity a=0.1 // acceleration iters=200 // number of iterations wvar=0.5*[0.1; 0.03] // vars for position and velocity errors Mvar=diag(wvar) // needed as diag matrix zvar=3 // 0.5 // measurement var A=[1 T ; 0 1] // ideal model matrices B=[T**2/2 ; T] C=[1 0] u=[a] // constant acceleration inputp=[p0]; v=[v0]; y=[p0] // vectors with initial // position, velocity and output x=[p0; v0] // initial state vector // // // // // // System simulation x(k+1)=A*x(k)+B*u(k)+w y(k)=C*x(k)+z w and z are random variablesfor k=2:iters do xnew=A*x+B*u+Mvar*rand(2,1,'normal') // rand gives N(0,1) y(k)=C*xnew+zvar*rand(1,1,'normal') p(k)=xnew(1) // saves the calculated values v(k)=xnew(2) x=xnew // update of state vector end // n allows to correct time index (starts at n=0) //n=0:(iters-1) //clf //plot(n,[p y v]) Sw=wvar*wvar' // Creates covariance matrices Sz=zvar*zvar' Pk=zeros(2,2) // initialize Pk xhat=[p0; v0] // initial state vector estimate phat=[p0]; vhat=[v0]// // // //Kalman filter, using the already calculated values Kk=A*Pk*C'*inv(C*Pk*C'+Sz) xhatnew=(A*xhat+B*u)+Kk*(y-C*xhat) Pknew=A*Pk*A'+Sw-A*Pk*C'*inv(Sz)*C*Pk*A'for k=2:iters do Kk=A*Pk*C'*inv(C*Pk*C'+Sz) xhatnew=(A*xhat+B*u)+Kk*(y(k)-C*xhat) Pknew=A*Pk*A'+Sw-A*Pk*C'*inv(Sz)*C*Pk*A' phat(k)=xhatnew(1) // saves the calculated values vhat(k)=xhatnew(2) xhat=xhatnew Pk=Pknew end // n allows to correct time index (starts at n=0) n=0:(iters-1) clf //plot(n,[p y v]) //plot(n,[v vhat]) plot(n,[ y p ])