diff --git a/main/main.ino b/main/main.ino
index e402bcc..7bb274e 100644
--- a/main/main.ino
+++ b/main/main.ino
@@ -252,6 +252,14 @@ void loop()
             }
             else
             {
+                ws.a_avg = {(ws.a_0[0] + ws.a_1[0])*0.5, (ws.a_0[1] + ws.a_1[1])*0.5, (ws.a_0[2] + ws.a_1[2])*0.5};
+                ws.r_body_in_inertial[0] += ws.dt*ws.v_body_wrt_inertial_in_inertial[0] + 0.5*a_avg[0]*ws.dt*ws.dt;
+                ws.r_body_in_inertial[1] += ws.dt*ws.v_body_wrt_inertial_in_inertial[1] + 0.5*a_avg[1]*ws.dt*ws.dt;
+                ws.r_body_in_inertial[2] += ws.dt*ws.v_body_wrt_inertial_in_inertial[2] + 0.5*a_avg[2]*ws.dt*ws.dt;
+
+                ws.v_body_wrt_inertial_in_inertial[0] += ws.dt*a_avg[0];
+                ws.v_body_wrt_inertial_in_inertial[1] += ws.dt*a_avg[1];
+                ws.v_body_wrt_inertial_in_inertial[2] += ws.dt*a_avg[2];
                 //construct x
                 //construct y
                 ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state
diff --git a/main/workspace.hh b/main/workspace.hh
index 0cf57a8..fb6b2b1 100644
--- a/main/workspace.hh
+++ b/main/workspace.hh
@@ -47,6 +47,7 @@ class Workspace
         // sensor measurements
         float a_0[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
         float a_1[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
+        float a_avg[3];
         float w_0[3] = {0.0, 0.0, 0.0};  // (rad/s) angular velocity, used for storing sensor measurements
         float w_1[3] = {0.0, 0.0, 0.0};  // (rad/s) angular velocity, used for storing sensor measurements