Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-07-04 14:54:51 +00:00
parent c080d5a155
commit 11256ac081
10 changed files with 436 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/inertial/scilab/README
+11
View File
@@ -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)
sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce
+92
View File
@@ -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)
sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce
+116
View File
@@ -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)
sw/in_progress/inertial/scilab/ahrs_euler_utils.sci
+157
View File
@@ -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
sw/in_progress/inertial/scilab/ahrs.sce → sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce
View File
sw/in_progress/inertial/scilab/ahrs_utils.sci
-155
View File
@@ -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
sw/in_progress/inertial/scilab/ekf.sci
+8 -8
View File
@@ -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
sw/in_progress/inertial/scilab/imu.sci
+11
View File
@@ -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);
sw/in_progress/inertial/scilab/tilt_ekf.sce
+17 -12
View File
@@ -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);
sw/in_progress/inertial/scilab/tilt_utils.sci
+24 -6
View File
@@ -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