[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4611]
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [4611] |
|
Date: |
Tue, 02 Mar 2010 23:39:46 +0000 |
Revision: 4611
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4611
Author: poine
Date: 2010-03-02 23:39:46 +0000 (Tue, 02 Mar 2010)
Log Message:
-----------
Modified Paths:
--------------
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
Added Paths:
-----------
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sensors.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
Added: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci
(rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -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
+
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci 2010-03-02
17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -1,4 +1,9 @@
+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
// -2 0 5 -2 0 5 ];
@@ -8,9 +13,17 @@
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;
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
2010-03-02 17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
2010-03-02 23:39:46 UTC (rev 4611)
@@ -31,12 +31,13 @@
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 @@
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];
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci 2010-03-02
17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -129,7 +129,7 @@
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 @@
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 @@
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 @@
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
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci 2010-03-02
17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -35,10 +35,10 @@
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_accel;
-function fdm_init(time, fdm_X0)
+function fdm_init(time, fdm_X0, cmd0)
global fdm_time;
fdm_time = time;
@@ -56,7 +56,9 @@
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 @@
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));
+ Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_LEFT) -
U(FDM_MOTOR_RIGHT));
-// pause
-
endfunction
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
2010-03-02 17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
2010-03-02 23:39:46 UTC (rev 4611)
@@ -30,6 +30,38 @@
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
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci 2010-03-02
17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -16,8 +16,8 @@
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 @@
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 @@
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
Added: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sensors.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sensors.sci
(rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sensors.sci 2010-03-02
23:39:46 UTC (rev 4611)
@@ -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
+
Added: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
(rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce 2010-03-02
23:39:46 UTC (rev 4611)
@@ -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();
+
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-03-02
17:56:36 UTC (rev 4610)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-03-02
23:39:46 UTC (rev 4611)
@@ -43,11 +43,11 @@
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)
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4611],
antoine drouin <=