kfilter.m

kalman滤波的程序及ppt

function [LX,LP,E]=kfilter(A,H,G,Q,R,X0,P0,Z,i)
YX=A*X0;
YP=A*P0*A'+G*Q*G';
S=H*YP*H'+R;
K=YP*H'*inv(S);
V=Z(:,i)-H*YX;
LX=YX+K*V;
LP=[eye(length(LX),length(LX))-K*H]*YP;
E=exp(-V'*inv(S)*V/2)/((2*pi)*sqrt(det(S)));