Kalman.m function[xt, Rx, ll] [xt, Rx, ll] function Kalman(xt,yt,A,C,Rw,Rv,Rx) % yp=C*xt predikovan v stupy Ry Ry Rv+C*Rx*C' kovariance inovace asov updt stavu Rx Rx Rx-Rx*C'*inv(Ry)*C*Rx datov updt kov stavu datov updt kov stavu ey ey yt-yp inovace inovace KG KG Rx*C'*inv(Rv) Kalman v gain xt xt xt+KG*ey datov updt stavu datov updt stavu yp yp yt-C*xt K K Rx*C'*inv(Ry) asov updt kov stavu asov updt kov stavu ll ll -1/2*log(det(Ry))-1/2*yp'*inv(Ry)*yp function[xt,Rx,ll]=Kalman(xt,yt,A,C,Rw,Rv,Rx) %yp=C*xt;%predikovan�v�stupy %Ry=Rv+C*Rx*C';%kovarianceinovace %Rx=Rx-Rx*C'*inv(Ry)*C*Rx;%datov�updtkov.stavu %ey=yt-yp;%inovace % %KG=Rx*C'*inv(Rv);%Kalman�vgain %xt=xt+KG*ey;%datov�updtstavu yp=yt-C*xt; Ry=C*Rx*C'+Rv; K=Rx*C'*inv(Ry); xt=xt+K*yp; Rx=(eye(length(C))-K*C)*Rx; xt=A*xt;%�asov�updtstavu Rx=Rw+A*Rx*A';%�asov�updtkov.stavu ll=-1/2*log(det(Ry))-1/2*yp'*inv(Ry)*yp; %keyboard