mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:42:47 +08:00
Antoine Drouin
51 lines
544 B
Scilab
51 lines
544 B
Scilab
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q)
|
|
|
|
X1 = X0 + X0dot * dt;
|
|
P0dot = F*P0 + P0*F' + Q;
|
|
P1 = P0 + P0dot * dt;
|
|
|
|
endfunction
|
|
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q)
|
|
|
|
X1 = X0 + X0dot * dt;
|
|
expF = expm(dt * F);
|
|
P0dot = expF*P0*expF' + Q;
|
|
P1 = P0 + P0dot * dt;
|
|
|
|
endfunction
|
|
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
//
|
|
function [X1, P1] = ekf_update(X0, P0, H, R, err)
|
|
|
|
E = H * P0 * H' + R;
|
|
K = P0 * H' * inv(E);
|
|
P1 = P0 - K * H * P0;
|
|
X1 = X0 + K * err;
|
|
|
|
endfunction
|
|
|
|
|
|
|