diff --git a/main/main.ino b/main/main.ino
index 5f4e9ae..85d7d3c 100644
--- a/main/main.ino
+++ b/main/main.ino
@@ -1,6 +1,6 @@
 #include "I2Cdev.h"
 #include "moding.hh"
-//#include <SPIFlash.h>  // flash chip library
+#include <SPIFlash.h>  // flash chip library
 #include "BNO055_support.h"		//Contains the bridge code between the API and Arduino
 #include <Wire.h>
 #include "workspace.hh"  // variable storage
@@ -8,7 +8,7 @@
 #include "matrix.hh"
 #include "mission_constants.hh"
 #include <cmath>
-
+#include <vector>
 using namespace std;
 
 // declare flash chip 
@@ -18,14 +18,11 @@ using namespace std;
 // (--) instance of workspace class storing all the variables used in the loop
 Workspace ws;
 
-int count=0;
-
-
 void main_display_matrix(Matrix param)
 {
   for (int i=0; i<param.rows*param.columns; i++)
   {
-     Serial.print(param.values[i]);
+     Serial.print(param.values[i], 4);
      Serial.print("    ");
   }
   Serial.println(" ");
@@ -71,8 +68,11 @@ void send_tvc(Matrix u, Matrix * last_u, double yaw)
 void setup()
 {
     // set up pins (OUTPUT and LOW are defined in Arduino.h)
-    pinMode(15, OUTPUT);
+    pinMode(15, INPUT);
     digitalWrite(15, HIGH);
+
+    pinMode(0, INPUT);
+    digitalWrite(0, LOW);
     
     //TODO flash setup
     //flash.begin(9600);  // begins flash chip at specified baud rate
@@ -88,7 +88,16 @@ void setup()
     Wire.begin();
 
     //Initialization of the BNO055
-    BNO_Init(&ws.myBNO); //Assigning the structure to hold information about the device
+    BNO_Init(&ws.bno_1); //Assigning the structure to hold information about the device
+    BNO_Init(&ws.bno_2); //Assigning the structure to hold information about the device
+
+    bno055_set_axis_remap_value(0X06);
+    delay(1);
+    bno055_set_z_remap_sign(0x01);
+    delay(1);
+    bno055_set_z_remap_sign(0x01);
+    delay(1);
+
 
     //Configuration to NDoF mode
     bno055_set_operation_mode(OPERATION_MODE_NDOF);
@@ -108,22 +117,11 @@ void setup()
 void loop()
 {
     // update the clock
-    ws.dt = micros() - ws.t_prev_cycle;  // (us) time step since previous loop
+    ws.dt = float(micros() - ws.t_prev_cycle);  // (us) time step since previous loop
     ws.t_prev_cycle += ws.dt;  // (us) update time for next loop
 
-    bno055_read_euler_hrp(&ws.myEulerData);
-    ws.yaw=float(ws.myEulerData.h)/16.00*DEG_2_RAD;
-    
-    //Serial.print(float(ws.myEulerData.h/16.00));
-    //Serial.print("    ");
-    //Serial.print(float(ws.myEulerData.r/16.00));
-    //Serial.print("    ");
-    //Serial.println(float(ws.myEulerData.p/16.00));
-    
-    ws.theta_0[0]=float(ws.myEulerData.r/16.00);
-    ws.theta_1[0]=float(ws.myEulerData.r/16.00);
-    ws.theta_0[1]=float(ws.myEulerData.p/16.00);
-    ws.theta_1[1]=float(ws.myEulerData.p/16.00);
+    bno055_read_euler_hrp(&ws.euler_1);
+    bno055_read_euler_hrp(&ws.euler_2);
 
     // check for moding change conditions
     switch (ws.mode)
@@ -153,7 +151,7 @@ void loop()
             else
             {
                 ws.construct_y();
-                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors
             }
             break;
         }
@@ -161,6 +159,9 @@ void loop()
         {
             if (change_mode_to_prep_tvc(millis() > ws.next_mode_time))
             {
+                digitalWrite(15, HIGH);
+                delay(1);
+                digitalWrite(15, HIGH);
                 transition_to_prep_tvc();
                 ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I;
                 ws.mode = PREP_TVC;
@@ -168,7 +169,7 @@ void loop()
             else
             {
                 ws.construct_y();
-                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors
             }
             break;
         }
@@ -184,8 +185,8 @@ void loop()
             {
                 ws.construct_y();
                 ws.u=(KC*ws.x).scale(-1);//calculate input
-                send_tvc(ws.u, &ws.last_u, ws.yaw);
-                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors
+                //send_tvc(ws.u, &ws.last_u, ws.yaw);
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors
             }
             break;
         }
@@ -200,8 +201,8 @@ void loop()
             {
                 ws.construct_y();
                 ws.u=(KC*ws.x).scale(-1); //calculate input
-                send_tvc(ws.u, &ws.last_u, ws.yaw);
-                ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation using Kalman filter
+                //send_tvc(ws.u, &ws.last_u, ws.yaw);
+                ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation using Kalman filter
             }
             break;
         }
@@ -212,6 +213,10 @@ void loop()
         }
         
     }
