| Line | |
|---|
| 1 | function [x_k, P_k] = extKF(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(5,1); |
|---|
| 15 | xp(1) = d*x(1) + e*(y(2)*cos(x(2)) - y(1)*sin(x(2))); |
|---|
| 16 | xp(2) = x(2) + dt*x(1); |
|---|
| 17 | xp(3) = pre(1); |
|---|
| 18 | xp(4) = pre(2); |
|---|
| 19 | xp(5) = pre(3); |
|---|
| 20 | yp = zeros(2,1); |
|---|
| 21 | yp(1) = a*y(1) + b*x(1)*sin(x(2)) + c*u(1); |
|---|
| 22 | yp(2) = a*y(2) - b*x(1)*cos(x(2)) + c*u(2); |
|---|
| 23 | |
|---|
| 24 | x_k = xp + K*(y - yp); |
|---|
| 25 | end |
|---|
