diff --git a/sw/in_progress/inertial/scilab/ahrs.sce b/sw/in_progress/inertial/scilab/ahrs.sce index bf492ef7ce..b4c68e31ed 100644 --- a/sw/in_progress/inertial/scilab/ahrs.sce +++ b/sw/in_progress/inertial/scilab/ahrs.sce @@ -1,7 +1,8 @@ clear(); +getf('rotations.sci'); +getf('imu.sci'); getf('ahrs_utils.sci'); getf('ekf.sci'); -getf('imu.sci'); use_sim = 1; @@ -19,10 +20,16 @@ else [time, accel, mag, gyro] = imu_read_log(filename); end + +AHRS_STEP_PHI = 0; +AHRS_STEP_THETA = 1; +AHRS_STEP_PSI = 2; +AHRS_STEP_NB = 3; + [m_eulers] = ahrs_compute_euler_measurements(accel, mag); dt = time(2) - time(1); -[X0] = ahrs_init(150, accel, mag); +[X0] = ahrs_quat_init(150, accel, mag); // initial state covariance matrix P0q = 1.; @@ -81,12 +88,42 @@ for i=1:length(time)-1 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 ]; - [X1 P] = ekf_predict_discrete(X(:, i), Xdot, dt, P, F, Q); + [X1, P] = ekf_predict_continuous(X(:, i), Xdot, dt, P, F, Q); X1 = normalise_quat(X1); - X = [X X1]; +// X = [X X1]; // P = [P P1]; - est_eulers = [est_eulers euler_of_quat(X1(1:4))']; + est_euler = euler_of_quat(X1(1:4))'; + est_eulers = [est_eulers est_euler]; + ahrs_state = modulo(i, AHRS_STEP_NB); + est_euler = euler_of_quat(X1); + H = [0;0;0;0;0;0;0]'; + measure = 0; + estimate = 0; + + select ahrs_state, + case AHRS_STEP_PHI, + measure = phi_of_accel(accel(:,i)); + estimate = est_euler(1); + H = ahrs_quat_get_dphi_dq(X1); + R = [1.3^2]; + case AHRS_STEP_THETA, + measure = theta_of_accel(accel(:,i)); + estimate = est_euler(2); + H = ahrs_quat_get_dtheta_dq(X1); + R = [1.3^2]; + case AHRS_STEP_PSI, + measure = psi_of_mag(m_eulers(1,i), m_eulers(2,i), mag(:,i)); + estimate = est_euler(3); + H = ahrs_quat_get_dpsi_dq(X1); + R = [2.5^2]; + end + + err = measure - estimate; + [X2 P] = ekf_update(X1, P, H, R, err); + + X = [X X2]; + @@ -94,12 +131,15 @@ end xbasc(); -subplot(2,1,1) +subplot(3,1,1) xtitle('Rates'); plot2d([time; time; time]', gyro', style=[5 3 2], leg="p@q@r"); -subplot(2,1,2) +subplot(3,1,2) plot2d([time;time;time]', m_eulers', style=[5 3 2], leg="phi@theta@psi"); xtitle('Angles'); plot2d([time;time;time]', est_eulers', style=[0 0 0]); +subplot(3,1,3) +xtitle('Biases'); +plot2d([time; time; time]', X(5:7, :)', style=[5 3 2], leg="p@q@r"); diff --git a/sw/in_progress/inertial/scilab/ahrs_utils.sci b/sw/in_progress/inertial/scilab/ahrs_utils.sci index ad06964436..c7a58cdaab 100644 --- a/sw/in_progress/inertial/scilab/ahrs_utils.sci +++ b/sw/in_progress/inertial/scilab/ahrs_utils.sci @@ -37,73 +37,8 @@ function [m_eulers] = ahrs_compute_euler_measurements(accel, mag) endfunction -// -// -// -// Angles convertion -// -// -// -function [quat] = quat_of_euler(euler) -phi2 = euler(1) / 2.0; -theta2 = euler(2) / 2.0; -psi2 = euler(3) / 2.0; - -sinphi2 = sin( phi2 ); -cosphi2 = cos( phi2 ); -sintheta2 = sin( theta2 ); -costheta2 = cos( theta2 ); -sinpsi2 = sin( psi2 ); -cospsi2 = cos( psi2 ); - -q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; -q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; -q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; -q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; - -quat = [q0 q1 q2 q3]; - -endfunction -function [euler] = euler_of_quat(quat) - dcm00 = 1.0 - 2*(quat(3)*quat(3) + quat(4)*quat(4)); - dcm01 = 2*(quat(2)*quat(3) + quat(1)*quat(4)); - dcm02 = 2*(quat(2)*quat(4) - quat(1)*quat(3)); - dcm12 = 2*(quat(3)*quat(4) + quat(1)*quat(2)); - dcm22 = 1.0 - 2*(quat(2)*quat(2) + quat(3)*quat(3)); - - phi = atan( dcm12, dcm22 ); - theta = -asin( dcm02 ); - psi = atan( dcm01, dcm00 ); - - euler = [phi theta psi]; -endfunction - -function [X1] = normalise_quat(X0) - quat = X0(1:4); - quat = quat / norm(quat); - X1 = [quat; X0(5:7)]; -endfunction - - -function [DCM] = dcm_of_euler(euler) - phi = euler(1); - theta = euler(2); - psi = euler(3); - dcm11 = cos(theta)*cos(psi); - dcm12 = cos(theta)*sin(psi); - dcm13 = -sin(theta); - dcm21 = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi); - dcm22 = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi); - dcm23 = sin(phi)*cos(theta); - dcm31 = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi); - dcm32 = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi); - dcm33 = cos(phi)*cos(theta); - DCM= [ dcm11 dcm12 dcm13 - dcm21 dcm22 dcm23 - dcm31 dcm32 dcm33 ]; -endfunction // // @@ -112,7 +47,7 @@ endfunction // // // -function [X0] = ahrs_init(avg_len, accel, mag, gyro) +function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro) avg_accel = sum(accel(:,1:avg_len), 'c') / avg_len; avg_mag = sum(mag(:,1:avg_len), 'c') / avg_len; @@ -132,7 +67,89 @@ endfunction // // // -// +// Filter // // // + + + + + + +// +// +// +// Display +// +// +// + + +// +// +// +// Derivative of measure wrt state +// +// +// +function [H] = ahrs_quat_get_dphi_dq(quat) + +DCM = dcm_of_quat(quat); +phi_err = 2 / (DCM(3,3)^2 + DCM(2,3)^2); +q0 = quat(1); +q1 = quat(2); +q2 = quat(3); +q3 = quat(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_dq(quat) + +DCM = dcm_of_quat(quat); +theta_err = 2 / sqrt(1 - DCM(1,3)^2); +q0 = quat(1); +q1 = quat(2); +q2 = quat(3); +q3 = quat(4); +H = [ + q2 * theta_err + -q3 * theta_err + q0 * theta_err + -q1 * theta_err + 0 + 0 + 0 + ]'; + +endfunction + +function [H] = ahrs_quat_get_dpsi_dq(quat) + +DCM = dcm_of_quat(quat); +psi_err = 2 / (DCM(1,1)^2 + DCM(1,2)^2); +q0 = quat(1); +q1 = quat(2); +q2 = quat(3); +q3 = quat(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 diff --git a/sw/in_progress/inertial/scilab/ekf.sci b/sw/in_progress/inertial/scilab/ekf.sci index 7cea243258..96e1d7dc49 100644 --- a/sw/in_progress/inertial/scilab/ekf.sci +++ b/sw/in_progress/inertial/scilab/ekf.sci @@ -10,7 +10,7 @@ function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q) X1 = X0 + X0dot * dt; Pdot = F*P + P*F' + Q; -P = P + Pdot * dt; +P1 = P0 + Pdot * dt; endfunction @@ -38,9 +38,9 @@ endfunction // function [X1, P1] = ekf_update(X0, P0, H, R, err) - E = H * P * H' + R; - K = P * H' * inv(E); - P = P - K * H * P; - X = X + K * err; +E = H * P * H' + R; +K = P * H' * inv(E); +P1 = P0 - K * H * P; +X1 = X0 + K * err; endfunction \ No newline at end of file diff --git a/sw/in_progress/inertial/scilab/imu.sci b/sw/in_progress/inertial/scilab/imu.sci index 5dc72dd8a1..f8967f0348 100644 --- a/sw/in_progress/inertial/scilab/imu.sci +++ b/sw/in_progress/inertial/scilab/imu.sci @@ -77,3 +77,42 @@ function [accel, mag, gyro] = imu_sim(time, rates, eulers) mag = [mag ma]; end endfunction + + +// +// +// +// Angles Measurements +// +// +// +function [phi] = phi_of_accel(accel) + phi = atan(accel(2), accel(3)); +endfunction + +function [theta] = theta_of_accel(accel) + theta = -asin(accel(1) / norm(accel)); +endfunction + +function [psi] = psi_of_mag(phi, theta, mag) + cphi = cos( phi ); + sphi = sin( phi ); + ctheta = cos( theta ); + stheta = sin( theta ); + mn = ctheta* mag(1)+ sphi*stheta* mag(2)+ cphi*stheta* mag(3); + me = cphi* mag(2) -sphi* mag(3); + psi = -atan( me, mn ); +endfunction + +function [m_eulers] = ahrs_compute_euler_measurements(accel, mag) + m_eulers = []; + [m n] = size(accel); + for i=1:n + phi = phi_of_accel(accel(:,i)); + theta = theta_of_accel(accel(:,i)); + psi = psi_of_mag(phi, theta, mag(:,i)); + m_eulers = [m_eulers [phi;theta;psi]]; + end + +endfunction + diff --git a/sw/in_progress/inertial/scilab/rotations.sci b/sw/in_progress/inertial/scilab/rotations.sci new file mode 100644 index 0000000000..ae4ace1f8e --- /dev/null +++ b/sw/in_progress/inertial/scilab/rotations.sci @@ -0,0 +1,90 @@ +// +// +// +// Angles convertion +// +// +// +function [quat] = quat_of_euler(euler) +phi2 = euler(1) / 2.0; +theta2 = euler(2) / 2.0; +psi2 = euler(3) / 2.0; + +sinphi2 = sin( phi2 ); +cosphi2 = cos( phi2 ); +sintheta2 = sin( theta2 ); +costheta2 = cos( theta2 ); +sinpsi2 = sin( psi2 ); +cospsi2 = cos( psi2 ); + +q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; +q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; +q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; +q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; + +quat = [q0 q1 q2 q3]; + +endfunction + + +function [euler] = euler_of_quat(quat) + dcm00 = 1.0 - 2*(quat(3)*quat(3) + quat(4)*quat(4)); + dcm01 = 2*(quat(2)*quat(3) + quat(1)*quat(4)); + dcm02 = 2*(quat(2)*quat(4) - quat(1)*quat(3)); + dcm12 = 2*(quat(3)*quat(4) + quat(1)*quat(2)); + dcm22 = 1.0 - 2*(quat(2)*quat(2) + quat(3)*quat(3)); + + phi = atan( dcm12, dcm22 ); + theta = -asin( dcm02 ); + psi = atan( dcm01, dcm00 ); + + euler = [phi theta psi]; +endfunction + +function [X1] = normalise_quat(X0) + quat = X0(1:4); + quat = quat / norm(quat); + X1 = [quat; X0(5:7)]; +endfunction + + +function [DCM] = dcm_of_euler(euler) + phi = euler(1); + theta = euler(2); + psi = euler(3); + dcm11 = cos(theta)*cos(psi); + dcm12 = cos(theta)*sin(psi); + dcm13 = -sin(theta); + dcm21 = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi); + dcm22 = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi); + dcm23 = sin(phi)*cos(theta); + dcm31 = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi); + dcm32 = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi); + dcm33 = cos(phi)*cos(theta); + DCM= [ dcm11 dcm12 dcm13 + dcm21 dcm22 dcm23 + dcm31 dcm32 dcm33 ]; +endfunction + +function [DCM] = dcm_of_quat(quat) + +q0 = quat(1); +q1 = quat(2); +q2 = quat(3); +q3 = quat(4); + +dcm00 = q0^2 + q1^2 - q2^2 - q3^2; +dcm01 = 2 * (q1*q2 + q0*q3); +dcm02 = 2 * (q1*q3 - q0*q2); +dcm10 = 2 * (q1*q2 - q0*q3); +dcm11 = q0^2 - q1^2 + q2^2 - q3^2; +dcm12 = 2 * (q2*q3 + q0*q1); +dcm20 = 2 * (q1*q3 + q0*q2); +dcm21 = 2 * (q2*q3 - q0*q1); +dcm22 = q0^2 - q1^2 - q2^2 + q3^2; + +DCM = [ dcm00 dcm01 dcm02 + dcm10 dcm11 dcm12 + dcm20 dcm21 dcm22 ]; + +endfunction diff --git a/sw/in_progress/inertial/scilab/tilt_ekf.sce b/sw/in_progress/inertial/scilab/tilt_ekf.sce new file mode 100644 index 0000000000..3aa6445282 --- /dev/null +++ b/sw/in_progress/inertial/scilab/tilt_ekf.sce @@ -0,0 +1,82 @@ +clear(); + +getf('rotations.sci'); +getf('imu.sci'); +getf('ekf.sci'); +getf('tilt_utils.sci'); + +use_sim = 1; + +if (use_sim), + getf('quadrotor.sci'); + true_euler0 = [ 0.01; 0.25; 0.5]; + dt = 0.015625; + [time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt); + [accel, mag, gyro] = imu_sim(time, true_rates, true_eulers); +else + //filename = "../data/log_ahrs_test"; + //filename = "../data/log_ahrs_bug"; + filename = "../data/log_ahrs_roll"; + //filename = "../data/log_ahrs_yaw_pitched"; + [time, accel, mag, gyro] = imu_read_log(filename); +end + + + +dt = time(2) - time(1); +[X0, P0] = tilt_init(150, accel, gyro); + +X=[X0]; +P=[P0]; + +for i=1:length(time)-1 + + true_rate = gyro(1, i) - X(2,i); + Xdot = [ true_rate + 0 ]; + // Jacobian of Xdot wrt X + F = [ 0 -1 + 0 0 ]; + // Process covariance noise + Q = [ 0 0 + 0 8e-3 ]; + + [Xpred, Ppred] = ekf_predict_continuous(X(:, i), Xdot, dt, P, F, Q); + +// X = [X Xpred]; + + measure = theta_of_accel(accel(:,i)); + err = Xpred(1); + // Jacobian of measurement wrt state + H = [ 1 0 ]; + // Measurement covariance noise + R = 0.3; + [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); + + X = [X Xup]; + +end + +tilt_display(time, X, P); + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/in_progress/inertial/scilab/tilt_utils.sci b/sw/in_progress/inertial/scilab/tilt_utils.sci new file mode 100644 index 0000000000..2ab0d6be66 --- /dev/null +++ b/sw/in_progress/inertial/scilab/tilt_utils.sci @@ -0,0 +1,33 @@ + + +function [X0, P0] = tilt_init(avg_len, accel, gyro) + + avg_accel = sum(accel(:,1:avg_len), 'c') / avg_len; + avg_gyro = sum(gyro(:,1:avg_len), 'c') / avg_len; + + X0 = [ phi_of_accel(avg_accel) + avg_gyro(1) ]; + + P0 = [ 1 0 + 0 1 ]; + + +endfunction + + + + +function [] = tilt_display(time, X, P) + +xbasc(); +subplot(2,1,1) +xtitle('Angle'); +plot2d([time]', X(1,:)', style=[5]); + +subplot(2,1,2) +xtitle('Bias'); +plot2d([time]', X(2,:)', style=[5]); + + +endfunction + \ No newline at end of file