+    Serial.println(ws.mode);
+    
+    //Serial.print(millis());
+    main_display_matrix(ws.x);
     // TODO: add data record
     // TODO: add (somewhere else) data struct
 }
diff --git a/main/matrix.cpp b/main/matrix.cpp
index 8472180..d267530 100644
--- a/main/matrix.cpp
+++ b/main/matrix.cpp
@@ -1,7 +1,6 @@
 #include "matrix.hh"
 #include <string>
 #include <vector>
-
 using namespace std;
 
 //matrix multiplication
diff --git a/main/matrix.hh b/main/matrix.hh
index 80fc910..fd16c51 100644
--- a/main/matrix.hh
+++ b/main/matrix.hh
@@ -4,6 +4,7 @@
 #include <cstdbool>
 #include <cstdint>
 #include <string>
+#include <vector>
 using namespace std;
 
 
diff --git a/main/mission_constants.hh b/main/mission_constants.hh
index 4d7f117..090a371 100644
--- a/main/mission_constants.hh
+++ b/main/mission_constants.hh
@@ -3,7 +3,6 @@
 
 #include "matrix.hh"
 #include <vector>
-
 using namespace std;
 
 //*****************************************************************************
@@ -32,6 +31,8 @@ enum Mode {
     SHUTDOWN_STABLE = 5
 };
 
+const bool RESTRICT_U_AXIS=false;
+
 //most modes change by time
 //after a predefined time period we switch to the next mode
 //MODE TIME PERIODS
@@ -81,6 +82,10 @@ const float BETA = 0.95;  // angle (rad) that corrects for misalignment between
 const float QR_IMU_TO_BODY[4] = {1.0, 0.0, 0.0, 0.0};  // (--) right, scalar-first, Hamiltonian quaternion transforming IMU frame to body frame
 const float QR_BODY_TO_IMU[4] = {1.0, 0.0, 0.0, 0.0};  // (--) right, scalar-first, Hamiltonian quaternion transforming body frame to IMU frame
 
+//IMU parameters
+const float EULER_X_OFFSET;
+const float EULER_Y_OFFSET;
+
 //rocket physical parameters
 const float THRUST=10; //TODO: make exact
 const float MOMENT_ARM=.265; //TODO: make exact
@@ -89,38 +94,38 @@ const float MOMENT_INERTIA_YY=1;//TODO: make exact
 const float MASS=1;
 
 //control constants TODO: fill these out
