[paparazzi-commits] [4613]

[antoine drouin]

Revision: 4613
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4613
Author:   poine
Date:     2010-03-03 01:29:22 +0000 (Wed, 03 Mar 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci 2010-03-02 
23:42:50 UTC (rev 4612)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_adaptation.sci 2010-03-03 
01:29:22 UTC (rev 4613)
@@ -81,20 +81,20 @@
 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);
+  adp_thetad_f(i) = adp_lp_tau*adp_lp_alpha*sensors_state(SEN_SG,i-1) + 
(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);
+  adp_ud_f(i)     = adp_lp_tau*adp_lp_alpha*ctl_u(CTL_UD,i-1)         + 
(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
+  adp_y(1,i-1) = sqrt(fdm_accel(FDM_AX,i-1)^2 + 
(fdm_accel(FDM_AZ,i-1)+9.81)^2);   // fixme
   // input  
-  W = ctl_u(CTL_UT,i); 
+  W = ctl_u(CTL_UT,i-1); 
   // residual
-  e1 = W*adp_est(1,i-1) - adp_y(1,i);
+  e1 = W*adp_est(1,i-1) - adp_y(1,i-1);
   // update state
   ad = -adp_P(1,1,i-1)*W'*e1;
   adp_est(1,i) = adp_est(1,i-1) + ad * adp_dt;
@@ -105,11 +105,11 @@
   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);
+  adp_y(2,i-1) = sensors_state(SEN_SG,i-1) - 1/adp_lp_tau * adp_thetad_f(i-1);
   // input
-  W =  adp_ud_f(i); 
+  W =  adp_ud_f(i-1); 
   // residual
-  e1 = W*adp_est(2,i-1) - adp_y(2,i);
+  e1 = W*adp_est(2,i-1) - adp_y(2,i-1);
   // update state
   ad = -adp_P(2,2,i-1)*W'*e1;
   adp_est(2,i) = adp_est(2,i-1) + ad * adp_dt;

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-02 
23:42:50 UTC (rev 4612)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
01:29:22 UTC (rev 4613)
@@ -48,18 +48,18 @@
 fb_cmd = zeros(2,length(time));
 motor_cmd = zeros(2,length(time));
 
-adp_init(time, [19 150]', []);
+adp_init(time, [15 100]', []);
 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));
+  diff_flat_cmd(:,i-1) = df_input_of_fo(fo_traj(:,:,i-1), adp_est(1,i-1), 
adp_est(2,i-1));
+  diff_flat_ref(:,i-1) = df_state_of_fo(fo_traj(:,:,i-1));
   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);
+  ctl_u(:,i-1) = diff_flat_cmd(:,i-1) + fb_cmd(:,i-1);
   MotorsOfCmds = 0.5*[1 -1 ; 1 1];
-  motor_cmd(:,i-1) =  MotorsOfCmds * ctl_u(:,i);
+  motor_cmd(:,i-1) =  MotorsOfCmds * ctl_u(:,i-1);
   fdm_run(i, motor_cmd(:,i-1));
   sensors_run(i);
   adp_run(i);