mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -0,0 +1,105 @@
|
||||
clear();
|
||||
getf('ahrs_utils.sci');
|
||||
getf('ekf.sci');
|
||||
getf('imu.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
|
||||
|
||||
[m_eulers] = ahrs_compute_euler_measurements(accel, mag);
|
||||
|
||||
dt = time(2) - time(1);
|
||||
[X0] = ahrs_init(150, accel, mag);
|
||||
|
||||
// initial state covariance matrix
|
||||
P0q = 1.;
|
||||
P0b = .1;
|
||||
|
||||
P0 = [ P0q 0 0 0 0 0 0
|
||||
0 P0q 0 0 0 0 0
|
||||
0 0 P0q 0 0 0 0
|
||||
0 0 0 P0q 0 0 0
|
||||
0 0 0 0 P0b 0 0
|
||||
0 0 0 0 0 P0b 0
|
||||
0 0 0 0 0 0 P0b ];
|
||||
|
||||
|
||||
Qq = 0.;
|
||||
Qb = 0.008;
|
||||
|
||||
Q = [ Qq 0 0 0 0 0 0
|
||||
0 Qq 0 0 0 0 0
|
||||
0 0 Qq 0 0 0 0
|
||||
0 0 0 Qq 0 0 0
|
||||
0 0 0 0 Qb 0 0
|
||||
0 0 0 0 0 Qb 0
|
||||
0 0 0 0 0 0 Qb ];
|
||||
|
||||
X=[X0];
|
||||
est_eulers = [euler_of_quat(X0(1:4))'];
|
||||
//P=[P0];
|
||||
P = P0;
|
||||
|
||||
for i=1:length(time)-1
|
||||
|
||||
q0 = X(1, i);
|
||||
q1 = X(2, i);
|
||||
q2 = X(3, i);
|
||||
q3 = X(4, i);
|
||||
|
||||
p = gyro(1,i) - X(5, i);
|
||||
q = gyro(2,i) - X(6, i);
|
||||
r = gyro(3,i) - X(7, i);
|
||||
|
||||
OMEGA = 1/2 * [ 0 -p -q -r 0 0 0
|
||||
p 0 r -q 0 0 0
|
||||
q -r 0 p 0 0 0
|
||||
r q -p 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 ];
|
||||
Xdot = OMEGA * X(:,i);
|
||||
|
||||
F = 1/2 * [ 0 -p -q -r q1 q2 q3;
|
||||
p 0 r -q -q0 q3 -q2;
|
||||
q -r 0 p -q3 q0 q1;
|
||||
r q -p 0 q2 -q1 -q0;
|
||||
0 0 0 0 0 0 0;
|
||||
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 = normalise_quat(X1);
|
||||
X = [X X1];
|
||||
// P = [P P1];
|
||||
est_eulers = [est_eulers euler_of_quat(X1(1:4))'];
|
||||
|
||||
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
xbasc();
|
||||
subplot(2,1,1)
|
||||
xtitle('Rates');
|
||||
plot2d([time; time; time]', gyro', style=[5 3 2], leg="p@q@r");
|
||||
|
||||
subplot(2,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]);
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// 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
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// 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
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Initialisation
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X0] = ahrs_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
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
@@ -0,0 +1,46 @@
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,79 @@
|
||||
//
|
||||
//
|
||||
//
|
||||
// Log parsing
|
||||
//
|
||||
//
|
||||
//
|
||||
function [time, accel, mag, gyro] = imu_read_log(filename)
|
||||
|
||||
time=[];
|
||||
accel=[];
|
||||
mag=[];
|
||||
gyro=[];
|
||||
dt = 0.015625;
|
||||
|
||||
u=mopen(filename, 'r');
|
||||
idx = 0;
|
||||
while meof(u) == 0,
|
||||
line = mgetl(u, 1);
|
||||
|
||||
if strindex(line, 'IMU_GYRO') == 1
|
||||
[nb_scan, gp, gq, gr] = msscanf(1, line, 'IMU_GYRO %f %f %f');
|
||||
gyro = [gyro [gp;gq;gr]];
|
||||
end
|
||||
|
||||
if strindex(line, 'IMU_ACCEL') == 1
|
||||
[nb_scan, ax, ay, az] = msscanf(1, line, 'IMU_ACCEL %f %f %f');
|
||||
accel = [accel [ax;ay;az]];
|
||||
end
|
||||
|
||||
if strindex(line, 'IMU_MAG') == 1
|
||||
[nb_scan, mx, my, mz] = msscanf(1, line, 'IMU_MAG %f %f %f');
|
||||
mag = [mag [mx;my;mz]];
|
||||
time = [time idx*dt];
|
||||
idx = idx + 1;
|
||||
end
|
||||
|
||||
end
|
||||
mclose(u);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Sensors sim
|
||||
//
|
||||
//
|
||||
//
|
||||
function [accel, mag, gyro] = imu_sim(time, rates, eulers)
|
||||
accel = [];
|
||||
mag = [];
|
||||
gyro = [];
|
||||
sigma_gyro = [0.01; 0.01; 0.01];
|
||||
bias_gyro = [0.05; 0.03; 0.04];
|
||||
|
||||
g0 = [0.; 0.; 9.81];
|
||||
sigma_accel = [0.05; 0.05; 0.05];
|
||||
|
||||
h0 = [185.; 0.; 157.];
|
||||
sigma_mag = [3.; 3.; 3.];
|
||||
|
||||
for i=1:length(time),
|
||||
noise_gyro = rand(3, 1, "normal") .* sigma_gyro;
|
||||
g = rates(:, i) + noise_gyro + bias_gyro;
|
||||
gyro = [gyro g];
|
||||
|
||||
DCM = dcm_of_euler(eulers(:,i));
|
||||
noise_accel = rand(3, 1, "normal") .* sigma_accel;
|
||||
acc = DCM * g0 + noise_accel;
|
||||
accel = [accel acc];
|
||||
|
||||
noise_mag = rand(3, 1, "normal") .* sigma_mag;
|
||||
ma = DCM * h0 + noise_mag;
|
||||
mag = [mag ma];
|
||||
end
|
||||
endfunction
|
||||
@@ -0,0 +1,49 @@
|
||||
//
|
||||
//
|
||||
//
|
||||
// trajectory generation
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
//
|
||||
// phi_dot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r
|
||||
// theta_dot = cos(phi) *q - sin(phi) *r
|
||||
// psi_dot = sin(phi)/cos(theta)*q + cos(phi)/cos(theta)*r
|
||||
//
|
||||
|
||||
function [time, rates, eulers] = quadrotor_gen_roll_step(euler0, dt)
|
||||
len_gen = 20.;
|
||||
t0 = 5.;
|
||||
t1 = 7.;
|
||||
t2 = 13.;
|
||||
t3 = 15.;
|
||||
ampl = 0.4;
|
||||
nb_samples = len_gen / dt;
|
||||
time=[0.];
|
||||
rates=[[0.; 0.; 0.]];
|
||||
eulers=[euler0];
|
||||
for i=2:nb_samples
|
||||
t = (i-1)*dt;
|
||||
time = [time t];
|
||||
rate = [0.; 0.; 0.];
|
||||
if (t >= t0 & t <= t1),
|
||||
rate = rate + [ampl * sin(%pi/(t1-t0)*(t-t0)); 0.; 0.];
|
||||
elseif (t >= t2 & t <= t3),
|
||||
rate = rate + [-ampl * sin(%pi/(t1-t0)*(t-t0)); 0.; 0.];
|
||||
end
|
||||
rates = [rates rate];
|
||||
phi = eulers(1,i-1);
|
||||
theta = eulers(2,i-1);
|
||||
psi = eulers(3,i-1);
|
||||
p = rates(1, i-1);
|
||||
q = rates(2, i-1);
|
||||
r = rates(3, i-1);
|
||||
euler_dot = [ p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r
|
||||
cos(phi) *q - sin(phi) *r
|
||||
sin(phi)/cos(theta)*q + cos(phi)/cos(theta)*r ];
|
||||
euler = eulers(:,i-1) + euler_dot * dt;
|
||||
eulers = [eulers euler];
|
||||
end
|
||||
|
||||
endfunction
|
||||
Reference in New Issue
Block a user