diff --git a/sw/in_progress/inertial/scilab/README b/sw/in_progress/inertial/scilab/README index 523201b831..1c0b16f2cf 100644 --- a/sw/in_progress/inertial/scilab/README +++ b/sw/in_progress/inertial/scilab/README @@ -1,4 +1,4 @@ -tilt : +0/ tilt 1/ ahrs_euler @@ -9,3 +9,6 @@ ahrs_euler_ekf_compm: ahrs_euler_ekf_seqm: euler angles + bias estimation using ekf filter and sequential measurements ( one of each euler angle every update) + +2/ ahrs_quat + diff --git a/sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce b/sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce index b4c68e31ed..6ef600efa0 100644 --- a/sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce +++ b/sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce @@ -1,7 +1,7 @@ clear(); getf('rotations.sci'); getf('imu.sci'); -getf('ahrs_utils.sci'); +getf('ahrs_quat_utils.sci'); getf('ekf.sci'); use_sim = 1; diff --git a/sw/in_progress/inertial/scilab/ahrs_quat_utils.sci b/sw/in_progress/inertial/scilab/ahrs_quat_utils.sci new file mode 100644 index 0000000000..68ebb9f02d --- /dev/null +++ b/sw/in_progress/inertial/scilab/ahrs_quat_utils.sci @@ -0,0 +1,104 @@ + + + + + +// +// +// +// Initialisation +// +// +// +function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro) + + [AE0] = ahrs_euler_init(avg_len, accel, mag, gyro); + + [quat] = quat_of_euler(AE0(1:3)); + + X0 = [ quat AE0(4:6) ]'; + +endfunction + + + + + + + + + + +// +// +// +// Derivative of measure wrt state +// +// +// +function [H] = ahrs_quat_get_dphi_dX(X) + +DCM = dcm_of_quat(X(1:4)); +phi_err = 2 / (DCM(3,3)^2 + DCM(2,3)^2); +q0 = X(1); +q1 = X(2); +q2 = X(3); +q3 = X(4); +H = [ + (q1 * DCM(3,3)) * phi_err + (q0 * DCM(3,3) + 2 * q1 * DCM(2,3)) * phi_err + (q3 * DCM(3,3) + 2 * q2 * DCM(2,3)) * phi_err + (q2 * DCM(3,3)) * phi_err + 0 + 0 + 0 + ]'; + +endfunction + + + + + + + + +function [H] = ahrs_quat_get_dtheta_dX(X) + +DCM = dcm_of_quat(X(1:4)); +theta_err = 2 / sqrt(1 - DCM(1,3)^2); +q0 = X(1); +q1 = X(2); +q2 = X(3); +q3 = X(4); +H = [ + q2 * theta_err + -q3 * theta_err + q0 * theta_err + -q1 * theta_err + 0 + 0 + 0 + ]'; + +endfunction + +function [H] = ahrs_quat_get_dpsi_dX(X) + +DCM = dcm_of_quat(X(1:4)); +psi_err = 2 / (DCM(1,1)^2 + DCM(1,2)^2); +q0 = X(1); +q1 = X(2); +q2 = X(3); +q3 = X(4); +H = [ + (q3 * DCM(1,1)) * psi_err + (q2 * DCM(1,1)) * psi_err + (q1 * DCM(1,1) + 2 * q2 * DCM(1,2)) * psi_err + (q0 * DCM(1,1) + 2 * q3 * DCM(1,2)) * psi_err + 0 + 0 + 0 + ]'; + +endfunction \ No newline at end of file