Revision 1255, 0.5 kB
(checked in by zimamiro, 14 years ago)
|
|
Line | |
---|
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'/(R);
|
---|
11 | predikce=model(H(:,1),u,system);
|
---|
12 | H(:,1)=predikce+K*(system.pozorovani-predikce([1 2]));
|
---|
13 | H(:,2:5)=P-P*system.C'/R*system.C*P;
|
---|
14 | end
|
---|
15 |
|
---|