-vector<float>  A_VALUES {0,THRUST/MASS,0,0,0,0,
+const vector<float>  A_VALUES {0,THRUST/MASS,0,0,0,0,
                 0,0,1,0,0,0,
                 0,0,0,0,0,0,
                 0,0,0,-THRUST/MASS,0,0,
                 0,0,0,0,0,1,
                 0,0,0,0,0,0}; //dynamics matrix
-Matrix A=Matrix(6, 6, A_VALUES);        
+const Matrix A=Matrix(6, 6, A_VALUES);        
  
-vector<float> B_VALUES {THRUST/MASS, 0, 
+const vector<float> B_VALUES {THRUST/MASS, 0, 
                         0, 0,
                         -THRUST*MOMENT_ARM/MOMENT_INERTIA_YY, 0,
                         0, -THRUST/MASS,
                         0, 0,
                         0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix
-Matrix B=Matrix(6, 2, B_VALUES);
+const Matrix B=Matrix(6, 2, B_VALUES);
  
-vector<float> K_VALUES {0,0,0,0,0,0,
-                        0,0,0,0,0,0}; //controller gain
-Matrix KC=Matrix(2, 6, K_VALUES);
+const vector<float> K_VALUES {-2.582,-6.9449,-.5665,0,0,0,
+                        0,0,0,2.582,-6.9449,-.5665}; //controller gain
+const Matrix KC=Matrix(2, 6, K_VALUES);
 
-vector<float> C_VALUES {1,0,0,0,0,0,
+const vector<float> C_VALUES {1,0,0,0,0,0,
                         0,0,1,0,0,0,
                         0,0,0,1,0,0,
                         0,0,0,0,0,1}; //sensor matrix
-Matrix C=Matrix(4, 6, C_VALUES);
+const Matrix C=Matrix(4, 6, C_VALUES);
  
-vector<float> L_VALUES {0, 0, 0, 0,
-                        0, 0, 0, 0,
-                        0, 0, 0, 0,
-                        0, 0, 0, 0,
-                        0, 0, 0, 0,
-                        0, 0, 0, 0}; //kalman gain
-Matrix L=Matrix(6, 4, L_VALUES);
+const vector<float> L_VALUES {13.824, 6.9192, 0, 0,
+                        6.9192, 8.3015, 0, 0,
+                        20.0079, 53.3949, 0, 0,
+                        0, 0, 13.824, -6.9192,
+                        0, 0, -6.9192, 8.3015,
+                        0, 0, -20.0079, 53.3949}; //kalman gain
+const Matrix L=Matrix(6, 4, L_VALUES);
 
 #endif
diff --git a/main/workspace.hh b/main/workspace.hh
index 0280be4..456f8a9 100644
--- a/main/workspace.hh
+++ b/main/workspace.hh
@@ -7,6 +7,8 @@
 #include "matrix.hh"
 #include <Servo.h>  // TODO: what's this?
 #include "Wire.h"  // Arduino library
+#include <vector>
+using namespace std;
 
 
 /* Workspace
@@ -24,8 +26,11 @@ class Workspace
         //*****************************************************************************
         //                              HARDWARE SETUP
         //*****************************************************************************
-        struct bno055_t myBNO;
-        struct bno055_euler myEulerData;
+        struct bno055_t bno_1;
+        struct bno055_euler euler_1;
+
+        struct bno055_t bno_2;
+        struct bno055_euler euler_2;
 
         //*****************************************************************************
         //                              LOOP VARIABLES
@@ -33,7 +38,7 @@ class Workspace
         // clock time
         unsigned long calibrate_time = 0;  //momment main loop starts
         unsigned long t_prev_cycle = 0;  // (us) contains the time of the previous cycle at the start of each loop
-        unsigned long dt = 0;  // (us) used to store time difference between t_prev_cycle and return of micros() at the start of each loop
+        float dt = 0;  // (us) used to store time difference between t_prev_cycle and return of micros() at the start of each loop
 
         // sensor measurements
         float a_0[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
@@ -51,14 +56,15 @@ class Workspace
 
         //control vectors
         // set up controller
-        float initialize_6 [6]={0, 0, 0, 0, 0, 0};
-        float initialize_4 [4]={0, 0, 0, 0};
-        float initialize_2 [2]={0, 0};
+        vector<float> initialize_6 {0, 0, 0, 0, 0, 0};
+        vector<float> initialize_4 {0, 0, 0, 0};
+        vector<float> initialize_2 {0, 0};
         Matrix x=Matrix(6, 1, initialize_6);
         Matrix y=Matrix(4, 1, initialize_4);
         Matrix u=Matrix(2, 1, initialize_2);
         Matrix last_u=Matrix(2, 1, initialize_2);
         vector<float> y_values {0, 0, 0, 0};
+        vector<float> last_y_values {0, 0, 0, 0};
         float yaw;        
 
         // state
@@ -69,10 +75,18 @@ class Workspace
 
         void construct_y()
         {
+            theta_0[2]=float(euler_1.h/16.00);
+            theta_1[2]=float(euler_2.h/16.00);
+            theta_0[1]=float(euler_1.r/16.00);
+            theta_1[1]=float(euler_2.r/16.00);
+            theta_0[0]=float(euler_1.p/16.00);
+            theta_1[0]=float(euler_2.p/16.00);
+          
             y_values[0]=0;
-            y_values[1]=(theta_0[1]+theta_1[1])/2;
+            y_values[1]=(theta_0[1]+theta_1[1])/2.0*DEG_2_RAD;
             y_values[2]= 0;
-            y_values[3]= (theta_0[0]+theta_1[0])/2;
+            y_values[3]= (theta_0[0]+theta_1[0])/2.0*DEG_2_RAD;
+            yaw=(theta_0[2]+theta_1[2])/2.0*DEG_2_RAD;
             y.redefine(y_values);
         }
 };