function H=kalman_filter(H0,u,system)
H=H0;
A=[system.a 0 system.b*sin(H(4,1)) system.b*H(3,1)*cos(H(4,1)) 
   0 system.a -system.b*cos(H(4,1)) system.b*H(3,1)*sin(H(4,1)) 
   -system.e*sin(H(4,1)) system.e*cos(H(4,1)) system.d -system.e*(H(2,1)*sin(H(4,1)) +H(1,1)*cos(H(4,1)))
   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);
i_alpha_hat=system.a*H(1,1)+system.b*H(3,1)*sin(H(4,1))+system.c*u(1,1);
i_beta_hat=system.a*H(2,1)-system.b*H(3,1)*cos(H(4,1))+system.c*u(2,1);
predikce=[i_alpha_hat; i_beta_hat];
H(:,1)=predikce+K*(system.pozorovani-predikce);
H(:,2:5)=P-P*system.C'*inv(R)*system.C*P;
end