|
Revision 1436, 0.6 kB
(checked in by vahalam, 14 years ago)
|
|
pridani a uprava lqg s hyperstavem viz clanek Kim2006
|
| Line | |
|---|
| 1 | function [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 |
|---|
| 27 | end |
|---|
