Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-07-10 16:15:27 +00:00
parent 7c52b03213
commit 2f55ba6b16
6 changed files with 149 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce
+20 -7
View File
@@ -5,6 +5,9 @@ getf('ahrs_euler_utils.sci');
getf('ekf.sci');
use_sim = 1;
predict_only = 0;
predict_discrete = 0;
use_state_for_psi = 0;
if (use_sim),
rand('seed', 0);
@@ -15,9 +18,9 @@ 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_bug";
//filename = "../data/log_ahrs_roll";
//filename = "../data/log_ahrs_yaw_pitched";
filename = "../data/log_ahrs_yaw_pitched";
[time, accel, mag, gyro] = imu_read_log(filename);
end
@@ -36,7 +39,7 @@ P0 = [ P0e 0 0 0 0 0
0 0 0 0 0 P0b ];
// process covariance noise
Qe = 0.;
Qe = 0.0;
Qb = 0.008;
Q = [ Qe 0 0 0 0 0
@@ -57,7 +60,6 @@ X=[X0]; // state
P=[P0]; // state covariance
M=[X0(1:3)]; // measurements
predict_only = 0;
for i=2:length(time)
// command
@@ -71,9 +73,20 @@ for i=2:length(time)
// 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);
measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
if predict_discrete
[Xpred, Ppred] = ekf_predict_discrete(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
else
[Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
end
if use_state_for_psi
m_phi = phi_of_accel(accel(:, i));
m_theta = theta_of_accel(accel(:, i));
m_psi = psi_of_mag(Xpred(1), Xpred(2), mag(:, i));
measure_i1 = [ m_phi; m_theta; m_psi];
else
measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
end
M = [M measure_i1];
sw/in_progress/inertial/scilab/calibrate_imu.sce
+29
View File
@@ -0,0 +1,29 @@
clear();
getf('rotations.sci');
getf('imu.sci');
rand('seed', 0);
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_misaligned(time, true_rates, true_eulers);
xbasc();
true_rates_deg = deg_of_rad(true_rates);
subplot(2,1,1)
plot2d([time]', true_rates_deg', style=[5, 3, 2], leg="rate_p@rate_q@rate_r");
xtitle( 'True rates', 's', 'deg/s') ;
subplot(2,1,2)
plot2d([time]', gyro', style=[5, 3, 2], leg="g_x@g_y@g_z");
xtitle( 'Raw gyros', 's', 'adc') ;
sw/in_progress/inertial/scilab/ekf.sci
+26 -2
View File
@@ -24,7 +24,8 @@ endfunction
function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q)
X1 = X0 + X0dot * dt;
P0dot = F*P0*F' + Q;
expF = expm(dt * F);
P0dot = expF*P0*expF' + Q;
P1 = P0 + P0dot * dt;
endfunction
@@ -43,4 +44,27 @@ K = P0 * H' * inv(E);
P1 = P0 - K * H * P0;
X1 = X0 + K * err;
endfunction
endfunction
//
// Pade approximation of matrix exponential
//
function [expA] = mat_exp(A, epsilon)
normA = norm(A, 'inf');
//Ns = max(0, int(normA)) ???
Ns = normA;
ki = 2^(-Ns) * A;
i=1;
// foo = 2^(3-2*i)*
expA = A;
endfunction
sw/in_progress/inertial/scilab/imu.sci
+29
View File
@@ -79,6 +79,35 @@ function [accel, mag, gyro] = imu_sim(time, rates, eulers)
endfunction
function [accel, mag, gyro] = imu_sim_misaligned(time, rates, eulers)
gyro_supply = 5.;
gyro_neutral = [512; 512; 512];
gyro_gains = rad_of_deg([ 300/512; 300/512; 300/512]);
gyro_sigma_noise = [ 3; 3; 3];
gyro_offset_deg = [0 0 0
0 0 0
0 0 0 ];
gyro_orientation = [] ;
gyro_align = [ 1 0 0
0.1 1 0
0 0 1 ];
accel = [];
mag = [];
gyro = [];
for i=1:length(time),
gyro_noise = rand(3, 1, "normal") .* gyro_sigma_noise;
// gyro_noise = [ 0; 0; 0];
g = gyro_neutral + (gyro_align * rates(:, i)) ./ gyro_gains + gyro_noise;
g = round(g);
gyro = [gyro g];
end
endfunction
//
//
//
sw/in_progress/inertial/scilab/rotations.sci
+24 -7
View File
@@ -1,7 +1,22 @@
//
//
//
// Angles convertion
// Angle units
//
//
//
function [rad] = rad_of_deg(deg)
rad = deg * %pi / 180;
endfunction
function [deg] = deg_of_rad(rad)
deg = rad * 180 / %pi;
endfunction
//
//
//
// Rotation convertion
//
//
//
@@ -41,12 +56,6 @@ function [euler] = euler_of_quat(quat)
euler = [phi; theta; psi];
endfunction
function [X1] = normalise_quat(X0)
quat = X0(1:4);
quat = quat / norm(quat);
X1 = [quat];
endfunction
function [DCM] = dcm_of_euler(euler)
phi = euler(1);
@@ -88,3 +97,11 @@ DCM = [ dcm00 dcm01 dcm02
dcm20 dcm21 dcm22 ];
endfunction
function [X1] = normalise_quat(X0)
quat = X0(1:4);
quat = quat / norm(quat);
X1 = [quat];
endfunction
sw/in_progress/inertial/scilab/tilt_ekf.sce
+21 -14
View File
@@ -1,11 +1,12 @@
clear();
getf('rotations.sci');
getf('imu.sci');
getf('ekf.sci');
getf('tilt_utils.sci');
use_sim = 1;
predict_only = 0;
predict_discrete = 0;
if (use_sim),
getf('quadrotor.sci');
@@ -42,23 +43,29 @@ for i=1:length(time)-1
0 8e-3 ];
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];
if ( predict_discrete )
[Xpred, Ppred] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q);
else
[Xpred, Ppred] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q);
end
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);
if ( predict_only )
X = [X Xpred];
P = [P Ppred];
else
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];
X = [X Xup];
P = [P Pup];
end
end