mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -0,0 +1,156 @@
|
||||
REF_Z = 1;
|
||||
REF_ZD = 2;
|
||||
REF_ZDD = 3;
|
||||
REF_SIZE = 3;
|
||||
|
||||
//
|
||||
// Parameters
|
||||
//
|
||||
|
||||
ctl_max_err_z = 5; // m
|
||||
ctl_kp = -50/20;
|
||||
ctl_kd = -10/20;
|
||||
|
||||
ctl_max_cmd = 0.99; // actuators high saturation
|
||||
ctl_min_cmd = 0.01; // actuators low saturation
|
||||
|
||||
ref_omega = rad_of_deg(200.);// second order linear dynamics
|
||||
ref_zeta = 0.85;
|
||||
ref_saturate = 1; // with saturations
|
||||
ref_max_accel = 2. * fdm_g; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g
|
||||
ref_min_accel = -0.9 * fdm_g;// m/s2
|
||||
ref_max_speed = 3.; // m/s
|
||||
|
||||
|
||||
//
|
||||
// propagation of a second order linear model with saturations
|
||||
//
|
||||
function [Xrefi1] = ctl_update_ref(Xrefi, Xspi, dt)
|
||||
|
||||
Xrefi1 = zeros(FDM_SIZE, 1);
|
||||
|
||||
// second order ref model
|
||||
Xrefi1(REF_Z) = Xrefi(REF_Z) + dt * Xrefi(REF_ZD);
|
||||
Xrefi1(REF_ZD) = Xrefi(REF_ZD) + dt * Xrefi(REF_ZDD);
|
||||
Xrefi1(REF_ZDD) = -2*ref_zeta*ref_omega * Xrefi(REF_ZD) ...
|
||||
-ref_omega^2 * ( Xrefi(REF_Z) - Xspi);
|
||||
|
||||
if ref_saturate
|
||||
// saturate acceleration
|
||||
if Xrefi1(REF_ZDD) > ref_max_accel
|
||||
Xrefi1(REF_ZDD) = ref_max_accel;
|
||||
elseif Xrefi1(REF_ZDD) < ref_min_accel
|
||||
Xrefi1(REF_ZDD) = ref_min_accel;
|
||||
end
|
||||
// saturate speed
|
||||
if Xrefi1(REF_ZD) >= ref_max_speed & Xrefi1(REF_ZDD) >= 0
|
||||
Xrefi1(REF_ZD) = ref_max_speed;
|
||||
Xrefi1(REF_ZDD) = 0;
|
||||
elseif Xrefi1(REF_ZD) <= -ref_max_speed & Xrefi1(REF_ZDD) <= 0
|
||||
Xrefi1(REF_ZD) = -ref_max_speed;
|
||||
Xrefi1(REF_ZDD) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
// Display
|
||||
//
|
||||
//
|
||||
|
||||
function ctl_display_ref(ctl_sp, ctl_ref_state, time)
|
||||
nr = 3;
|
||||
nc = 1;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
//plot2d(time, ctl_sp, 5);
|
||||
//plot2d(time, ctl_ref_state(REF_Z,:),2);
|
||||
_x = [time; time]';
|
||||
_y = [ ctl_sp; ctl_ref_state(REF_Z,:)]';
|
||||
plot2d(_x, _y, leg="input@ref", style=[3,2]);
|
||||
xtitle('Z');//, 'time in s', 'm');
|
||||
|
||||
subplot(nr,nc,2);
|
||||
plot2d(time, ctl_ref_state(REF_ZD,:),2);
|
||||
plot2d(time, ref_max_speed * ones(1, length(time)), 5);
|
||||
plot2d(time, -ref_max_speed * ones(1, length(time)), 5);
|
||||
xtitle('ZD');
|
||||
|
||||
subplot(nr,nc,3);
|
||||
plot2d(time, ctl_ref_state(REF_ZDD,:),2);
|
||||
plot2d(time, ref_max_accel * ones(1, length(time)), 5);
|
||||
plot2d(time, ref_min_accel * ones(1, length(time)), 5);
|
||||
xtitle('ZDD');
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function ctl_display(ctl_type, ctl_list, ctl_sp, ctl_ref_state, ins_state, ctl_adp_state, ctl_adp_cov, ctl_adp_state, time)
|
||||
|
||||
nr = 3;
|
||||
nc = 2;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
_rect = [0, -0.2, 5, .7];
|
||||
plot2d(time, ins_state(INS_Z,:),3);
|
||||
plot2d(time, ctl_ref_state(REF_Z,:),2, rect=_rect);
|
||||
plot2d(time, ctl_sp, 5);
|
||||
legends(["setpoint", "reference", "achieved"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Altitude');
|
||||
|
||||
subplot(nr,nc,3);
|
||||
plot2d(time, ins_state(INS_ZD,:),3);
|
||||
plot2d(time, ctl_ref_state(REF_ZD,:),2);
|
||||
legends(["reference", "achieved"],[2 3], with_box=%f, opt="ur");
|
||||
xtitle('Vertical Speed');
|
||||
|
||||
subplot(nr,nc,5);
|
||||
plot2d(time, ins_state(INS_ZDD,:),3);
|
||||
plot2d(time, ctl_ref_state(REF_ZDD,:),2);
|
||||
legends(["reference", "achieved"],[2 3], with_box=%f, opt="ur");
|
||||
xtitle('Vertical Acceleration');
|
||||
|
||||
select ctl_type
|
||||
case CTL_MIAC
|
||||
subplot(nr,nc,2);
|
||||
// _rect = [time(1), 20, time($), 500];
|
||||
// plot2d(time, ctl_adp_meas,3, rect=_rect);
|
||||
// plot2d(time, ctl_adp_state,2, rect=_rect);
|
||||
plot2d(time, ctl_adp_meas,3);
|
||||
plot2d(time, ctl_adp_state,2);
|
||||
legends(["estimation", "measure"],[2 3], with_box=%f, opt="ur");
|
||||
xtitle('Parameter adaptation');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
_rect = [time(1), 0., time($), 0.003];
|
||||
plot2d(time, ctl_adp_cov,2, rect=_rect);
|
||||
// plot2d(time, ctl_adp_cov,2);
|
||||
xtitle('Parameter adaptation covariance');
|
||||
case CTL_MRAC
|
||||
subplot(nr,nc,2);
|
||||
_rect = [time(1), 1, time($), 4];
|
||||
plot2d(time, ctl_adp_state,2, rect=_rect);
|
||||
legends(["estimation"],[2 ], with_box=%f, opt="ur");
|
||||
xtitle('Parameter adaptation');
|
||||
end
|
||||
|
||||
subplot(nr,nc,6);
|
||||
xset("color",5);
|
||||
xfpoly(ctl_list(5),ctl_list(3));
|
||||
xset("color",8);
|
||||
xfpoly(ctl_list(5),ctl_list(4));
|
||||
xset("color",1);
|
||||
//plot2d(time, ctl_list(1), 1);
|
||||
|
||||
_rect = [time(1), 0, time($), 1.0];
|
||||
plot2d(time, ctl_list(2), 2, rect=_rect);
|
||||
// plot2d(time, ctl_list(2), 2);
|
||||
|
||||
legends(["feedforward", "feedback"],[2 5], with_box=%f, opt="ur");
|
||||
xtitle('Controler Output');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
//
|
||||
// Adaptation of the invert dynamic model parameters :
|
||||
// dimension one kalman filter estimating invert
|
||||
// of the mass
|
||||
//
|
||||
adp_sys_noise = 0.0000005;
|
||||
adp_accel_noise = 0.5;
|
||||
|
||||
adp_min_cmd = 0.01;
|
||||
|
||||
MIAC_USE_VNULL = 1;
|
||||
miac_vnull = 7.;
|
||||
|
||||
|
||||
function [Xadpi1p, Padpi1p, Madpi1] = ctl_adapt_model(Xadpip, Padpip, Xinsi, Ui)
|
||||
|
||||
if (Ui < adp_min_cmd)
|
||||
Xadpi1p = Xadpip
|
||||
// so propagate covariance only
|
||||
Padpi1p = Padpip; // + adp_sys_noise;
|
||||
Madpi1 = 0;
|
||||
else
|
||||
// Propagate
|
||||
// we're estimating a constant
|
||||
Xadpi1m = Xadpip
|
||||
// so propagate covariance only
|
||||
Padpi1m = Padpip + adp_sys_noise;
|
||||
|
||||
// Update
|
||||
// our measurement
|
||||
Madpi1 = (Xinsi(INS_ZDD) + fdm_g) / ( Ui );
|
||||
if MIAC_USE_VNULL
|
||||
Madpi1 = Madpi1 / (1. - Xinsi(INS_ZD)/miac_vnull);
|
||||
end
|
||||
// compute residual
|
||||
res = Madpi1 - Xadpi1m;
|
||||
// kalman gain
|
||||
adp_meas_noise = adp_accel_noise/Ui;
|
||||
E = Padpi1m + adp_meas_noise;
|
||||
K = Padpi1m / E;
|
||||
// update state and covariance
|
||||
Padpi1p = Padpi1m - K * Padpi1m;
|
||||
Xadpi1p = Xadpi1m + K * res;
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [Xrefi, Ui, Xadpi, Padpi, Madpi, feed_fwd] = ctl_miac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Padpim1, Uim1)
|
||||
// adapt model parameters
|
||||
[Xadpi, Padpi, Madpi] = ctl_adapt_model(Xadpim1, Padpim1, Xinsi, Uim1)
|
||||
// update reference
|
||||
Xrefi = ctl_update_ref(Xrefim1, Xspi, dt);
|
||||
// error control
|
||||
err_z = Xinsi(INS_Z) - Xrefi(REF_Z);
|
||||
err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z);
|
||||
err_zd = Xinsi(INS_ZD) - Xrefi(REF_ZD);
|
||||
// invert model
|
||||
feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1);
|
||||
if MIAC_USE_VNULL
|
||||
feed_fwd = feed_fwd / (1 - Xrefi(REF_ZD) / miac_vnull);
|
||||
end
|
||||
// compute command
|
||||
feed_bck = ctl_kp * err_z + ctl_kd * err_zd
|
||||
feed_fwd = trim(feed_fwd, ctl_min_cmd, ctl_max_cmd);
|
||||
Ui = feed_fwd + feed_bck;
|
||||
Ui = trim(Ui, ctl_min_cmd, ctl_max_cmd);
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
//
|
||||
// MRAC Adaptive control parameter
|
||||
//
|
||||
|
||||
mrac_gma = 0.001;
|
||||
mrac_lbd = (-ctl_kd + sqrt(ctl_kd^2-4*ctl_kp))/2;
|
||||
mrac_c = (-ctl_kd - sqrt(ctl_kd^2-4*ctl_kp))/2;
|
||||
|
||||
function [Xadpi1p] = ctl_mrac(Xadpim1, Xinsi, Xrefim1, Uim1)
|
||||
|
||||
// error control
|
||||
err_z = Xinsi(INS_Z) - Xrefim1(REF_Z);
|
||||
err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z);
|
||||
err_zd = Xinsi(INS_ZD) - Xrefim1(REF_ZD);
|
||||
// generalized error
|
||||
s = err_zd + mrac_lbd*err_z;
|
||||
Xadpi1p = Xadpim1 + mrac_gma*s*(Uim1- ...
|
||||
Xinsi(INS_ZD) * abs(Xinsi(INS_ZD)) * fdm_Cd);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [Xrefi, Ui, Xadpi, feed_fwd] = ctl_mrac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Uim1)
|
||||
// adapt model parameters
|
||||
[Xadpi] = ctl_mrac(Xadpim1, Xinsi, Xrefim1, Uim1);
|
||||
// update reference
|
||||
Xrefi = ctl_update_ref(Xrefim1, Xspi, dt);
|
||||
// error control
|
||||
err_z = Xinsi(INS_Z) - Xrefi(REF_Z);
|
||||
err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z);
|
||||
err_zd = Xinsi(INS_ZD) - Xrefi(REF_ZD);
|
||||
// invert model
|
||||
if 1
|
||||
feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1);
|
||||
else
|
||||
// feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi + ...
|
||||
// Xinsi(INS_ZD) * abs(Xinsi(INS_ZD)) * fdm_Cd;
|
||||
feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1) + ...
|
||||
Xrefi(REF_ZD) * abs(Xrefi(REF_ZD)) * fdm_Cd;
|
||||
end
|
||||
// compute command
|
||||
feed_bck = 1/Xadpi(1)*(ctl_kp * err_z + ctl_kd * err_zd);
|
||||
Ui = feed_fwd + feed_bck;
|
||||
Ui = trim(Ui, ctl_min_thrust, ctl_max_thrust);
|
||||
endfunction
|
||||
@@ -0,0 +1,67 @@
|
||||
//
|
||||
// Flight dynamic model
|
||||
//
|
||||
|
||||
// State Vector
|
||||
FDM_Z = 1;
|
||||
FDM_ZD = 2;
|
||||
FDM_ZDD = 3;
|
||||
FDM_SIZE = 3;
|
||||
|
||||
// u
|
||||
|
||||
// Parameters
|
||||
fdm_mass = 0.5; // mass in kg
|
||||
fdm_g = 9.81; // gravity acceleration m/s2
|
||||
fdm_max_thrust = 4. * fdm_mass * fdm_g; // actuators high saturation
|
||||
fdm_min_thrust = 0.05 * fdm_mass * fdm_g; // actuators low saturation
|
||||
fdm_Cd = 0.01; // non dimentional drag coefficient
|
||||
fdm_Ct0 = fdm_max_thrust; // non dimentional lift coefficient
|
||||
fdm_Ctzd = -fdm_max_thrust/7; // non dimensional lift coefficient
|
||||
|
||||
function [Xdot] = fdm_get_derivatives(t, X, U, perturb, param)
|
||||
|
||||
mass = param(1);
|
||||
Xdot = zeros(FDM_ZD, 1);
|
||||
Xdot(FDM_Z) = X(FDM_ZD);
|
||||
u_trim = trim(U(1), 0.01, 1.);
|
||||
thrust = u_trim * (fdm_Ct0 + fdm_Ctzd * X(FDM_ZD));
|
||||
Xdot(FDM_ZD) = 1/mass*(thrust - fdm_Cd * X(FDM_ZD) * abs(X(FDM_ZD))) - fdm_g + perturb;
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
// Integrate the dynamic model
|
||||
//
|
||||
function [Xfdmi1] = fdm_run(Xfdmi, Ui, ti, ti1, perturbi, parami)
|
||||
|
||||
Xfdmi1 = zeros(FDM_SIZE,1);
|
||||
Xfdmi1(FDM_Z:FDM_ZD) = ode(Xfdmi(FDM_Z:FDM_ZD), ti, ti1, list(fdm_get_derivatives, Ui, perturbi, parami));
|
||||
xd = fdm_get_derivatives(ti1, Xfdmi1(FDM_Z:FDM_ZD), Ui, perturbi, parami);
|
||||
Xfdmi1(FDM_ZDD) = xd(FDM_ZD);
|
||||
Xfdmi1 = Xfdmi1;
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
// Simple display
|
||||
//
|
||||
function fdm_display_simple(Xfdm, time)
|
||||
|
||||
nr = 3;
|
||||
nc = 1;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(time, Xfdm(FDM_Z,:));
|
||||
xtitle('Z');
|
||||
|
||||
subplot(nr,nc,2);
|
||||
plot2d(time, Xfdm(FDM_ZD,:));
|
||||
xtitle('ZD');
|
||||
|
||||
subplot(nr,nc,3);
|
||||
plot2d(time, Xfdm(FDM_ZDD,:));
|
||||
xtitle('ZDD');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,110 @@
|
||||
|
||||
|
||||
|
||||
INS_Z = 1;
|
||||
INS_ZD = 2;
|
||||
INS_BIAS = 3;
|
||||
INS_SIZE = 3;
|
||||
INS_ZDD = 4; // WARNING : hack - byproduct
|
||||
// keeping size to 3 for covariance
|
||||
|
||||
function [Xi1, Pi1] = ins_run(Xi, Pi, sensors_i, sensors_i1, dt)
|
||||
|
||||
//
|
||||
// propagate
|
||||
//
|
||||
F = [ 1 dt -dt^2/2
|
||||
0 1 -dt
|
||||
0 0 1 ];
|
||||
B = [ dt^2/2 dt 0]';
|
||||
|
||||
Qz = 0.01*dt;
|
||||
Qzd = 0.01 * dt^2/2;
|
||||
Qbias = 0.0001 * dt;
|
||||
Q = [ Qz 0 0
|
||||
0 Qzd 0
|
||||
0 0 Qbias ];
|
||||
|
||||
accel = sensors_i( SENSORS_ACCEL ) - 9.81;
|
||||
|
||||
Xi1m = F * Xi + B * accel;
|
||||
|
||||
Pi1m = F * Pi * F' + Q;
|
||||
|
||||
//
|
||||
// Update
|
||||
//
|
||||
H = [1 0 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
y = sensors_i1( SENSORS_BARO ) - H * Xi1m;
|
||||
// covariance residual
|
||||
S = H*Pi1m*H' + R;
|
||||
// kalman gain
|
||||
K = Pi1m*H'*inv(S);
|
||||
// update state
|
||||
Xi1 = Xi1m + K*y;
|
||||
// update covariance
|
||||
Pi1 = Pi1m - K*H*Pi1m;
|
||||
Xi1(4) = accel - Xi1(INS_BIAS);
|
||||
|
||||
endfunction
|
||||
|
||||
function [Pi] = getP(n, P,i)
|
||||
Pi = P(:,(i-1)*n+1:i*n);
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
// Simple display
|
||||
//
|
||||
function ins_display_simple(Xins, Pins, Xfdm, Xsensors, time)
|
||||
|
||||
nr = 3;
|
||||
nc = 2;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(time, Xsensors(SENSORS_BARO,:),3);
|
||||
plot2d(time, Xfdm(FDM_Z,:),2);
|
||||
plot2d(time, Xins(INS_Z,:), 5);
|
||||
legends(["Estimation", "Truth", "Measurement"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Altitude');
|
||||
|
||||
subplot(nr,nc,3);
|
||||
plot2d(time, Xfdm(FDM_ZD,:), 2);
|
||||
plot2d(time, Xins(INS_ZD,:), 5);
|
||||
legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur");
|
||||
xtitle('Vertical Speed');
|
||||
|
||||
subplot(nr,nc,5);
|
||||
plot2d(time, Xfdm(FDM_ZDD,:),2);
|
||||
plot2d(time, Xins(4,:), 5);
|
||||
legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur");
|
||||
xtitle('Vertical Acceleration');
|
||||
|
||||
subplot(nr,nc,2);
|
||||
plot2d(time, 1.0*Xsensors(SENSORS_ACCEL_BIAS,:),2);
|
||||
plot2d(time, Xins(INS_BIAS,:), 5);
|
||||
legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur");
|
||||
xtitle('Accelerometer Bias');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
Pzz = zeros(1,length(time));
|
||||
Pzdzd = zeros(1,length(time));
|
||||
for i=1:length(time)
|
||||
Pi = getP(INS_SIZE, Pins, i);
|
||||
Pzz(i) = Pi(INS_Z, INS_Z);
|
||||
Pzdzd(i) = Pi(INS_ZD, INS_ZD);
|
||||
Pbb(i) = Pi(INS_BIAS, INS_BIAS);
|
||||
end
|
||||
|
||||
plot2d(time, Pzz, 5);
|
||||
plot2d(time, Pzdzd, 2);
|
||||
plot2d(time, Pbb, 3);
|
||||
legends(["Altitude", "Speed", "Bias"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Estimation Covariance');
|
||||
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,215 @@
|
||||
clear();
|
||||
|
||||
exec("tvc_utils.sci");
|
||||
exec("tvc_fdm.sci");
|
||||
exec("tvc_sensors.sci");
|
||||
exec("tvc_ins.sci");
|
||||
exec("tvc_ctl_common.sci");
|
||||
exec("tvc_ctl_miac.sci");
|
||||
exec("tvc_ctl_mrac.sci");
|
||||
|
||||
|
||||
//
|
||||
// Duration of the simulation
|
||||
//
|
||||
ts = 0;
|
||||
te = 14;
|
||||
dt = 1./512.;
|
||||
time = ts:dt:te;
|
||||
|
||||
//
|
||||
// type of control
|
||||
//
|
||||
CTL_MIAC = 0;
|
||||
CTL_MRAC = 1;
|
||||
ctl_type = CTL_MIAC;
|
||||
|
||||
|
||||
//
|
||||
// input trajectory
|
||||
//
|
||||
// single input - height
|
||||
ctl_sp = zeros(1, length(time));
|
||||
if 0
|
||||
// step 1m 5s
|
||||
k = find ( time > 3. & time < 6.);
|
||||
ctl_sp(k) = 1.;
|
||||
end
|
||||
if 1
|
||||
// step 15m 14s
|
||||
k = find ( time > 1. & time < 7);
|
||||
ctl_sp(k) = 15.;
|
||||
end
|
||||
if 0
|
||||
// 1rad/s sine input
|
||||
omega = 2.;
|
||||
ctl_sp = 3.5 * sin(omega * time);
|
||||
end
|
||||
|
||||
|
||||
//
|
||||
// Flight dynamic model
|
||||
// aka true state
|
||||
//
|
||||
fdm_state = zeros(FDM_SIZE, length(time));
|
||||
fdm_perturb = zeros(1, length(time));
|
||||
fdm_param = fdm_mass * ones(1, length(time));
|
||||
fdm_state(:,1) = [ 0; 0; 0];
|
||||
if 0
|
||||
k = find(time >= 5 & time < 5.25);
|
||||
fdm_perturb(k) = 20.;
|
||||
end
|
||||
if 0
|
||||
k = find(time >= 5);
|
||||
fdm_param(k) = 0.5*fdm_mass;
|
||||
end
|
||||
|
||||
|
||||
//
|
||||
// Sensor Model state
|
||||
//
|
||||
sensors_state = zeros(SENSORS_SIZE, length(time));
|
||||
|
||||
//
|
||||
// State Estimation ( INS )
|
||||
//
|
||||
BYPASS_INS = 0;
|
||||
ins_state = zeros(INS_SIZE+1, length(time));
|
||||
ins_state(:,1) = [ 0; 0; 0; 0];
|
||||
ins_cov = zeros(INS_SIZE, INS_SIZE * length(time));
|
||||
ins_cov(1:INS_SIZE,1:INS_SIZE) = [ 1 0 0
|
||||
0 1 0
|
||||
0 0 1 ];
|
||||
|
||||
|
||||
//
|
||||
// reference model
|
||||
//
|
||||
ctl_ref_state = zeros(REF_SIZE, length(time));
|
||||
// parameter of the inverted model
|
||||
ctl_adp_state = zeros(1, length(time));
|
||||
ctl_adp_state(1) = 35;
|
||||
ctl_adp_cov = zeros(1, length(time));
|
||||
ctl_adp_cov(1) = 0.05;
|
||||
ctl_adp_meas = zeros(1, length(time));
|
||||
|
||||
// output of the controller
|
||||
ctl_command = zeros(1, length(time));
|
||||
ctl_command(1) = 1;
|
||||
ctl_feed_fwd = zeros(1, length(time));
|
||||
ctl_feed_fwd(1) = 1;
|
||||
ctl_max = zeros(1, length(time));
|
||||
ctl_max(1) = 0;
|
||||
ctl_min = zeros(1, length(time));
|
||||
ctl_min(1) = 0;
|
||||
|
||||
for i = 1:length(time)-1
|
||||
|
||||
ti = time(i);
|
||||
ti1 = time(i+1);
|
||||
// run control
|
||||
Xinsi = ins_state(:,i);
|
||||
if i==1
|
||||
im1 = i;
|
||||
else
|
||||
im1 = i - 1;
|
||||
end
|
||||
Xspi = ctl_sp(:,i);
|
||||
Xrefim1 = ctl_ref_state(:,im1);
|
||||
Xadpim1 = ctl_adp_state(:,im1);
|
||||
Padpim1 = ctl_adp_cov(:,im1);
|
||||
Uim1 = ctl_command(:,im1);
|
||||
select ctl_type
|
||||
case CTL_MIAC
|
||||
[Xrefi, Ui, Xadpi, Padpi, Madpi, feed_fwd] = ctl_miac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Padpim1, Uim1);
|
||||
case CTL_MRAC
|
||||
[Xrefi, Ui, Xadpi, feed_fwd] = ctl_mrac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Uim1);
|
||||
end
|
||||
ctl_ref_state(:,i) = Xrefi;
|
||||
ctl_command(:,i) = Ui;
|
||||
ctl_feed_fwd(:,i) = feed_fwd;
|
||||
ctl_max(:,i) = max([Ui feed_fwd]);
|
||||
ctl_min(:,i) = min([Ui feed_fwd]);
|
||||
ctl_adp_state(:,i) = Xadpi;
|
||||
if ctl_type == CTL_MIAC
|
||||
ctl_adp_cov(:,i) = Padpi;
|
||||
ctl_adp_meas(:,i) = Madpi;
|
||||
end
|
||||
// run_fdm
|
||||
Xfdmi = fdm_state(:,i);
|
||||
Xfdmi1 = fdm_run(Xfdmi, Ui, ti, ti1, fdm_perturb(:,i), fdm_param(i));
|
||||
fdm_state(:,i+1) = Xfdmi1;
|
||||
// run sensor model
|
||||
Xsensorsi1 = sensors_run(ti, Xfdmi);
|
||||
sensors_state(:,i+1) = Xsensorsi1;
|
||||
// run ins
|
||||
Pinsi = getP(INS_SIZE, ins_cov, i);
|
||||
Xsensorsi = sensors_state(:,i);
|
||||
[Xinsi1, Pinsi1] = ins_run(Xinsi(1:INS_SIZE), Pinsi, Xsensorsi, Xsensorsi1, dt);
|
||||
ins_state(:,i+1) = Xinsi1;
|
||||
ins_cov(:,(i)*INS_SIZE+1:(i+1)*INS_SIZE) = Pinsi1;
|
||||
//
|
||||
if BYPASS_INS
|
||||
ins_state(INS_Z,i+1) = fdm_state(FDM_Z,i+1);
|
||||
ins_state(INS_ZD,i+1) = fdm_state(FDM_ZD,i+1);
|
||||
ins_state(INS_ZDD,i+1) = fdm_state(FDM_ZDD,i+1);
|
||||
end
|
||||
end
|
||||
|
||||
ctl_max = [0 ctl_max 0];
|
||||
ctl_min = [0 ctl_min 0];
|
||||
time_exp = [0 time te];
|
||||
ctl_list = list(ctl_command, ctl_feed_fwd, ctl_max, ctl_min, time_exp);
|
||||
|
||||
if 0
|
||||
|
||||
set("current_figure",0);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="FDM";
|
||||
drawlater();
|
||||
fdm_display_simple(fdm_state, time);
|
||||
drawnow();
|
||||
|
||||
set("current_figure",1);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Sensors";
|
||||
drawlater();
|
||||
sensors_display_simple(sensors_state, time);
|
||||
drawnow();
|
||||
|
||||
end
|
||||
|
||||
if 1
|
||||
|
||||
set("current_figure",2);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="INS";
|
||||
drawlater();
|
||||
ins_display_simple(ins_state, ins_cov, fdm_state, sensors_state, time);
|
||||
drawnow();
|
||||
|
||||
|
||||
set("current_figure",3);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Control";
|
||||
drawlater();
|
||||
ctl_display(ctl_type, ctl_list, ctl_sp, ctl_ref_state, ins_state, ctl_adp_state, ctl_adp_cov, ctl_adp_state, time);
|
||||
drawnow();
|
||||
|
||||
end
|
||||
|
||||
if 0
|
||||
|
||||
set("current_figure",4);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Control - Reference";
|
||||
drawlater();
|
||||
ctl_display_ref(ctl_sp, ctl_ref_state, time);
|
||||
drawnow();
|
||||
|
||||
end
|
||||
@@ -0,0 +1,51 @@
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
|
||||
|
||||
SENSORS_ACCEL = 1;
|
||||
SENSORS_BARO = 2;
|
||||
SENSORS_ACCEL_BIAS = 3;
|
||||
SENSORS_SIZE = 3;
|
||||
|
||||
|
||||
accel_noise_std_dev = 0.20;
|
||||
accel_bias = 0.4;
|
||||
baro_noise_std_dev = 0.1;
|
||||
baro_period = 10;
|
||||
baro_step = 0.06;
|
||||
|
||||
|
||||
function [Xsensorsi] = sensors_run(ti, Xfdmi)
|
||||
Xsensorsi = zeros(SENSORS_SIZE, 1);
|
||||
Xsensorsi(SENSORS_ACCEL) = Xfdmi(FDM_ZDD) + 9.81 +...
|
||||
accel_bias +...
|
||||
accel_noise_std_dev * rand(1,1,'normal');
|
||||
Xsensorsi(SENSORS_ACCEL_BIAS) = accel_bias;
|
||||
|
||||
baro_real = Xfdmi(FDM_Z) + baro_noise_std_dev * rand(1,1,'normal');
|
||||
baro_disc = round(baro_real/baro_step) * baro_step;
|
||||
Xsensorsi(SENSORS_BARO) = baro_disc;
|
||||
endfunction
|
||||
|
||||
//
|
||||
// Simple display
|
||||
//
|
||||
function sensors_display_simple(Xsensors, time)
|
||||
|
||||
nr = 2;
|
||||
nc = 1;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(time, Xsensors(SENSORS_ACCEL,:));
|
||||
xtitle('ACCEL');
|
||||
|
||||
subplot(nr,nc,2);
|
||||
plot2d(time, Xsensors(SENSORS_BARO,:));
|
||||
xtitle('BARO');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
|
||||
function [rad] = rad_of_deg(deg)
|
||||
rad = deg / 180 * %pi;
|
||||
endfunction
|
||||
|
||||
function [deg] = deg_of_rad(rad)
|
||||
deg = rad * 180 / %pi;
|
||||
endfunction
|
||||
|
||||
function [o] = trim(i,_min, _max)
|
||||
o = i;
|
||||
if i > _max
|
||||
o = _max
|
||||
elseif i < _min
|
||||
o = _min
|
||||
end
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,172 @@
|
||||
|
||||
|
||||
|
||||
global ctl_sp_pos;
|
||||
global ctl_ref_0;
|
||||
global ctl_ref_1;
|
||||
global ctl_ref_2;
|
||||
global ctl_ref_3;
|
||||
global ctl_ref_4;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function ctl_init()
|
||||
|
||||
global fdm_time;
|
||||
|
||||
global ctl_sp_pos;
|
||||
ctl_sp_pos = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_0;
|
||||
ctl_ref_0 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_1;
|
||||
ctl_ref_1 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_2;
|
||||
ctl_ref_2 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_3;
|
||||
ctl_ref_3 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_4;
|
||||
ctl_ref_4 = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function ctl_run(i)
|
||||
|
||||
global ctl_sp_pos;
|
||||
if fdm_time(i) < 10
|
||||
ctl_sp_pos(:,i)= [ 5; 0];
|
||||
else
|
||||
ctl_sp_pos(:,i)= [ 0; 1];
|
||||
end
|
||||
|
||||
ctl_update_ref(i);
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
ctl_omega_ref1 = [ rad_of_deg(90); rad_of_deg(90)];
|
||||
ctl_zeta_ref1 = [ 0.85; 0.85 ];
|
||||
|
||||
ctl_omega_ref2 = [ rad_of_deg(1440); rad_of_deg(1440)];
|
||||
ctl_zeta_ref2 = [ 0.85; 0.85 ];
|
||||
|
||||
a0 = ctl_omega_ref1^2 .* ctl_omega_ref2^2;
|
||||
a1 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_omega_ref2^2 + ...
|
||||
2 * ctl_zeta_ref2 .* ctl_omega_ref2 .* ctl_omega_ref1^2;
|
||||
a2 = ctl_omega_ref1^2 + ...
|
||||
2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_zeta_ref2 .* ctl_omega_ref2 + ...
|
||||
ctl_omega_ref2^2;
|
||||
a3 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 + 2 * ctl_zeta_ref2 .* ctl_omega_ref2;
|
||||
a4 = [1;1];
|
||||
|
||||
|
||||
|
||||
|
||||
function ctl_update_ref(i)
|
||||
global ctl_sp_pos;
|
||||
global ctl_ref_0;
|
||||
global ctl_ref_1;
|
||||
global ctl_ref_2;
|
||||
global ctl_ref_3;
|
||||
global ctl_ref_4;
|
||||
|
||||
err_pos = ctl_ref_0(:,i-1) - ctl_sp_pos(:,i-1);
|
||||
ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i-1) -a2 .* ctl_ref_2(:,i-1) -a1 .* ctl_ref_1(:,i-1) -a0.*err_pos;
|
||||
|
||||
dt = 1/512;
|
||||
ctl_ref_3(:,i) = ctl_ref_3(:,i-1) + dt * ctl_ref_4(:,i-1);
|
||||
ctl_ref_2(:,i) = ctl_ref_2(:,i-1) + dt * ctl_ref_3(:,i-1);
|
||||
ctl_ref_1(:,i) = ctl_ref_1(:,i-1) + dt * ctl_ref_2(:,i-1);
|
||||
ctl_ref_0(:,i) = ctl_ref_0(:,i-1) + dt * ctl_ref_1(:,i-1);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function ctl_display()
|
||||
nr = 5;
|
||||
nc = 3;
|
||||
subplot(nr,nc,1);
|
||||
plot2d(fdm_time, fdm_state(FDM_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_0(AXIS_X,:),3);
|
||||
plot2d(fdm_time, ctl_sp_pos(AXIS_X,:),5);
|
||||
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(0)');
|
||||
|
||||
subplot(nr,nc,2);
|
||||
plot2d(fdm_time, fdm_state(FDM_SZ,:),2);
|
||||
plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3);
|
||||
plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
|
||||
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(0)');
|
||||
|
||||
subplot(nr,nc,3);
|
||||
plot2d(fdm_time, fdm_state(FDM_STHETA,:),2);
|
||||
// plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3);
|
||||
// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Theta(0)');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_1(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(1)');
|
||||
|
||||
subplot(nr,nc,5);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_1(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(1)');
|
||||
|
||||
subplot(nr,nc,6);
|
||||
plot2d(fdm_time, fdm_state(FDM_STHETAD,:),2);
|
||||
// plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3);
|
||||
// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Theta(1)');
|
||||
|
||||
subplot(nr,nc,7);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_2(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(2)');
|
||||
|
||||
subplot(nr,nc,8);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_2(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(2)');
|
||||
|
||||
subplot(nr,nc,10);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_3(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(3)');
|
||||
|
||||
subplot(nr,nc,11);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_3(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(3)');
|
||||
|
||||
subplot(nr,nc,13);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_4(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(4)');
|
||||
|
||||
subplot(nr,nc,14);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_4(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(4)');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
//
|
||||
// Flight Dynamic Model of a planar quadrotor
|
||||
//
|
||||
|
||||
//
|
||||
// State Vector
|
||||
//
|
||||
FDM_SX = 1;
|
||||
FDM_SZ = 2;
|
||||
FDM_STHETA = 3;
|
||||
FDM_SXD = 4;
|
||||
FDM_SZD = 5;
|
||||
FDM_STHETAD = 6;
|
||||
FDM_SSIZE = 6;
|
||||
|
||||
//
|
||||
// Actuators
|
||||
//
|
||||
FDM_MOTOR_RIGHT = 1;
|
||||
FDM_MOTOR_LEFT = 2;
|
||||
FDM_MOTOR_NB = 2;
|
||||
|
||||
fdm_g = 9.81;
|
||||
fdm_mass = 0.25;
|
||||
fdm_inertia = 0.0078;
|
||||
|
||||
fdm_dt = 1./512.;
|
||||
|
||||
global fdm_time;
|
||||
global fdm_state;
|
||||
global fdm_accel;
|
||||
|
||||
|
||||
function fdm_init(t_start, t_stop)
|
||||
|
||||
global fdm_time;
|
||||
fdm_time = t_start:fdm_dt:t_stop;
|
||||
global fdm_state;
|
||||
fdm_state = zeros(FDM_SSIZE, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
function fdm_run(i, cmd)
|
||||
|
||||
global fdm_state;
|
||||
global fdm_time;
|
||||
fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd));
|
||||
|
||||
endfunction
|
||||
|
||||
function [Xdot] = fdm_get_derivatives(t, X, U)
|
||||
|
||||
Xdot = zeros(length(X),1);
|
||||
Xdot(FDM_SX) = X(FDM_SXD);
|
||||
Xdot(FDM_SZ) = X(FDM_SZD);
|
||||
Xdot(FDM_STHETA) = X(FDM_STHETAD);
|
||||
Xdot(FDM_SXD) = sum(U)/fdm_mass*sin(X(FDM_STHETA));
|
||||
Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g);
|
||||
Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
|
||||
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
function [rad] = rad_of_deg(deg)
|
||||
rad = deg / 180 * %pi;
|
||||
endfunction
|
||||
|
||||
function [deg] = deg_of_rad(rad)
|
||||
deg = rad * 180 / %pi;
|
||||
endfunction
|
||||
|
||||
AXIS_X = 1;
|
||||
AXIS_Z = 2;
|
||||
AXIS_NB = 2;
|
||||
@@ -0,0 +1,27 @@
|
||||
clear();
|
||||
clearglobal();
|
||||
|
||||
|
||||
exec('q3d_utils.sci');
|
||||
exec('q3d_fdm.sci');
|
||||
exec('q3d_ctl.sci');
|
||||
|
||||
|
||||
|
||||
fdm_init(0,20.);
|
||||
ctl_init();
|
||||
|
||||
for i=1:length(fdm_time)-1
|
||||
fdm_run(i+1, [1.227; 1.227]);
|
||||
ctl_run(i+1);
|
||||
|
||||
|
||||
end
|
||||
|
||||
drawlater();
|
||||
set("current_figure",0);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="CTL";
|
||||
ctl_display();
|
||||
drawnow();
|
||||
@@ -0,0 +1,107 @@
|
||||
|
||||
|
||||
AHRS_QI = 1;
|
||||
AHRS_QX = 2;
|
||||
AHRS_QY = 3;
|
||||
AHRS_QZ = 4;
|
||||
AHRS_BP = 5;
|
||||
AHRS_BQ = 6;
|
||||
AHRS_BR = 7;
|
||||
AHRS_SSIZE = 7;
|
||||
|
||||
global ahrs_state;
|
||||
global ahrs_euler;
|
||||
global ahrs_rate;
|
||||
|
||||
function ahrs_init()
|
||||
|
||||
global fdm_time;
|
||||
|
||||
global ahrs_state;
|
||||
ahrs_state = zeros(AHRS_SSIZE, length(fdm_time));
|
||||
ahrs_state(AHRS_QI, 1) = 1;
|
||||
global ahr_euler;
|
||||
ahrs_euler = zeros(AXIS_NB, length(fdm_time));
|
||||
global ahrs_rate;
|
||||
ahrs_rate = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function ahrs_propagate(i)
|
||||
|
||||
global sensor_gyro;
|
||||
global ahrs_state;
|
||||
global ahrs_euler;
|
||||
global ahrs_rate;
|
||||
|
||||
ahrs_state(AHRS_BP:AHRS_BR, i) = ahrs_state(AHRS_BP:AHRS_BR, i-1);
|
||||
ahrs_rate(:,i) = sensor_gyro(:,i) - ahrs_state(AHRS_BP:AHRS_BR, i-1);
|
||||
|
||||
W = get_omega_quat(ahrs_rate(:,i-1));
|
||||
K_lagrange = 1.;
|
||||
quat = fdm_state(FDM_SQI:FDM_SQZ);
|
||||
quat_dot = -1/2 * W * quat;
|
||||
quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat;
|
||||
dt = 1/512;
|
||||
ahrs_state(AHRS_QI:AHRS_QZ, i) = ahrs_state(AHRS_QI:AHRS_QZ, i-1) + dt * quat_dot;
|
||||
|
||||
ahrs_euler(:,i) = euler_of_quat(ahrs_state(AHRS_QI:AHRS_QZ,i));
|
||||
|
||||
endfunction
|
||||
|
||||
function ahrs_display()
|
||||
nr = 3;
|
||||
nc = 3;
|
||||
global fdm_state;
|
||||
global fdm_euler;
|
||||
global fdm_raccel;
|
||||
|
||||
global fdm_time;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PHI,:)),3);
|
||||
plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_PHI,:)),2);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Phi');
|
||||
subplot(nr,nc,2);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_THETA,:)),3);
|
||||
plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_THETA,:)),2);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Theta');
|
||||
subplot(nr,nc,3);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PSI,:)),3);
|
||||
plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_PSI,:)),2);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Psi');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SP,:)),3);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('P');
|
||||
subplot(nr,nc,5);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SQ,:)),3);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Q');
|
||||
subplot(nr,nc,6);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SR,:)),3);
|
||||
legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('R');
|
||||
|
||||
subplot(nr,nc,7);
|
||||
plot2d(fdm_time, fdm_raccel(AXIS_X,:),3);
|
||||
legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Pd');
|
||||
subplot(nr,nc,8);
|
||||
plot2d(fdm_time, fdm_raccel(AXIS_Y,:),3);
|
||||
legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Qd');
|
||||
subplot(nr,nc,9);
|
||||
plot2d(fdm_time, fdm_raccel(AXIS_Z,:),3);
|
||||
legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Rd');
|
||||
|
||||
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,223 @@
|
||||
function [rad] = rad_of_deg(deg)
|
||||
rad = deg / 180 * %pi;
|
||||
endfunction
|
||||
|
||||
function [deg] = deg_of_rad(rad)
|
||||
deg = rad * 180 / %pi;
|
||||
endfunction
|
||||
|
||||
|
||||
EULER_PHI = 1;
|
||||
EULER_THETA = 2;
|
||||
EULER_PSI = 3;
|
||||
EULER_NB = 3;
|
||||
|
||||
AXIS_X = 1;
|
||||
AXIS_Y = 2;
|
||||
AXIS_Z = 3;
|
||||
AXIS_NB = 3;
|
||||
|
||||
|
||||
Q_QI = 1;
|
||||
Q_QX = 2;
|
||||
Q_QY = 3;
|
||||
Q_QZ = 4;
|
||||
Q_SIZE = 4;
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [dcm] = dcm_of_quat(q)
|
||||
|
||||
qi2 = q(Q_QI)*q(Q_QI);
|
||||
qiqx = q(Q_QI)*q(Q_QX);
|
||||
qiqy = q(Q_QI)*q(Q_QY);
|
||||
qiqz = q(Q_QI)*q(Q_QZ);
|
||||
qx2 = q(Q_QX)*q(Q_QX);
|
||||
qxqy = q(Q_QX)*q(Q_QY);
|
||||
qxqz = q(Q_QX)*q(Q_QZ);
|
||||
qy2 = q(Q_QY)*q(Q_QY);
|
||||
qyqz = q(Q_QY)*q(Q_QZ);
|
||||
qz2 = q(Q_QZ)*q(Q_QZ);
|
||||
m00 = qi2 + qx2 - qy2 - qz2;
|
||||
m01 = 2 * ( qxqy + qiqz );
|
||||
m02 = 2 * ( qxqz - qiqy );
|
||||
m10 = 2 * ( qxqy - qiqz );
|
||||
m11 = qi2 - qx2 + qy2 - qz2;
|
||||
m12 = 2 * ( qyqz + qiqx );
|
||||
m20 = 2 * ( qxqz + qiqy );
|
||||
m21 = 2 * ( qyqz - qiqx );
|
||||
m22 = qi2 - qx2 - qy2 + qz2;
|
||||
dcm = [ m00 m01 m02
|
||||
m10 m11 m12
|
||||
m20 m21 m22 ];
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [quat] = quat_of_euler(euler)
|
||||
|
||||
phi2 = euler(EULER_PHI) / 2.0;
|
||||
theta2 = euler(EULER_THETA) / 2.0;
|
||||
psi2 = euler(EULER_PSI) / 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(Q_QY)*quat(Q_QY) + quat(Q_QZ)*quat(Q_QZ));
|
||||
dcm01 = 2*(quat(Q_QX)*quat(Q_QY) + quat(Q_QI)*quat(Q_QZ));
|
||||
dcm02 = 2*(quat(Q_QX)*quat(Q_QZ) - quat(Q_QI)*quat(Q_QY));
|
||||
dcm12 = 2*(quat(Q_QY)*quat(Q_QZ) + quat(Q_QI)*quat(Q_QX));
|
||||
dcm22 = 1.0 - 2*(quat(Q_QX)*quat(Q_QX) + quat(Q_QY)*quat(Q_QY));
|
||||
|
||||
phi = atan( dcm12, dcm22 );
|
||||
theta = -asin( dcm02 );
|
||||
psi = atan( dcm01, dcm00 );
|
||||
|
||||
euler = [phi; theta; psi];
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [vo] = quat_vect_mult(q, vi)
|
||||
dcm = dcm_of_quat(q);
|
||||
vo = dcm * vi;
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [vo] = quat_vect_inv_mult(q, vi)
|
||||
dcm = dcm_of_quat(q);
|
||||
vo = dcm' * vi;
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
// q_a2c = q_a2b comp q_b2c , aka q_a2c = q_b2c * q_a2b
|
||||
//
|
||||
function [q_a2c] = quat_comp(q_a2b, q_b2c)
|
||||
M = [ q_b2c(Q_QI) -q_b2c(Q_QX) -q_b2c(Q_QY) -q_b2c(Q_QZ)
|
||||
q_b2c(Q_QX) q_b2c(Q_QI) q_b2c(Q_QZ) -q_b2c(Q_QY)
|
||||
q_b2c(Q_QY) -q_b2c(Q_QZ) q_b2c(Q_QI) q_b2c(Q_QX)
|
||||
q_b2c(Q_QZ) q_b2c(Q_QY) -q_b2c(Q_QX) q_b2c(Q_QI) ];
|
||||
q_a2c = M * q_a2b;
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
// q_a2b = q_a2b comp_inv q_b2c , aka q_a2b = qinv(_b2c) * q_a2c
|
||||
//
|
||||
function [q_a2b] = quat_comp_inv(q_a2c, q_b2c)
|
||||
q_c2b = quat_inv(q_b2c);
|
||||
q_a2b = quat_comp(q_a2c, q_c2b);
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
// q_a2c = q_b2a inv_comp q_b2c , aka q_a2b = qinv(_b2c) * q_a2c
|
||||
//
|
||||
function [q_a2c] = quat_inv_comp(q_b2a, q_b2c)
|
||||
q_a2b = quat_inv(q_b2a);
|
||||
q_a2c = quat_comp(q_a2b, q_b2c);
|
||||
endfunction
|
||||
|
||||
|
||||
function [b] = quat_div(a, c)
|
||||
M = [ c(Q_QI) +c(Q_QX) +c(Q_QY) +c(Q_QZ)
|
||||
c(Q_QX) -c(Q_QI) -c(Q_QZ) +c(Q_QY)
|
||||
c(Q_QY) +c(Q_QZ) -c(Q_QI) -c(Q_QX)
|
||||
c(Q_QZ) -c(Q_QY) +c(Q_QX) -c(Q_QI) ];
|
||||
b = M * a;
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [b] = quat_inv(a)
|
||||
b(Q_QI) = a(Q_QI);
|
||||
b(Q_QX) = -a(Q_QX);
|
||||
b(Q_QY) = -a(Q_QY);
|
||||
b(Q_QZ) = -a(Q_QZ);
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [b] = quat_wrap_shortest(a)
|
||||
if a(Q_QI) < 0
|
||||
b = quat_explementary(a);
|
||||
else
|
||||
b = a;
|
||||
end
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [b] = quat_explementary(a)
|
||||
b(Q_QI) = -a(Q_QI);
|
||||
b(Q_QX) = -a(Q_QX);
|
||||
b(Q_QY) = -a(Q_QY);
|
||||
b(Q_QZ) = -a(Q_QZ);
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [b] = quat_wrap_shortest(a)
|
||||
if a(Q_QI) < 0
|
||||
b = quat_explementary(a);
|
||||
else
|
||||
b = a;
|
||||
end
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [omega] = get_omega_quat(rate)
|
||||
p = rate(1);
|
||||
q = rate(2);
|
||||
r = rate(3);
|
||||
omega = [ 0 p q r
|
||||
-p 0 -r q
|
||||
-q r 0 -p
|
||||
-r -q p 0 ];
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function [ c ] = cross_product(a, b)
|
||||
c = [ a(2)*b(3) - a(3)*b(2)
|
||||
a(3)*b(1) - a(1)*b(3)
|
||||
a(1)*b(2) - a(2)*b(1) ];
|
||||
endfunction
|
||||
@@ -0,0 +1,144 @@
|
||||
//
|
||||
// Flight Dynamic Model of a quadrotor
|
||||
//
|
||||
|
||||
|
||||
//
|
||||
// State Vector
|
||||
//
|
||||
FDM_SX = 1;
|
||||
FDM_SY = 2;
|
||||
FDM_SZ = 3;
|
||||
FDM_SXD = 4;
|
||||
FDM_SYD = 5;
|
||||
FDM_SZD = 6;
|
||||
FDM_SQI = 7;
|
||||
FDM_SQX = 8;
|
||||
FDM_SQY = 9;
|
||||
FDM_SQZ = 10;
|
||||
FDM_SP = 11;
|
||||
FDM_SQ = 12;
|
||||
FDM_SR = 13;
|
||||
FDM_SSIZE = 13;
|
||||
|
||||
//
|
||||
// Actuators
|
||||
//
|
||||
FDM_MOTOR_FRONT = 1;
|
||||
FDM_MOTOR_RIGHT = 2;
|
||||
FDM_MOTOR_BACK = 3;
|
||||
FDM_MOTOR_LEFT = 4;
|
||||
FDM_MOTOR_NB = 4;
|
||||
|
||||
fdm_g = 9.81; // gravitational acceleration
|
||||
fdm_mass = 0.5; // mass in kg
|
||||
fdm_inertia = [0.0078 0. 0. // inertia tensor
|
||||
0. 0.0078 0.
|
||||
0. 0. 0.0137 ];
|
||||
fdm_Ct0 = 4. * fdm_mass * fdm_g / 4; // thrust coefficient
|
||||
fdm_V0 = 7.; //
|
||||
fdm_Cd = 0.01; // drag coefficient
|
||||
fdm_la = 0.25; // arm length
|
||||
fdm_Cm = fdm_Ct0 / 10; // torque coefficient
|
||||
|
||||
fdm_wind = [ 0; 0; 0];
|
||||
|
||||
fdm_dt = 1/512;
|
||||
|
||||
global fdm_time;
|
||||
global fdm_state;
|
||||
global fdm_euler;
|
||||
global fdm_accel;
|
||||
global fdm_raccel;
|
||||
|
||||
|
||||
function fdm_init(t_start, t_stop)
|
||||
global fdm_time;
|
||||
fdm_time = t_start:fdm_dt:t_stop;
|
||||
global fdm_state;
|
||||
fdm_state = zeros(FDM_SSIZE, length(fdm_time));
|
||||
fdm_state(FDM_SQI, 1) = 1.;
|
||||
global fdm_euler;
|
||||
fdm_euler = zeros(AXIS_NB, length(fdm_time));
|
||||
global fdm_accel;
|
||||
fdm_accel = zeros(AXIS_NB, length(fdm_time));
|
||||
global fdm_raccel;
|
||||
fdm_raccel = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function fdm_run(i, cmd)
|
||||
|
||||
global fdm_state;
|
||||
global fdm_time;
|
||||
fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd));
|
||||
xd = fdm_get_derivatives(fdm_time(i), fdm_state(:, i), cmd);
|
||||
global fdm_accel;
|
||||
fdm_accel(:,i) = xd(FDM_SXD:FDM_SZD);
|
||||
global fdm_raccel;
|
||||
fdm_raccel(:,i) = xd(FDM_SP:FDM_SR);
|
||||
global fdm_euler;
|
||||
fdm_euler(:,i) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,i));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function [Xdot] = fdm_get_derivatives(t, X, U)
|
||||
|
||||
Xdot = zeros(length(X),1);
|
||||
// position
|
||||
Xdot(FDM_SX:FDM_SZ) = X(FDM_SXD:FDM_SZD);
|
||||
// speed
|
||||
Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass * fdm_get_forces_ltp(X, U);
|
||||
// orientation quaternion
|
||||
rates_body = X(FDM_SP:FDM_SR);
|
||||
W = get_omega_quat(rates_body);
|
||||
K_lagrange = 1.;
|
||||
quat = X(FDM_SQI:FDM_SQZ);
|
||||
quat_dot = -1/2 * W * quat;
|
||||
quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat;
|
||||
Xdot(FDM_SQI:FDM_SQZ) = quat_dot;
|
||||
// body rates
|
||||
moments_body = fdm_get_moments_body(X, U);
|
||||
i_omega = fdm_inertia * rates_body;
|
||||
omega_i_omega = cross_product(rates_body, i_omega);
|
||||
Xdot(FDM_SP:FDM_SR) = inv(fdm_inertia) * (moments_body - omega_i_omega);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [F_ltp] = fdm_get_forces_ltp(X, U)
|
||||
|
||||
gspeed_ltp = X(FDM_SXD:FDM_SZD);
|
||||
airspeed_ltp = gspeed_ltp - fdm_wind;
|
||||
quat = X(FDM_SQI:FDM_SQZ);
|
||||
airspeed_body = quat_vect_mult(quat, airspeed_ltp);
|
||||
|
||||
lift_body = [0; 0; -sum(U) * fdm_Ct0 * ( 1 + 1/fdm_V0 * airspeed_body(AXIS_Z))];
|
||||
lift_ltp = quat_vect_inv_mult(quat, lift_body);
|
||||
|
||||
weight_ltp = [0; 0; fdm_g * fdm_mass];
|
||||
|
||||
drag_ltp = -fdm_Cd * norm(airspeed_ltp) * airspeed_ltp;
|
||||
|
||||
F_ltp = lift_ltp + weight_ltp + drag_ltp;
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [M_body] = fdm_get_moments_body(X, U)
|
||||
|
||||
k1 = fdm_la * fdm_Ct0;
|
||||
k2 = fdm_Cm;
|
||||
moments_of_u = [ 0 -k1 0 k1
|
||||
k1 0 -k1 0
|
||||
-k2 k2 -k2 k2 ];
|
||||
|
||||
M_body = moments_of_u * U;
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -0,0 +1,295 @@
|
||||
|
||||
|
||||
|
||||
|
||||
global guidance_sp_psi;
|
||||
global guidance_sp_pos;
|
||||
global guidance_ref_pos;
|
||||
global guidance_ref_speed;
|
||||
global guidance_ref_accel;
|
||||
|
||||
global guidance_cmd_ff;
|
||||
global guidance_cmd_fb;
|
||||
global guidance_output_quat;
|
||||
global guidance_output_thrust;
|
||||
|
||||
guidance_omega_ref = [ rad_of_deg(90); rad_of_deg(90); rad_of_deg(270) ];
|
||||
guidance_zeta_ref = [ 0.8; 0.8; 0.8 ];
|
||||
|
||||
guidance_mass = 0.5;
|
||||
guidance_Ct0 = 0.5*9.81*4;
|
||||
|
||||
guidance_Kp = 0.5* [ -0.1; -0.1; -0.1];
|
||||
guidance_Kd = 1.5* [ -0.1; -0.1; -0.1];
|
||||
|
||||
|
||||
function guidance_init()
|
||||
|
||||
global fdm_time;
|
||||
|
||||
global guidance_sp_psi;
|
||||
guidance_sp_psi = zeros(1, length(fdm_time));
|
||||
global guidance_sp_pos;
|
||||
guidance_sp_pos = zeros(AXIS_NB, length(fdm_time));
|
||||
global guidance_ref_pos;
|
||||
guidance_ref_pos = zeros(AXIS_NB, length(fdm_time));
|
||||
global guidance_ref_speed;
|
||||
guidance_ref_speed = zeros(AXIS_NB, length(fdm_time));
|
||||
global guidance_ref_accel;
|
||||
guidance_ref_accel = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
global guidance_output_quat;
|
||||
guidance_output_quat = zeros(Q_SIZE, length(fdm_time));
|
||||
global guidance_output_thrust;
|
||||
guidance_output_thrust = zeros(1, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
function guidance_run(i)
|
||||
|
||||
// guidance_step_phi(i);
|
||||
// guidance_step_theta(i);
|
||||
// guidance_step_psi(i);
|
||||
// global guidance_output_thrust;
|
||||
// guidance_output_thrust(i) = 0.29;
|
||||
|
||||
// guidance_step_x(i);
|
||||
guidance_step_y(i);
|
||||
// guidance_step_z(i);
|
||||
guidance_update_ref(i);
|
||||
guidance_hover(i);
|
||||
|
||||
|
||||
global stabilization_sp_thrust;
|
||||
global stabilization_sp_quat;
|
||||
global guidance_output_quat;
|
||||
global guidance_output_thrust;
|
||||
stabilization_sp_thrust(i) = guidance_output_thrust(i);
|
||||
stabilization_sp_quat(:,i) = guidance_output_quat(:, i);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
function guidance_hover(i)
|
||||
|
||||
global guidance_ref_pos;
|
||||
global guidance_ref_speed;
|
||||
global guidance_ref_accel;
|
||||
global ins_state;
|
||||
global ahrs_state;
|
||||
|
||||
pos_err = ins_state(INS_SX:INS_SZ,i) - guidance_ref_pos(:,i);
|
||||
speed_err = ins_state(INS_SXD:INS_SZD,i) - guidance_ref_speed(:,i);
|
||||
|
||||
corr_thrust_ltp = guidance_Kp .* pos_err + guidance_Kd .* speed_err;
|
||||
|
||||
nli_thrust_ltp = -guidance_mass / guidance_Ct0 * ( [ 0; 0; 9.81] - guidance_ref_accel(:,i));
|
||||
|
||||
thrust_ltp = nli_thrust_ltp + corr_thrust_ltp;
|
||||
|
||||
axis_foo = cross_product(thrust_ltp/norm(thrust_ltp), [0; 0; -1]);
|
||||
|
||||
angle_foo = asin(norm(axis_foo));
|
||||
|
||||
quat_foo = [ cos(angle_foo); -axis_foo];
|
||||
|
||||
// printf("%f %f %f\n", nli_thrust_ltp(1), nli_thrust_ltp(2), nli_thrust_ltp(3));
|
||||
// printf("%f %f %f %f\n", quat_foo(1), quat_foo(2), quat_foo(3), quat_foo(4));
|
||||
|
||||
// thrust_body = quat_vect_mult(ahrs_state(AHRS_QI:AHRS_QZ, i), thrust_ltp);
|
||||
|
||||
global guidance_output_thrust;
|
||||
guidance_output_thrust(i) = norm(thrust_ltp);
|
||||
|
||||
global guidance_output_quat;
|
||||
guidance_output_quat(:, i) = quat_foo; //[1; 0; 0; 0];
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_update_ref(i)
|
||||
|
||||
global guidance_sp_pos;
|
||||
global guidance_ref_pos;
|
||||
global guidance_ref_speed;
|
||||
global guidance_ref_accel;
|
||||
|
||||
ref_err_pos = guidance_ref_pos(:,i-1) - guidance_sp_pos(:,i-1);
|
||||
|
||||
guidance_ref_accel(:,i) = -guidance_omega_ref^2 .* ref_err_pos ...
|
||||
-2*guidance_zeta_ref .* guidance_omega_ref .* guidance_ref_speed(:,i-1);
|
||||
dt = 1/512;
|
||||
guidance_ref_speed(:,i) = guidance_ref_speed(:,i-1) + dt * guidance_ref_accel(:,i-1);
|
||||
guidance_ref_pos(:,i) = guidance_ref_pos(:,i-1) + dt * guidance_ref_speed(:,i-1);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_x(i)
|
||||
global fdm_time;
|
||||
if modulo(i,2048) < 1024
|
||||
pos_sp = [ 1; 0; 0];
|
||||
else
|
||||
pos_sp = [ -1; 0; 0];
|
||||
end
|
||||
global guidance_sp_pos;
|
||||
guidance_sp_pos(:,i) = pos_sp;
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_y(i)
|
||||
global fdm_time;
|
||||
if modulo(i,2048) < 1024
|
||||
pos_sp = [ 0; 1; 0];
|
||||
else
|
||||
pos_sp = [ 0; -1; 0];
|
||||
end
|
||||
global guidance_sp_pos;
|
||||
guidance_sp_pos(:,i) = pos_sp;
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_z(i)
|
||||
global fdm_time;
|
||||
if modulo(i,1024) < 512
|
||||
pos_sp = [ 0; 0; -1];
|
||||
else
|
||||
pos_sp = [ 0; 0; 0];
|
||||
end
|
||||
global guidance_sp_pos;
|
||||
guidance_sp_pos(:,i) = pos_sp;
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_phi(i)
|
||||
global fdm_time;
|
||||
if modulo(i,512) < 256
|
||||
euler_sp = [ rad_of_deg(10); 0; 0];
|
||||
else
|
||||
euler_sp = [ rad_of_deg(-10); 0; 0];
|
||||
end
|
||||
global guidance_output_quat;
|
||||
guidance_output_quat(:, i) = quat_of_euler(euler_sp);
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_theta(i)
|
||||
global fdm_time;
|
||||
if modulo(i,512) < 256
|
||||
euler_sp = [ 0; rad_of_deg(10); 0];
|
||||
else
|
||||
euler_sp = [ 0; rad_of_deg(-10); 0];
|
||||
end
|
||||
global guidance_output_quat;
|
||||
guidance_output_quat(:, i) = quat_of_euler(euler_sp);
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_step_psi(i)
|
||||
global fdm_time;
|
||||
if modulo(i,512) < 256
|
||||
euler_sp = [ 0; 0; rad_of_deg(10)];
|
||||
else
|
||||
euler_sp = [ 0; 0; rad_of_deg(-10)];
|
||||
end
|
||||
global guidance_output_quat;
|
||||
guidance_output_quat(:, i) = quat_of_euler(euler_sp);
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
function guidance_display()
|
||||
|
||||
nr = 4;
|
||||
nc = 3;
|
||||
global ins_state;
|
||||
global ins_accel;
|
||||
global guidance_ref_pos;
|
||||
global guidance_ref_speed;
|
||||
global guidance_ref_accel;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, guidance_ref_pos(AXIS_X,:),3);
|
||||
plot2d(fdm_time, guidance_sp_pos(AXIS_X,:),5);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X');
|
||||
subplot(nr,nc,2);
|
||||
plot2d(fdm_time, ins_state(INS_SY,:),2);
|
||||
plot2d(fdm_time, guidance_ref_pos(AXIS_Y,:),3);
|
||||
plot2d(fdm_time, guidance_sp_pos(AXIS_Y,:),5);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Y');
|
||||
subplot(nr,nc,3);
|
||||
plot2d(fdm_time, ins_state(INS_SZ,:),2);
|
||||
plot2d(fdm_time, guidance_ref_pos(AXIS_Z,:),3);
|
||||
plot2d(fdm_time, guidance_sp_pos(AXIS_Z,:),5);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
plot2d(fdm_time, ins_state(INS_SXD,:),2);
|
||||
plot2d(fdm_time, guidance_ref_speed(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Xdot');
|
||||
subplot(nr,nc,5);
|
||||
plot2d(fdm_time, ins_state(INS_SYD,:),2);
|
||||
plot2d(fdm_time, guidance_ref_speed(AXIS_Y,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Ydot');
|
||||
subplot(nr,nc,6);
|
||||
plot2d(fdm_time, ins_state(INS_SZD,:),2);
|
||||
plot2d(fdm_time, guidance_ref_speed(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Zdot');
|
||||
|
||||
subplot(nr,nc,7);
|
||||
plot2d(fdm_time, ins_accel(AXIS_X,:),2);
|
||||
plot2d(fdm_time, guidance_ref_accel(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Xdotdot');
|
||||
subplot(nr,nc,8);
|
||||
plot2d(fdm_time, ins_accel(AXIS_Y,:),2);
|
||||
plot2d(fdm_time, guidance_ref_accel(AXIS_Y,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Ydotdot');
|
||||
subplot(nr,nc,9);
|
||||
plot2d(fdm_time, ins_accel(AXIS_Z,:),2);
|
||||
plot2d(fdm_time, guidance_ref_accel(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Zdotdot');
|
||||
|
||||
subplot(nr,nc,12);
|
||||
plot2d(fdm_time, guidance_output_thrust,3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Thrust');
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
|
||||
INS_SX = 1;
|
||||
INS_SY = 2;
|
||||
INS_SZ = 3;
|
||||
INS_SXD = 4;
|
||||
INS_SYD = 5;
|
||||
INS_SZD = 6;
|
||||
INS_SBX = 7;
|
||||
INS_SBY = 8;
|
||||
INS_SBZ = 9;
|
||||
INS_SSIZE = 9;
|
||||
|
||||
|
||||
global ins_state;
|
||||
global ins_accel;
|
||||
|
||||
|
||||
function ins_init()
|
||||
|
||||
global ins_state;
|
||||
ins_state = zeros(INS_SSIZE, length(fdm_time));
|
||||
global ins_accel;
|
||||
ins_accel = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
// propagate from i-1 to i
|
||||
function ins_propagate(i)
|
||||
|
||||
global ins_state;
|
||||
global ins_accel;
|
||||
global ahrs_state;
|
||||
global sensor_accel;
|
||||
|
||||
// convert to LTP
|
||||
accel_ltp = quat_vect_inv_mult(ahrs_state(Q_QI:Q_QZ, i), sensor_accel(:,i-1));
|
||||
// correct for gravity
|
||||
accel_ltp = accel_ltp + [ 0; 0; 9.81];
|
||||
// store unbiased value
|
||||
ins_accel(:,i) = accel_ltp - ins_state(INS_SBX:INS_SBZ,i-1);
|
||||
// compute derivative
|
||||
state_dot = [ ins_state(INS_SXD:INS_SZD, i-1); ins_accel(:,i); 0; 0; 0];
|
||||
// propagate
|
||||
dt = fdm_time(i) - fdm_time(i-1);
|
||||
ins_state(:,i) = ins_state(:,i-1) + state_dot * dt;
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function ins_display()
|
||||
|
||||
nr = 3;
|
||||
nc = 3;
|
||||
global ins_state;
|
||||
global fdm_state;
|
||||
global fdm_time;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(fdm_time, fdm_state(FDM_SX,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X');
|
||||
subplot(nr,nc,2);
|
||||
plot2d(fdm_time, fdm_state(FDM_SY,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SY,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Y');
|
||||
subplot(nr,nc,3);
|
||||
plot2d(fdm_time, fdm_state(FDM_SZ,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SZ,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z');
|
||||
|
||||
subplot(nr,nc,4);
|
||||
plot2d(fdm_time, fdm_state(FDM_SXD,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SXD,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Xdot');
|
||||
subplot(nr,nc,5);
|
||||
plot2d(fdm_time, fdm_state(FDM_SYD,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SYD,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Ydot');
|
||||
subplot(nr,nc,6);
|
||||
plot2d(fdm_time, fdm_state(FDM_SZD,:),3);
|
||||
plot2d(fdm_time, ins_state(INS_SZD,:),2);
|
||||
legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Zdot');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
//
|
||||
// Sensors model
|
||||
//
|
||||
|
||||
|
||||
|
||||
|
||||
global sensor_gyro;
|
||||
global sensor_accel;
|
||||
global sensor_baro;
|
||||
global sensor_gps_pos;
|
||||
global sensor_gps_speed;
|
||||
|
||||
|
||||
function sensors_init()
|
||||
|
||||
global fdm_time;
|
||||
global sensor_accel;
|
||||
sensor_accel = zeros(AXIS_NB, length(fdm_time));
|
||||
global sensor_gyro;
|
||||
sensor_gyro = zeros(AXIS_NB, length(fdm_time));
|
||||
global sensor_baro;
|
||||
sensor_baro = zeros(1, length(fdm_time));
|
||||
global sensor_gps_pos;
|
||||
sensor_gps_pos = zeros(AXIS_NB, length(fdm_time));
|
||||
global sensor_gps_speed;
|
||||
sensor_gps_speed = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function sensors_run(i)
|
||||
|
||||
global fdm_state;
|
||||
global fdm_accel;
|
||||
global sensor_gyro;
|
||||
global sensor_accel;
|
||||
global sensor_baro;
|
||||
global sensor_gps_pos;
|
||||
global sensor_gps_speed;
|
||||
|
||||
sensor_gyro(:,i) = fdm_state(FDM_SP:FDM_SR, i);
|
||||
|
||||
sensor_accel(:,i) = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), (fdm_accel(:,i)-[0; 0; 9.81]));
|
||||
|
||||
sensor_gps_pos(:,i) = fdm_state(FDM_SX:FDM_SZ, i);
|
||||
|
||||
sensor_gps_speed(:,i) = fdm_state(FDM_SXD:FDM_SZD, i);
|
||||
|
||||
// sensor_baro(:,i) = fdm_state(FDM_SZ, i);
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,207 @@
|
||||
|
||||
|
||||
|
||||
|
||||
global stabilization_sp_thrust;
|
||||
global stabilization_sp_quat;
|
||||
global stabilization_sp_euler;
|
||||
global stabilization_ref_quat;
|
||||
global stabilization_ref_euler;
|
||||
global stabilization_ref_rate;
|
||||
global stabilization_ref_raccel;
|
||||
|
||||
global stabilization_cmd_ff;
|
||||
global stabilization_cmd_fb;
|
||||
global stabilization_cmd_axis;
|
||||
global stabilization_cmd_motors;
|
||||
|
||||
|
||||
stab_omega_ref = [ rad_of_deg(1440); rad_of_deg(1440); rad_of_deg(720) ];
|
||||
stab_zeta_ref = [ 0.8; 0.8; 0.8 ];
|
||||
|
||||
|
||||
function stabilization_init()
|
||||
|
||||
global fdm_time;
|
||||
|
||||
global stabilization_sp_thrust;
|
||||
global stabilization_sp_quat;
|
||||
global stabilization_sp_euler;
|
||||
global stabilization_ref_quat;
|
||||
global stabilization_ref_euler;
|
||||
global stabilization_ref_rate;
|
||||
global stabilization_ref_raccel;
|
||||
|
||||
stabilization_sp_thrust = zeros(1, length(fdm_time));
|
||||
stabilization_sp_quat = zeros(Q_SIZE, length(fdm_time));
|
||||
stabilization_sp_euler = zeros(EULER_NB, length(fdm_time));
|
||||
stabilization_ref_quat = zeros(Q_SIZE, length(fdm_time));
|
||||
stabilization_ref_quat(Q_QI, 1) = 1.;
|
||||
stabilization_ref_euler = zeros(EULER_NB, length(fdm_time));
|
||||
stabilization_ref_rate = zeros(AXIS_NB, length(fdm_time));
|
||||
stabilization_ref_raccel = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
global stabilization_cmd_ff;
|
||||
global stabilization_cmd_fb;
|
||||
global stabilization_cmd_axis;
|
||||
global stabilization_cmd_motors;
|
||||
stabilization_cmd_ff = zeros(AXIS_NB, length(fdm_time));
|
||||
stabilization_cmd_fb = zeros(AXIS_NB, length(fdm_time));
|
||||
stabilization_cmd_axis = zeros(4, length(fdm_time));
|
||||
stabilization_cmd_motors = zeros(4, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
Kp = [ -0.5; -0.5; -0.5 ];
|
||||
Kd = [ -0.5; -0.5; -0.5 ];
|
||||
Kdd = [ 0.007; 0.007; 0.007 ];
|
||||
|
||||
function stabilization_run(i)
|
||||
|
||||
global ahrs_state;
|
||||
global stabilization_sp_euler;
|
||||
global stabilization_sp_quat;
|
||||
stabilization_sp_euler(:,i) = euler_of_quat(stabilization_sp_quat(:,i));
|
||||
|
||||
stabilization_update_ref(i);
|
||||
|
||||
err_angle = quat_div(stabilization_ref_quat(:,i), ahrs_state(AHRS_QI:AHRS_QZ,i));
|
||||
err_rate = ahrs_rate(:,i) - stabilization_ref_rate(:,i);
|
||||
|
||||
global stabilization_cmd_fb;
|
||||
stabilization_cmd_fb(:,i) = Kp .* err_angle(Q_QX:Q_QZ) + Kd .* err_rate ;
|
||||
global stabilization_cmd_ff;
|
||||
stabilization_cmd_ff(:,i) = Kdd .* stabilization_ref_raccel(:,i);
|
||||
global stabilization_cmd_axis;
|
||||
global stabilization_sp_thrust;
|
||||
stabilization_cmd_axis(:,i) = [stabilization_cmd_ff(:,i)+stabilization_cmd_fb(:,i)
|
||||
stabilization_sp_thrust(i)];
|
||||
stabilization_mix(i);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function stabilization_update_ref(i)
|
||||
|
||||
global stabilization_sp_quat;
|
||||
global stabilization_ref_quat;
|
||||
global stabilization_ref_rate;
|
||||
global stabilization_ref_raccel;
|
||||
|
||||
// pause
|
||||
|
||||
// error_quat = quat_mult_inv(stabilization_sp_quat(:,i-1), stabilization_ref_quat(:,i-1));
|
||||
// error_quat = quat_inv_comp(stabilization_ref_quat(:,i-1), stabilization_sp_quat(:,i-1));
|
||||
error_quat = quat_div(stabilization_sp_quat(:,i-1), stabilization_ref_quat(:,i-1));
|
||||
error_quat = quat_wrap_shortest(error_quat);
|
||||
|
||||
ref_accel_0 = -stab_omega_ref^2 .* error_quat(Q_QX:Q_QZ);
|
||||
ref_accel_1 = -2. * stab_zeta_ref .* stab_omega_ref .* stabilization_ref_rate(:,i-1);
|
||||
|
||||
stabilization_ref_raccel(:,i) = ref_accel_0 + ref_accel_1;
|
||||
|
||||
W = get_omega_quat(stabilization_ref_rate(:,i-1));
|
||||
K_lagrange = 1.;
|
||||
quat = stabilization_ref_quat(:,i-1);
|
||||
quat_dot = -1/2 * W * quat;
|
||||
quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat;
|
||||
dt = 1/512;
|
||||
stabilization_ref_quat(:,i) = stabilization_ref_quat(:,i-1) + dt * quat_dot;
|
||||
|
||||
stabilization_ref_rate(:,i) = stabilization_ref_rate(:,i-1) + dt * stabilization_ref_raccel(:,i-1);
|
||||
|
||||
global stabilization_ref_euler;
|
||||
stabilization_ref_euler(:,i) = euler_of_quat(stabilization_ref_quat(:,i));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function stabilization_mix(i)
|
||||
|
||||
global stabilization_cmd_axis;
|
||||
global stabilization_cmd_motors;
|
||||
|
||||
motors_of_axis = [ 0 1 -1 1
|
||||
-1 0 1 1
|
||||
0 -1 -1 1
|
||||
1 0 1 1 ];
|
||||
|
||||
stabilization_cmd_motors(:,i) = motors_of_axis * stabilization_cmd_axis(:,i);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
|
||||
function stabilization_display()
|
||||
|
||||
nr = 3;
|
||||
nc = 3;
|
||||
global stabilization_ref_euler;
|
||||
global stabilization_sp_euler;
|
||||
|
||||
global fdm_state;
|
||||
global fdm_euler;
|
||||
global fdm_time;
|
||||
|
||||
subplot(nr,nc,1);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_PHI,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_PHI,:)),5);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PHI,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('PHI');
|
||||
subplot(nr,nc,2);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_THETA,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_THETA,:)),5);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_THETA,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('THETA');
|
||||
subplot(nr,nc,3);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_PSI,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_PSI,:)),5);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PSI,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('PSI');
|
||||
|
||||
global stabilization_ref_rate;
|
||||
subplot(nr,nc,4);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_X,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SP,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('P');
|
||||
subplot(nr,nc,5);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_Y,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SQ,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Q');
|
||||
subplot(nr,nc,6);
|
||||
plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_Z,:)),2);
|
||||
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SR,:)),3);
|
||||
legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('R');
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
global stabilization_cmd_axis;
|
||||
global stabilization_cmd_motors;
|
||||
|
||||
subplot(nr,nc,7);
|
||||
plot2d(fdm_time, stabilization_cmd_axis(AXIS_X,:),1);
|
||||
plot2d(fdm_time, stabilization_cmd_axis(AXIS_Y,:),2);
|
||||
plot2d(fdm_time, stabilization_cmd_axis(AXIS_Z,:),3);
|
||||
legends(["p", "q", "r"],[1 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Cmd axis');
|
||||
|
||||
subplot(nr,nc,8);
|
||||
plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_FRONT,:),1);
|
||||
plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_RIGHT,:),2);
|
||||
plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_BACK,:),3);
|
||||
plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_LEFT,:),4);
|
||||
legends(["f", "r", "b", "l"],[1 2 3 4], with_box=%f, opt="ur");
|
||||
xtitle('Cmd motor');
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
clear();
|
||||
clearglobal();
|
||||
|
||||
exec('tins_algebra.sci');
|
||||
exec('tins_fdm.sci');
|
||||
exec('tins_sensors.sci');
|
||||
exec('tins_ahrs.sci');
|
||||
exec('tins_ins.sci');
|
||||
exec('tins_guidance.sci');
|
||||
exec('tins_stabilization.sci');
|
||||
|
||||
|
||||
|
||||
|
||||
fdm_init(0.,4.);
|
||||
sensors_init();
|
||||
ahrs_init();
|
||||
ins_init();
|
||||
stabilization_init();
|
||||
guidance_init();
|
||||
|
||||
sensors_run(1);
|
||||
|
||||
for i=1:length(fdm_time)-1
|
||||
|
||||
fdm_run(i+1, stabilization_cmd_motors(:,i));
|
||||
sensors_run(i+1);
|
||||
ahrs_propagate(i+1);
|
||||
ins_propagate(i+1);
|
||||
guidance_run(i+1);
|
||||
stabilization_run(i+1);
|
||||
|
||||
end
|
||||
|
||||
if 0
|
||||
set("current_figure",0);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="AHRS";
|
||||
drawlater();
|
||||
ahrs_display();
|
||||
drawnow();
|
||||
|
||||
set("current_figure",1);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="INS";
|
||||
drawlater();
|
||||
ins_display();
|
||||
drawnow();
|
||||
end
|
||||
|
||||
set("current_figure",2);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="STABILIZATION";
|
||||
drawlater();
|
||||
stabilization_display();
|
||||
drawnow();
|
||||
|
||||
set("current_figure",3);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="GUIDANCE";
|
||||
drawlater();
|
||||
guidance_display();
|
||||
drawnow();
|
||||
Reference in New Issue
Block a user