diff --git a/sw/simulator/scilab/q3d/q3d_adaptation.sci b/sw/simulator/scilab/q3d/q3d_adaptation.sci new file mode 100644 index 0000000000..3f64a4ddf5 --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_adaptation.sci @@ -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 + diff --git a/sw/simulator/scilab/q3d/q3d_ctl.sci b/sw/simulator/scilab/q3d/q3d_ctl.sci index 471998c346..3198150e72 100644 --- a/sw/simulator/scilab/q3d/q3d_ctl.sci +++ b/sw/simulator/scilab/q3d/q3d_ctl.sci @@ -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; diff --git a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci index 84820b1aa1..bb9494298a 100644 --- a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci +++ b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci @@ -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]; diff --git a/sw/simulator/scilab/q3d/q3d_display.sci b/sw/simulator/scilab/q3d/q3d_display.sci index 6a46a7931b..806cc1135c 100644 --- a/sw/simulator/scilab/q3d/q3d_display.sci +++ b/sw/simulator/scilab/q3d/q3d_display.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/q3d_fdm.sci b/sw/simulator/scilab/q3d/q3d_fdm.sci index 479c31fbb5..b1ecb309d0 100644 --- a/sw/simulator/scilab/q3d/q3d_fdm.sci +++ b/sw/simulator/scilab/q3d/q3d_fdm.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci b/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci index e5119bf593..e0f745b45a 100644 --- a/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci +++ b/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/q3d_sbb.sci b/sw/simulator/scilab/q3d/q3d_sbb.sci index d591170cc9..1e0885c044 100644 --- a/sw/simulator/scilab/q3d/q3d_sbb.sci +++ b/sw/simulator/scilab/q3d/q3d_sbb.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/q3d_sensors.sci b/sw/simulator/scilab/q3d/q3d_sensors.sci new file mode 100644 index 0000000000..754c29d79d --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_sensors.sci @@ -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 + diff --git a/sw/simulator/scilab/q3d/test_adapt.sce b/sw/simulator/scilab/q3d/test_adapt.sce new file mode 100644 index 0000000000..2c974be252 --- /dev/null +++ b/sw/simulator/scilab/q3d/test_adapt.sce @@ -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(); + diff --git a/sw/simulator/scilab/q3d/test_stop_stop.sce b/sw/simulator/scilab/q3d/test_stop_stop.sce index 64069e2f47..87a3aa1858 100644 --- a/sw/simulator/scilab/q3d/test_stop_stop.sce +++ b/sw/simulator/scilab/q3d/test_stop_stop.sce @@ -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)