root/applications/dual/SIDP/bakalarka/SIDPS/matlab/motor/kalman_filter.asv @ 1351

function H=kalman_filter(H0,system)
H=zeros(size(H0));
A=[system.a 0 system.b*sin(H(1,4)) system.b*H(1,3)*cos(H(1,4)) 
   0 system.a -system.b*cos(H(1,4)) system.b*H(1,3)*sin(H(1,4)) 
   -system.e*sin(H(1,4)) system.e*cos(H(1,4)) system.d -system.e*(H(1,2)*sin(H(1,4)) +H(1,1)*cos(H(1,4)))
   0 0 system.deltat 1];

P=A*H(2:5,:)*A'+system.Q;
R=system.C*P*system.C'+system.R;
K=P*system.C'*inv(R);
H(1,:)=(H0(1,:)'-K*(system.x1([1 2])-get_next(system,H0(1,:)',u)))';
H(2:5,:)=P-P*system.C'*inv(R)*system.C*P;
end