diff --git a/sw/in_progress/inertial/scilab/README b/sw/in_progress/inertial/scilab/README new file mode 100644 index 0000000000..523201b831 --- /dev/null +++ b/sw/in_progress/inertial/scilab/README @@ -0,0 +1,11 @@ +tilt : + + +1/ ahrs_euler + +ahrs_euler_ekf_compm: + euler angles + bias estimation using ekf filter and complete measurements ( all 3 euler angles for every update) + +ahrs_euler_ekf_seqm: + euler angles + bias estimation using ekf filter and sequential measurements ( one of each euler angle every update) + diff --git a/sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce b/sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce new file mode 100644 index 0000000000..28212e2e97 --- /dev/null +++ b/sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce @@ -0,0 +1,92 @@ +clear(); +getf('rotations.sci'); +getf('imu.sci'); +getf('ahrs_euler_utils.sci'); +getf('ekf.sci'); + +use_sim = 1; + +if (use_sim), + getf('quadrotor.sci'); + true_euler0 = [ 0.01; 0.2; 0.4]; + 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] = ahrs_euler_init(150, accel, mag); + +// initial state covariance +P0e = 1.; +P0b = 1.; + +P0 = [ P0e 0 0 0 0 0 + 0 P0e 0 0 0 0 + 0 0 P0e 0 0 0 + 0 0 0 P0b 0 0 + 0 0 0 0 P0b 0 + 0 0 0 0 0 P0b ]; + +// process covariance noise +Qe = 0.; +Qb = 0.008; + +Q = [ Qe 0 0 0 0 0 + 0 Qe 0 0 0 0 + 0 0 Qe 0 0 0 + 0 0 0 Qb 0 0 + 0 0 0 0 Qb 0 + 0 0 0 0 0 Qb ]; + +// measure covariance noise +R = [ + 1.3^2 0. 0. + 0. 1.3^2 0. + 0. 0. 2.5^2 + ]; + +X=[X0]; +P=[P0]; +M=[X0(1:3)]; + +for i=2:length(time) + // command + rate_i_ = gyro(:,i-1) - X(4:6,i-1); + // state + Xi_ = X(:, i-1); + // state time derivative + Xdoti_ = ahrs_euler_get_Xdot(Xi_, rate_i_); + // process covariance + Pi_ = ahrs_euler_get_P(P, i-1); + // Jacobian of state time derivative wrt state + Fi_ = ahrs_euler_get_F(Xi_, rate_i_); + + [Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q); + +// X = [X Xpred]; +// P = [P Ppred]; + + measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i)); + + M = [M measure_i1]; + + err = measure_i1 - Xpred(1:3); + + H = ahrs_euler_compm_get_deuler_dX(Xpred); + + [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); + + X = [X Xup]; + P = [P Pup]; + + +end + +ahrsh_euler_display(time, X, P, M) diff --git a/sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce b/sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce new file mode 100644 index 0000000000..93ce4dcb7c --- /dev/null +++ b/sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce @@ -0,0 +1,116 @@ +clear(); +getf('rotations.sci'); +getf('imu.sci'); +getf('ahrs_euler_utils.sci'); +getf('ekf.sci'); + +use_sim = 1; + +if (use_sim), + getf('quadrotor.sci'); + true_euler0 = [ 0.01; 0.2; 0.4]; + 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 + +AHRS_STEP_PHI = 0; +AHRS_STEP_THETA = 1; +AHRS_STEP_PSI = 2; +AHRS_STEP_NB = 3; + +dt = time(2) - time(1); +[X0] = ahrs_euler_init(150, accel, mag); + +// initial state covariance +P0e = 1.; +P0b = 1.; + +P0 = [ P0e 0 0 0 0 0 + 0 P0e 0 0 0 0 + 0 0 P0e 0 0 0 + 0 0 0 P0b 0 0 + 0 0 0 0 P0b 0 + 0 0 0 0 0 P0b ]; + +// process covariance noise +Qe = 0.000; +Qb = 0.008; + +Q = [ Qe 0 0 0 0 0 + 0 Qe 0 0 0 0 + 0 0 Qe 0 0 0 + 0 0 0 Qb 0 0 + 0 0 0 0 Qb 0 + 0 0 0 0 0 Qb ]; + +// measure covariance noise +R = [ + 1.3^2 0. 0. + 0. 1.3^2 0. + 0. 0. 2.5^2 + ]; + +X=[X0]; +P=[P0]; +M=[X0(1:3)]; + +for i=2:length(time) + // command + rate_i_ = gyro(:,i-1) - X(4:6,i-1); + // state + Xi_ = X(:, i-1); + // state time derivative + Xdoti_ = ahrs_euler_get_Xdot(Xi_, rate_i_); + // process covariance + Pi_ = ahrs_euler_get_P(P, i-1); + // Jacobian of state time derivative wrt state + Fi_ = ahrs_euler_get_F(Xi_, rate_i_); + + [Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q); + +// X = [X Xpred]; +// P = [P Ppred]; + + ahrs_state = modulo(i, AHRS_STEP_NB); + measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i)); + select ahrs_state, + case AHRS_STEP_PHI, + measure = phi_of_accel(accel(:,i)); + estimate = Xpred(1); + H = [1 0 0 0 0 0]; + R = [1.3^2]; + case AHRS_STEP_THETA, + measure = theta_of_accel(accel(:,i)); + estimate = Xpred(2); + H = [0 1 0 0 0 0]; + R = [1.3^2]; + case AHRS_STEP_PSI, + phi = phi_of_accel(accel(:,i)); + theta = theta_of_accel(accel(:,i)); + measure = psi_of_mag(Xpred(1), Xpred(2), mag(:,i)); +// measure = psi_of_mag(phi, theta, mag(:,i)); + estimate = Xpred(3); + H = [0 0 1 0 0 0]; + R = [2.5^2]; + end + + M = [M measure_i1]; + + err = measure - estimate; + + [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); + + X = [X Xup]; + P = [P Pup]; + + +end + +ahrsh_euler_display(time, X, P, M) diff --git a/sw/in_progress/inertial/scilab/ahrs_euler_utils.sci b/sw/in_progress/inertial/scilab/ahrs_euler_utils.sci new file mode 100644 index 0000000000..3028c00629 --- /dev/null +++ b/sw/in_progress/inertial/scilab/ahrs_euler_utils.sci @@ -0,0 +1,157 @@ +// +// +// +// Initialisation +// +// +// +function [X0] = ahrs_euler_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; + avg_gyro = sum(gyro(:,1:avg_len), 'c') / avg_len; + + phi0 = phi_of_accel(avg_accel); + theta0 = theta_of_accel(avg_accel); + psi0 = psi_of_mag(phi0, theta0, avg_mag); + + + X0 = [ phi0 theta0 psi0 avg_gyro(1) avg_gyro(2) avg_gyro(3) ]'; + +endfunction + +// +// +// +// Filter +// +// +// + +function [Pi] = ahrs_euler_get_P(P, i) + Pi = P(:, 1+6*(i-1):6*i); +endfunction + +// +// +// +// Display +// +// +// +function [] = ahrsh_euler_display(time, X, P, M) + xbasc(); + subplot(3,1,1) + xtitle('Angle'); + EstAngleDeg = X(1:3,:) * 180 / %pi; + MeasAngleDeg = M * 180 / %pi; + plot2d([time; time; time]', EstAngleDeg', style=[5, 3, 2], leg="phi@theta@psi"); + plot2d([time; time; time]', MeasAngleDeg', style=[0, 0, 0]); + + subplot(3,1,2) + xtitle('Bias'); + EstBiasDeg = X(4:6,:) * 180 / %pi; + plot2d([time; time; time]', EstBiasDeg', style=[5, 3, 2], leg="phi@theta@psi"); + + subplot(3,1,3) + xtitle('Covariance'); + + Pdiag = []; + for i=1:length(time) + Pi = ahrs_euler_get_P(P, i); + Pdiag = [Pdiag [Pi(1,1) Pi(2,2) Pi(3,3) Pi(4,4) Pi(5,5) Pi(6,6)]']; + end + plot2d([time; time; time; time; time; time]', Pdiag', style=[5, 3, 2, 0, 0, 0],... + leg="phi@theta@psi@bp@bq@br"); + + +endfunction + +// +// +// +// Time derivative of state +// +// +// +function [Xdot] = ahrs_euler_get_Xdot(X, rate) + + phi = X(1); + theta = X(2); + psi = X(3); + + euler_dot = [ 1 sin(phi)*tan(theta) cos(phi)*tan(theta) + 0 cos(phi) -sin(phi) + 0 sin(phi)/cos(theta) cos(phi)/cos(theta) + ] * rate; + Xdot = [ euler_dot; 0; 0; 0]; + +endfunction + + +// +// +// +// Derivative of Xdot wrt X +// +// +// +function [F] = ahrs_euler_get_F(X, rate) + phi = X(1); + theta = X(2); + psi = X(3); + + p = rate(1); + q = rate(2); + r = rate(3); + + d_phidot_dX = [ cos(phi)*tan(theta)*q - sin(phi)*tan(theta)*r + 1/(1+theta^2) * (sin(phi)*q+cos(phi)*r) + 0 + -1 + -sin(phi)*tan(theta) + -cos(phi)*tan(theta) ]'; + + d_thetadot_dX = [ -sin(phi)*q - cos(phi)*r + 0 + 0 + 0 + -cos(phi) + sin(phi) ]'; + + d_psidot_dX = [ cos(phi)/cos(theta)*q - sin(phi)/cos(theta)*r + sin(theta)/cos(theta)^2*(sin(phi)*q+cos(phi)*r) + 0 + 0 + -sin(phi)/cos(theta) + -cos(phi)/cos(theta) + ]'; + + F = [ d_phidot_dX + d_thetadot_dX + d_psidot_dX + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + ]; + +endfunction + + + +// +// +// +// Derivative of measure wrt state +// +// +// +function [H] = ahrs_euler_compm_get_deuler_dX(X) + +H = [ + 1 0 0 0 0 0 + 0 1 0 0 0 0 + 0 0 1 0 0 0 + ]; + +endfunction diff --git a/sw/in_progress/inertial/scilab/ahrs.sce b/sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce similarity index 100% rename from sw/in_progress/inertial/scilab/ahrs.sce rename to sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce diff --git a/sw/in_progress/inertial/scilab/ahrs_utils.sci b/sw/in_progress/inertial/scilab/ahrs_utils.sci deleted file mode 100644 index c7a58cdaab..0000000000 --- a/sw/in_progress/inertial/scilab/ahrs_utils.sci +++ /dev/null @@ -1,155 +0,0 @@ - - -// -// -// -// 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 - - - - -// -// -// -// Initialisation -// -// -// -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; -avg_gyro = sum(gyro(:,1:avg_len), 'c') / avg_len; - -phi0 = phi_of_accel(avg_accel); -theta0 = theta_of_accel(avg_accel); -psi0 = psi_of_mag(phi0, theta0, avg_mag); - -[quat] = quat_of_euler([phi0, theta0, psi0]) - -X0 = [ quat avg_gyro(1) avg_gyro(2) avg_gyro(3) ]'; - -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 96e1d7dc49..c8f8f86007 100644 --- a/sw/in_progress/inertial/scilab/ekf.sci +++ b/sw/in_progress/inertial/scilab/ekf.sci @@ -9,8 +9,8 @@ function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q) X1 = X0 + X0dot * dt; -Pdot = F*P + P*F' + Q; -P1 = P0 + Pdot * dt; +P0dot = F*P0 + P0*F' + Q; +P1 = P0 + P0dot * dt; endfunction @@ -24,8 +24,8 @@ endfunction function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q) X1 = X0 + X0dot * dt; -Pdot = F*P*F' + Q; -P1 = P0 + Pdot * dt; +P0dot = F*P0*F' + Q; +P1 = P0 + P0dot * 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); -P1 = P0 - K * H * P; +E = H * P0 * H' + R; +K = P0 * H' * inv(E); +P1 = P0 - K * H * P0; 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 f8967f0348..d64a17ed19 100644 --- a/sw/in_progress/inertial/scilab/imu.sci +++ b/sw/in_progress/inertial/scilab/imu.sci @@ -104,6 +104,17 @@ function [psi] = psi_of_mag(phi, theta, mag) psi = -atan( me, mn ); endfunction +function [euler] = euler_of_accel_mag(accel, mag) + + phi = phi_of_accel(accel); + theta = theta_of_accel(accel); + psi = psi_of_mag(phi, theta, mag); + + euler = [ phi; theta; psi]; + +endfunction + + function [m_eulers] = ahrs_compute_euler_measurements(accel, mag) m_eulers = []; [m n] = size(accel); diff --git a/sw/in_progress/inertial/scilab/tilt_ekf.sce b/sw/in_progress/inertial/scilab/tilt_ekf.sce index 3aa6445282..57ba970284 100644 --- a/sw/in_progress/inertial/scilab/tilt_ekf.sce +++ b/sw/in_progress/inertial/scilab/tilt_ekf.sce @@ -15,49 +15,54 @@ if (use_sim), [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_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]; +M=[X0(1)]; for i=1:length(time)-1 true_rate = gyro(1, i) - X(2,i); - Xdot = [ true_rate - 0 ]; + X0dot = [ true_rate + 0 ]; // Jacobian of Xdot wrt X F = [ 0 -1 0 0 ]; // Process covariance noise - Q = [ 0 0 + Q = [ 1e-5 0 0 8e-3 ]; - - [Xpred, Ppred] = ekf_predict_continuous(X(:, i), Xdot, dt, P, F, Q); + X0 = X(:, i); + P0 = tilt_get_P(P, i); + [Xpred, Ppred] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q); +// [Xpred, Ppred] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q); // X = [X Xpred]; +// P = [P Ppred]; - measure = theta_of_accel(accel(:,i)); - err = Xpred(1); - // Jacobian of measurement wrt state + measure = phi_of_accel(accel(:,i+1)); + M = [M measure]; + err = measure - Xpred(1); + // Jacobian of measurement wrt X H = [ 1 0 ]; // Measurement covariance noise R = 0.3; [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); X = [X Xup]; + P = [P Pup]; end -tilt_display(time, X, P); +tilt_display(time, X, P, M); diff --git a/sw/in_progress/inertial/scilab/tilt_utils.sci b/sw/in_progress/inertial/scilab/tilt_utils.sci index 2ab0d6be66..59a6f30ef9 100644 --- a/sw/in_progress/inertial/scilab/tilt_utils.sci +++ b/sw/in_progress/inertial/scilab/tilt_utils.sci @@ -11,23 +11,41 @@ function [X0, P0] = tilt_init(avg_len, accel, gyro) P0 = [ 1 0 0 1 ]; - endfunction +function [Pi] = tilt_get_P(P, i) + + Pi = P(:, 1+2*(i-1):2+2*(i-1)); + +endfunction -function [] = tilt_display(time, X, P) + +function [] = tilt_display(time, X, P, M) xbasc(); -subplot(2,1,1) +subplot(3,1,1) xtitle('Angle'); -plot2d([time]', X(1,:)', style=[5]); +EstAngleDeg = X(1,:) * 180 / %pi; +MeasAngleDeg = M * 180 / %pi; +plot2d([time; time]', [EstAngleDeg; MeasAngleDeg]', style=[5, 3], leg="estimated@measure"); -subplot(2,1,2) +subplot(3,1,2) xtitle('Bias'); -plot2d([time]', X(2,:)', style=[5]); +EstBiasDeg = X(2,:) * 180 / %pi; +plot2d([time]', EstBiasDeg', style=[5]); +subplot(3,1,3) +xtitle('Covariance'); +P11 = []; +P22 = []; +for i=1:length(time) + P11 = [P11 P(1, 1+2*(i-1))]; + P22 = [P22 P(2, 2+2*(i-1))]; +end +plot2d([time; time]', [P11; P22]', style=[5 3], leg="angle@bias"); + endfunction \ No newline at end of file