Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-02 23:39:46 +00:00
parent 4bc9060099
commit d21d9f51ec
10 changed files with 341 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_adaptation.sci
+123
View File
@@ -0,0 +1,123 @@
//
//
// Model parameters estimation
//
//
// estimated parameters
ADP_EST_A = 1; // a = fdm_Ct0/fdm_mass
ADP_EST_B = 2; // b = fdm_la*fdm_Ct0/fdm_inertia
ADP_EST_SIZE = 2;
global adp_est;
global adp_P;
global adp_y;
adp_dt = 1/512;
adp_lp_tau = 0.75;
adp_lp_alpha = adp_dt/(adp_dt+adp_lp_tau);
global adp_thetad_f;
global adp_ud_f;
function adp_init(time, est0, P0)
global adp_est;
adp_est = zeros(ADP_EST_SIZE, length(time));
adp_est(:,1) = est0;
global adp_P;
adp_P = zeros(ADP_EST_SIZE, ADP_EST_SIZE, length(time));
adp_P(:,:,1) = [ 20 0 ; 0 50000];
global adp_y;
adp_y = zeros(ADP_EST_SIZE, length(time));
global adp_thetad_f;
adp_thetad_f = zeros(1, length(time));
global adp_ud_f;
adp_ud_f = zeros(1, length(time));
endfunction
// propagate the adaptation from step i-1 to step 1
function adp_run2(i)
// low pass filter thetad and ud
global adp_thetad_f;
adp_thetad_f(i) = adp_lp_tau*adp_lp_alpha*sensors_state(SEN_SG,i) + (1-adp_lp_alpha)*adp_thetad_f(i-1);
global adp_ud_f;
adp_ud_f(i) = adp_lp_tau*adp_lp_alpha*ctl_u(CTL_UD,i) + (1-adp_lp_alpha)*adp_ud_f(i-1);
// output
global adp_y;
adp_y(:,i) = [ sqrt(fdm_accel(FDM_AX,i)^2 + (fdm_accel(FDM_AZ,i)+9.81)^2) // FIXME
sensors_state(SEN_SG,i) - 1/adp_lp_tau * adp_thetad_f(i) ];
// input
W = [ ctl_u(CTL_UT,i) 0; 0 adp_ud_f(i) ];
global adp_est;
global adp_P;
// residual
e1 = W*adp_est(:,i-1) - adp_y(:,i);
// update state
ad = -adp_P(:,:,i-1)*W'*e1;
// first order integration
adp_est(:,i) = adp_est(:,i-1) + ad * adp_dt;
// pause
// update gain
// lambda = 0.25*(1-norm(adp_P(:,:,i-1))/2.5);
lambda = 1.025;
Pd = lambda*adp_P(:,:,i-1) - adp_P(:,:,i-1)* W' * W * adp_P(:,:,i-1);
adp_P(:,:,i) = adp_P(:,:,i-1) + Pd * dt;
endfunction
// propagate the adaptation from step i-1 to step 1
function adp_run(i)
// low pass filter thetad and ud
global adp_thetad_f;
adp_thetad_f(i) = adp_lp_tau*adp_lp_alpha*sensors_state(SEN_SG,i) + (1-adp_lp_alpha)*adp_thetad_f(i-1);
global adp_ud_f;
adp_ud_f(i) = adp_lp_tau*adp_lp_alpha*ctl_u(CTL_UD,i) + (1-adp_lp_alpha)*adp_ud_f(i-1);
global adp_y;
global adp_est;
global adp_P;
//output
adp_y(1,i) = sqrt(fdm_accel(FDM_AX,i)^2 + (fdm_accel(FDM_AZ,i)+9.81)^2); // fixme
// input
W = ctl_u(CTL_UT,i);
// residual
e1 = W*adp_est(1,i-1) - adp_y(1,i);
// update state
ad = -adp_P(1,1,i-1)*W'*e1;
adp_est(1,i) = adp_est(1,i-1) + ad * adp_dt;
// update gain
lambda = 2.5*(1-abs(adp_P(1,1,i-1))/20);
// lambda = 2.25;
Pd = lambda*adp_P(1,1,i-1) - adp_P(1,1,i-1)* W' * W * adp_P(1,1,i-1);
adp_P(1,1,i) = adp_P(1,1,i-1) + Pd * dt;
//output
adp_y(2,i) = sensors_state(SEN_SG,i) - 1/adp_lp_tau * adp_thetad_f(i);
// input
W = adp_ud_f(i);
// residual
e1 = W*adp_est(2,i-1) - adp_y(2,i);
// update state
ad = -adp_P(2,2,i-1)*W'*e1;
adp_est(2,i) = adp_est(2,i-1) + ad * adp_dt;
// update gain
lambda = 2.5*(1-abs(adp_P(2,2,i-1))/50000);
// lambda = 2.25;
Pd = lambda*adp_P(2,2,i-1) - adp_P(2,2,i-1)* W' * W * adp_P(2,2,i-1);
adp_P(2,2,i) = adp_P(2,2,i-1) + Pd * dt;
endfunction
sw/simulator/scilab/q3d/q3d_ctl.sci
+14 -1
View File
@@ -1,3 +1,8 @@
CTL_UT = 1;
CTL_UD = 2;
CTL_USIZE = 2;
global ctl_u;
// X Z Theta Xd Zd Theta_d
//ctl_gain = [ 0 -1 0 0 -1 0
@@ -8,9 +13,17 @@ ctl_gain = [ 0 0 0 0 0 0
0 0 0 0 0 0 ];
else
ctl_gain = [ 0 -1 0 0 -1 0
-0.95 0 3 -1.2 0 3 ];
0.95 0 -3 1.2 0 -3 ];
end
function ctl_init(time)
global ctl_u;
ctl_u = zeros(CTL_USIZE, length(time));
endfunction
function [fb_cmd] = ctl_compute_feeback(fdm_state, ref)
state_err = fdm_state - ref;
sw/simulator/scilab/q3d/q3d_diff_flatness.sci
+5 -3
View File
@@ -31,12 +31,13 @@ function [state] = df_state_of_fo(fo)
endfunction
// control input from flat output
function [inp] = df_input_of_fo(fo)
function [inp] = df_input_of_fo(fo, model_a, model_b)
x2 = fo(1,3);
z2pg = fo(2,3)+9.81;
u1 = fo_mass / fo_Ct0 * sqrt((x2)^2 + (z2pg)^2);
// u1 = fo_mass / fo_Ct0 * sqrt((x2)^2 + (z2pg)^2);
u1 = 1/model_a * sqrt((x2)^2 + (z2pg)^2);
x3 = fo(1,4);
z3 = fo(2,4);
@@ -46,7 +47,8 @@ function [inp] = df_input_of_fo(fo)
b = z2pg^2+x2^2;
c = 2 * (z2pg*z3 + x2*x3);
d = x3*z2pg-z3*x2;
u2 = fo_J / fo_la /fo_Ct0 * ( a/b - c*d/b^2);
// u2 = -fo_J / fo_la /fo_Ct0 * ( a/b - c*d/b^2);
u2 = -1/model_b * ( a/b - c*d/b^2);
inp = [u1; u2];
sw/simulator/scilab/q3d/q3d_display.sci
+44 -6
View File
@@ -129,7 +129,7 @@ function display_fo_ref(time, diff_flat_ref)
endfunction
function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd );
function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd )
f=get("current_figure");
f.figure_name="Control";
@@ -168,8 +168,7 @@ function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd,
xtitle('Thetad');
subplot(4,2,5);
// plot2d(time, diff_flat_cmd(1,:)+ fb_cmd(1,:), 5);
subplot(4,3,7);
xset("color",5);
foo = diff_flat_cmd(1,:) + fb_cmd(1,:);
xfpoly([time time($:-1:1)], [diff_flat_cmd(1,:) foo($:-1:1)]);
@@ -177,7 +176,7 @@ function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd,
plot2d(time, diff_flat_cmd(1,:), 2);
xtitle('u_t');
subplot(4,2,6);
subplot(4,3,8);
tot_cmd = diff_flat_cmd(2,:) + fb_cmd(2,:);
xset("color",5);
xfpoly([time time($:-1:1)], [diff_flat_cmd(2,:) tot_cmd($:-1:1)]);
@@ -185,14 +184,53 @@ function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd,
plot2d(time, diff_flat_cmd(2,:), 2);
xtitle('u_d');
subplot(4,2,7);
subplot(4,3,10);
plot2d(time, motor_cmd(1,:), 2);
xtitle('u1');
subplot(4,2,8);
subplot(4,3,11);
plot2d(time, motor_cmd(2,:), 2);
xtitle('u2');
endfunction
function display_adaptation()
f=get("current_figure");
f.figure_name="Adaptation";
clf();
subplot(2,3,1);
plot2d(fdm_time, adp_y(1,:), 3);
plot2d(fdm_time, fdm_Ct0/fdm_mass*ctl_u(CTL_UT,:), 2);
legends(["y1", "ut"],[3 2], with_box=%f, opt="ul");
xtitle('apd_y1');
subplot(2,3,2);
plot2d(fdm_time, fdm_Ct0/fdm_mass*ones(1,length(time)),3);
plot2d(fdm_time, adp_est(ADP_EST_A,:), 2);
xtitle('adp_est A');
subplot(2,3,3);
plot2d(fdm_time, matrix(adp_P(1,1,:), 1, length(time)), 2);
xtitle('adp_P11');
subplot(2,3,4);
plot2d(fdm_time, adp_y(2,:), 3);
plot2d(fdm_time, fdm_la*fdm_Ct0/fdm_inertia*adp_ud_f, 2);
legends(["y2", "udf"],[3 2], with_box=%f, opt="ul");
xtitle('ud_f');
subplot(2,3,5);
plot2d(fdm_time, fdm_la*fdm_Ct0/fdm_inertia*ones(1, length(time)),3);
plot2d(fdm_time, adp_est(ADP_EST_B,:), 2);
xtitle('adp_est B');
subplot(2,3,6);
plot2d(fdm_time, matrix(adp_P(2,2,:), 1, length(time)), 2);
xtitle('adp_P22');
endfunction
sw/simulator/scilab/q3d/q3d_fdm.sci
+7 -7
View File
@@ -35,10 +35,10 @@ fdm_la = 0.25; // arm length
fdm_Ct0 = 4. * fdm_mass * fdm_g / 2; // thrust coefficient
fdm_V0 = 1e9; //
fdm_Cd = 1e-2; // drag coefficient
fdm_Cd = 1e-9; // drag coefficient
fdm_min_thrust = 0.05; // 5%
fdm_max_thrust = 1.0; // 400%
fdm_max_thrust = 1.00; // 100%
fdm_wind = [0 0]';
@@ -47,7 +47,7 @@ global fdm_state;
global fdm_accel;
function fdm_init(time, fdm_X0)
function fdm_init(time, fdm_X0, cmd0)
global fdm_time;
fdm_time = time;
@@ -56,7 +56,9 @@ function fdm_init(time, fdm_X0)
fdm_state(:,1) = fdm_X0;
global fdm_accel;
fdm_accel = zeros(FDM_ASIZE, length(fdm_time));
accel = fdm_get_derivatives(time(1), fdm_state(:,1), cmd0);
fdm_accel(:,1) = accel(FDM_SXD:FDM_STHETAD);
endfunction
function fdm_run(i, cmd)
@@ -94,9 +96,7 @@ function [Xdot] = fdm_get_derivatives(t, X, U)
Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass*(lift_ltp+weight_ltp+drag_ltp);
// moments
Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
// pause
Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_LEFT) - U(FDM_MOTOR_RIGHT));
endfunction
sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
+32
View File
@@ -30,6 +30,38 @@ function [fo_traj] = fo_traj_circle(time, _center, radius, omega)
endfunction
function [fo_traj] = fo_traj_swing(time)
omega = 2.;
n_comp = 2;
order = 5;
fo_traj = zeros(n_comp, order, length(time));
for i=1:length(time)
alpha = omega*time(i);
radius = 2;
fo_traj(1,1,i) = radius * cos(alpha);
fo_traj(1,2,i) = -omega * radius * sin(alpha);
fo_traj(1,3,i) = -omega^2 * radius * cos(alpha);
fo_traj(1,4,i) = omega^3 * radius * sin(alpha);
fo_traj(1,5,i) = omega^4 * radius * cos(alpha);
end
endfunction
function [time_out, traj_out] = merge_traj(time_in, traj_in)
time_out = [];
for t=time_in
sw/simulator/scilab/q3d/q3d_sbb.sci
+5 -5
View File
@@ -16,8 +16,8 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
dist = b1(compo)-b0(compo);
if (abs(dist) > 0.01)
step_dt = -log(sbb_tolerance)/(dyn(compo,2)*dyn(compo,1)*sqrt(1-dyn(compo,2)^2));
step_xdd = dist / (2*step_dt^2);
step_xdd = abs(dist) / (2*step_dt^2);
if step_xdd < max_accel(compo)
if step_xdd > max_speed(compo)/step_dt
step_xdd = max_speed(compo)/step_dt;
@@ -30,7 +30,7 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
step_xdd = max_speed(compo)/step_dt;
end
end
t_tot = (dist - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 4*step_dt;
t_tot = (abs(dist) - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 4*step_dt;
else
step_dt = 0;
step_xdd = 0;
@@ -45,9 +45,9 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
fo_traj(compo,1,1) = b0(compo);
for i=2:length(time)
if time(i)-time(1) < step_dt
sp = step_xdd;
sp = sign(dist)*step_xdd;
elseif time(i)-time(1) < t_tot - step_dt & time(i)-time(1) >= t_tot - 2 * step_dt
sp = -step_xdd;
sp = -sign(dist)*step_xdd;
else
sp = 0;
end
sw/simulator/scilab/q3d/q3d_sensors.sci
+36
View File
@@ -0,0 +1,36 @@
SEN_SAX = 1;
SEN_SAZ = 2;
SEN_SG = 3;
SEN_SSIZE = 3;
global sensors_time;
global sensors_state;
gyro_noise = rad_of_deg(3.);
gyro_bias = rad_of_deg(0.);
accel_noise = 0.5;
function sensors_init(time)
global sensors_time;
sensors_time = time;
global sensors_state;
sensors_state = zeros(SEN_SSIZE, length(time));
endfunction
function sensors_run(i)
global sensors_state;
accel_inertial = fdm_accel(FDM_AX:FDM_AZ, i) - [0, -9.81]';
in_2_body = [ cos(fdm_state(FDM_STHETA, i)) sin(fdm_state(FDM_STHETA, i))
-sin(fdm_state(FDM_STHETA, i)) cos(fdm_state(FDM_STHETA, i)) ];
accel_body = in_2_body * accel_inertial + accel_noise * rand(2,1,'normal');
sensors_state(SEN_SAX:SEN_SAZ, i) = accel_body;
sensors_state(SEN_SG, i) = fdm_state(FDM_STHETAD, i) + gyro_noise * rand(1,1,'normal') + gyro_bias;
endfunction
sw/simulator/scilab/q3d/test_adapt.sce
+73
View File
@@ -0,0 +1,73 @@
clear();
exec('q3d_utils.sci');
exec('q3d_sbb.sci');
exec('q3d_fo_traj_misc.sci');
exec('q3d_diff_flatness.sci');
exec('q3d_ctl.sci');
exec('q3d_adaptation.sci');
exec('q3d_fdm.sci');
exec('q3d_sensors.sci');
exec('q3d_display.sci');
t0 = 0;
t1 = 4.;
t2 = 8.;
dt = 1/512;
time1 = t0:dt:t1;
time2 = t1:dt:t2;
// trajectory generation
if 1
dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
max_speed = [ 5 2.5];
max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
b0 = [-5 0];
b1 = [ 5 -5];
b2 = [ 0 -5];
[fo_traj1] = sbb_gen_traj(time1, dyn, max_speed, max_accel, b0, b1);
[fo_traj2] = sbb_gen_traj(time2, dyn, max_speed, max_accel, b1, b2);
[time, fo_traj] = merge_traj(list(time1, time2), list(fo_traj1, fo_traj2));
else
[fo_traj] = fo_traj_swing(time1);
time = time1;
end
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)), [0.25 0.25]');
ctl_init(time);
diff_flat_cmd = zeros(2,length(time));
diff_flat_ref = zeros(FDM_SSIZE, length(time));
fb_cmd = zeros(2,length(time));
motor_cmd = zeros(2,length(time));
adp_init(time, [19 150]', []);
sensors_init(time)
for i=2:length(time)
// diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), adp_est(1,i-1), adp_est(2,i-1));
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1));
global ctl_u;
ctl_u(:,i) = diff_flat_cmd(:,i) + fb_cmd(:,i-1);
MotorsOfCmds = 0.5*[1 -1 ; 1 1];
motor_cmd(:,i-1) = MotorsOfCmds * ctl_u(:,i);
fdm_run(i, motor_cmd(:,i-1));
sensors_run(i);
adp_run(i);
end
set("current_figure",1);
display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd );
set("current_figure",2);
display_adaptation();
sw/simulator/scilab/q3d/test_stop_stop.sce
+2 -2
View File
@@ -43,11 +43,11 @@ end
diff_flat_cmd = zeros(2,length(time));
diff_flat_ref = zeros(FDM_SSIZE, length(time));
for i=1:length(time)
diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
end
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)), [0.25 0.25]');
fb_cmd = zeros(2,length(time));
motor_cmd = zeros(2,length(time));
for i=2:length(time)