mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
X
This commit is contained in:
Antoine Drouin
@@ -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");
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user