Revision 1351, 0.7 kB
(checked in by zimamiro, 13 years ago)
|
|
Rev | Line | |
---|
[1351] | 1 | function H=kalman_filter(H0,u,system) |
---|
| 2 | H=H0; |
---|
| 3 | A=[system.a 0 system.b*sin(H(4,1)) system.b*H(3,1)*cos(H(4,1)) |
---|
| 4 | 0 system.a -system.b*cos(H(4,1)) system.b*H(3,1)*sin(H(4,1)) |
---|
| 5 | -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))) |
---|
| 6 | 0 0 system.deltat 1]; |
---|
| 7 | |
---|
| 8 | P=A*H(:,2:5)*A'+system.Q; |
---|
| 9 | R=system.C*P*system.C'+system.R; |
---|
| 10 | K=P*system.C'*inv(R); |
---|
| 11 | i_alpha_hat=system.a*H(1,1)+system.b*H(3,1)*sin(H(4,1))+system.c*u(1,1); |
---|
| 12 | i_beta_hat=system.a*H(2,1)-system.b*H(3,1)*cos(H(4,1))+system.c*u(2,1); |
---|
| 13 | predikce=[i_alpha_hat; i_beta_hat]; |
---|
| 14 | H(:,1)=predikce+K*(system.pozorovani-predikce); |
---|
| 15 | H(:,2:5)=P-P*system.C'*inv(R)*system.C*P; |
---|
| 16 | end |
---|