root/applications/dual/vahala/kim/extKF4.m @ 1436

Revision 1436, 0.6 kB (checked in by vahalam, 12 years ago)

pridani a uprava lqg s hyperstavem viz clanek Kim2006

Line 
1function [x_k, P_k] = extKF4(x, y, u, pre, A, C, P, Q, R)
2    a = 0.9898;
3    b = 0.0072;
4    c = 0.0361;
5    d = 1.0;
6    e = 0.0149;
7    dt = 0.000125;
8
9    Pp = A*P*A' + Q;
10    S = C*Pp*C' + R;
11    K = Pp*C'/S;
12    P_k = Pp - K*C*Pp;   
13   
14    xp = zeros(14,1);
15    xp(1) = a*x(1) + b*x(3)*sin(x(4)) + c*u(1);
16    xp(2) = a*x(2) - b*x(3)*cos(x(4)) + c*u(2);
17    xp(3) = d*x(3) + e*(x(2)*cos(x(4)) - x(1)*sin(x(4)));
18    xp(4) = x(4) + dt*x(3);
19    xp(5:14) = pre;
20       
21    x_k = xp + K*(y - xp(1:2));
22   
23    %max x(14) = pi^2/3 ... variance of uniform -pi,pi
24%     if(x_k(14) > pi^2/3)
25%         x_k(14) = pi^2/3;
26%     end
27end
Note: See TracBrowser for help on using the browser.