diff --git a/conf/autopilot/booz2_board_usb.h b/conf/autopilot/booz2_board_usb.h
new file mode 100644
index 0000000000..f921b2e511
--- /dev/null
+++ b/conf/autopilot/booz2_board_usb.h
@@ -0,0 +1,20 @@
+#ifndef CONFIG_DEMO_H
+#define CONFIG_DEMO_H
+
+/* Master oscillator freq.       */
+#define FOSC (12000000) 
+/* PLL multiplier                */
+#define PLL_MUL (5)         
+/* CPU clock freq.               */
+#define CCLK (FOSC * PLL_MUL) 
+/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
+#define PBSD_BITS 0x02    
+#define PBSD_VAL 2
+/* Peripheral bus clock freq.    */
+#define PCLK (CCLK / PBSD_VAL) 
+
+#define LED_1_BANK 1
+#define LED_1_PIN 24
+
+
+#endif /* CONFIG_DEMO_H */
diff --git a/sw/airborne/booz/test/booz2_test_usb.c b/sw/airborne/booz/test/booz2_test_usb.c
new file mode 100644
index 0000000000..5a4bfecba2
--- /dev/null
+++ b/sw/airborne/booz/test/booz2_test_usb.c
@@ -0,0 +1,57 @@
+#include <inttypes.h>
+
+#include "std.h"
+#include "init_hw.h"
+#include "sys_time.h"
+#include "led.h"
+
+//#include "uart.h"
+#include "usb_serial.h"
+
+#include "messages.h"
+#include "downlink.h"
+
+#include "interrupt_hw.h"
+
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+
+int main( void ) {
+  main_init();
+  while(1) {
+    if (sys_time_periodic())
+      main_periodic_task();
+    main_event_task();
+  }
+  return 0;
+}
+
+static inline void main_init( void ) {
+  hw_init();
+  sys_time_init();
+  led_init();
+
+/*   LED_ON(4); */
+/*   LED_ON(5); */
+/*   LED_ON(6); */
+/*   LED_ON(7); */
+
+  //uart1_init_tx();
+  VCOM_init();
+
+  int_enable();
+}
+
+static inline void main_periodic_task( void ) {
+  RunOnceEvery(100, {
+      LED_TOGGLE(1);
+      DOWNLINK_SEND_ALIVE(16, MD5SUM);
+    });
+}
+
+static inline void main_event_task( void ) {
+
+}
+
diff --git a/sw/simulator/scilab/q3d/q3d_ref_misc.sci b/sw/simulator/scilab/q3d/q3d_ref_misc.sci
index 54250ee935..08f769e786 100644
--- a/sw/simulator/scilab/q3d/q3d_ref_misc.sci
+++ b/sw/simulator/scilab/q3d/q3d_ref_misc.sci
@@ -202,8 +202,47 @@ function [time_out, ref_out] = get_reference_poly3(time_in, ref_in, duration, st
 endfunction
 
 
+function [time_out, ref_out] = get_reference_lti4(time_in, ref_in, duration, pos_out)
+
+  dt = 1/512;
+  time_out = time_in;
+  ref_out = ref_in;
+  for i=1:duration/dt
+    time_out = [time_out time_out($)+dt];
+    refi = ode(ref_out(1:8,$), time_out($-1), time_out($), list(lti4_get_derivatives, pos_out));
+    xdot = lti4_get_derivatives(0, refi, pos_out)
+    ref_out = [ref_out [refi; xdot(7:8)]];
+  end
+  
+endfunction
+
+lti4_omega1 = [ rad_of_deg(35); rad_of_deg(35)]; 
+lti4_zeta1  = [ 0.9; 0.9 ]; 
+
+lti4_omega2 = [ rad_of_deg(720); rad_of_deg(720)]; 
+lti4_zeta2  = [ 0.9; 0.9 ]; 
+
+lti4_a0 = lti4_omega1^2 .* lti4_omega2^2;
+lti4_a1 = 2 * lti4_zeta1 .* lti4_omega1 .* lti4_omega2^2 + ...
+    2 * lti4_zeta2 .* lti4_omega2 .* lti4_omega1^2; 
+lti4_a2 = lti4_omega1^2 + ...
+    2 * lti4_zeta1 .* lti4_omega1 .* lti4_zeta2 .* lti4_omega2 + ...
+    lti4_omega2^2;
+lti4_a3 = 2 * lti4_zeta1 .* lti4_omega1 + 2 * lti4_zeta2 .* lti4_omega2;
+
+lti4_sat_err = 5;
 
 
+function [Xdot] =lti4_get_derivatives(t, X, u)
+
+  Xdot(1:2) = X(3:4);
+  Xdot(3:4) = X(5:6);
+  Xdot(5:6) = X(7:8);
+  err_pos = X(1:2) - u;
+  err_pos = trim(err_pos, -lti4_sat_err, lti4_sat_err);
+  Xdot(7:8) = -lti4_a3 .* X(7:8) -lti4_a2 .* X(5:6) -lti4_a1 .* X(3:4) -lti4_a0.*err_pos;
+
+endfunction
 
 
 
diff --git a/sw/simulator/scilab/q3d/test_12.sce b/sw/simulator/scilab/q3d/test_12.sce
index 8c4d0bb0b4..9b39ea5b14 100644
--- a/sw/simulator/scilab/q3d/test_12.sce
+++ b/sw/simulator/scilab/q3d/test_12.sce
@@ -18,20 +18,30 @@ start = [ -10; 0
 circle_center =  [  0; 0 ];
        
        
-stop  = [  10; 0
+stop  = [  200; 0
            0; 0
 	   0; 0
 	   0; 0
 	   0; 0 ];
 
+if 0
+  time_ref = [0];
+  ref = start;
+  
+  // stay
+  //[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, start);
+  [time_ref, ref] = get_reference_poly3(time_ref, ref, 20, stop);
+  //[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, stop);
+  
+  
+  clf();
+  ref_display(time_ref, ref);
+  pause
+end
+
 time_ref = [0];
 ref = start;
-       
-// stay
-//[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, start);
-[time_ref, ref] = get_reference_poly3(time_ref, ref, 20, stop);
-//[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, stop);
-
+[time_ref, ref] = get_reference_lti4(time_ref, ref, 10, stop(1:2));
 
 clf();
 ref_display(time_ref, ref);