diff --git a/sw/simulator/scilab/q6d/q6d_fdm.sci b/sw/simulator/scilab/q6d/q6d_fdm.sci
index 4423a9b8e4..eb83571017 100644
--- a/sw/simulator/scilab/q6d/q6d_fdm.sci
+++ b/sw/simulator/scilab/q6d/q6d_fdm.sci
@@ -54,9 +54,9 @@ fdm_inertia = [0.0078 0.     0.        // inertia tensor
 	       0.     0.     0.0137 ];
 fdm_Ct0 = 4. * fdm_mass * fdm_g / 4;   // thrust coefficient
 //fdm_V0  = 7.;	                       // 
-fdm_V0  = 1000.;	                       // 
-//fdm_Cd  = 0.01;                        // drag coefficient
-fdm_Cd  = 0.000001;                        // drag coefficient
+fdm_V0  = 1e6;	                       // 
+//fdm_Cd  = 0.01;                      // drag coefficient
+fdm_Cd  = 1e-6;                        // drag coefficient
 fdm_la  = 0.25;                        // arm length
 fdm_Cm  = fdm_Ct0 / 10;                // torque coefficient
 
@@ -74,14 +74,15 @@ global fdm_accel;
 global fdm_raccel;
 
 
-function fdm_init(time) 
+function fdm_init(time, X0) 
   global fdm_time;
   fdm_time = time;
   global fdm_state;
   fdm_state = zeros(FDM_SSIZE, length(fdm_time));
-  fdm_state(FDM_SQI, 1) = 1.;
+  fdm_state(:,1) = X0;
   global fdm_euler;
   fdm_euler = zeros(AXIS_NB, length(fdm_time));
+  fdm_euler(:,1) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,1));
   global fdm_accel;
   fdm_accel = zeros(AXIS_NB, length(fdm_time));
   global fdm_raccel;
diff --git a/sw/simulator/scilab/q6d/q6d_sbb.sci b/sw/simulator/scilab/q6d/q6d_sbb.sci
index d1dfea1f84..d3c8d6669b 100644
--- a/sw/simulator/scilab/q6d/q6d_sbb.sci
+++ b/sw/simulator/scilab/q6d/q6d_sbb.sci
@@ -39,7 +39,13 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
   n_comp = 4;  // x, y, z, psi
   order = 5;   
   fo_traj = zeros(n_comp, order, length(time));
+
+  // psi
+  for i=1:length(time)
+    fo_traj(4,1,i) = rad_of_deg(30);
+  end
   
+  // x and y
   disp_xy = b1(1:2) - b0(1:2);
   [pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_xy), dyn(1,:), max_accel(1), max_speed(1));
   fo_traj(1:2,1,1) = b0(1:2);
@@ -61,6 +67,7 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
     fo_traj(1:2,5,i) = -2*dyn(1,2)*dyn(1,1)*fo_traj(1:2,4,i)-dyn(1,1)^2*(fo_traj(1:2,3,i)-sp*u);  
   end
   
+  // z
   disp_z = b1(3) - b0(3);
   [pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_z), dyn(2,:), max_accel(2), max_speed(2));
   fo_traj(3,1,1) = b0(3);
diff --git a/sw/simulator/scilab/q6d/test_stop_stop.sce b/sw/simulator/scilab/q6d/test_stop_stop.sce
index eb81a4d8d8..3fff14541b 100644
--- a/sw/simulator/scilab/q6d/test_stop_stop.sce
+++ b/sw/simulator/scilab/q6d/test_stop_stop.sce
@@ -18,7 +18,7 @@ max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
 //b0 = [ 0    0   0];
 //b1 = [-10   1  -2];
 b0 = [ 0   0   0 ];
-b1 = [ 1   0   0 ];
+b1 = [ 1   0  -1 ];
 [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
 
 
@@ -56,7 +56,13 @@ end
 
 display_motor_cmd(time, motor_cmd);
 
-fdm_init(time);
+X0 = [diff_flat_ref(DF_REF_X,1)  ; diff_flat_ref(DF_REF_Y,1) ; diff_flat_ref(DF_REF_Z,1)
+      diff_flat_ref(DF_REF_XD,1) ; diff_flat_ref(DF_REF_YD,1); diff_flat_ref(DF_REF_ZD,1)
+      quat_of_euler([diff_flat_ref(DF_REF_PHI,1) diff_flat_ref(DF_REF_THETA,1) diff_flat_ref(DF_REF_PSI,1)])
+      diff_flat_ref(DF_REF_P,1)  ; diff_flat_ref(DF_REF_Q,1) ; diff_flat_ref(DF_REF_R,1) 
+      ];
+  
+fdm_init(time, X0);
 for i=2:length(time)
   fdm_run(i, motor_cmd(:,i-1));
 end