diff --git a/include/sim.h b/include/sim.h
index 6fd1b11..9d11248 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -16,7 +16,7 @@ double limit(double value, double upr, double lwr);
 
 // Any parameters that are constants should be declared here instead of
 // buried in code
-double const dt = 0.01;
+double const dt = 0.001;
 double const g = -9.81;
 
 bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
@@ -35,13 +35,11 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
     pidController(State, PrevState);
     TVC(State, PrevState);
     state2vec(State, PrevState, stateVector, t);
-    //std::cout << State.vz << "\n";
 
     t += State.stepSize;
   } while ((State.z > 0.0));
-std::cout << t << "\n";
+
   write2CSV(stateVector, State, t);
-  std::cout << t << "\n";
 
   bool pass = 1;
 
@@ -60,10 +58,10 @@ std::cout << t << "\n";
   std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
             << std::endl;
 
-  if (landing_velocity < 5.0) {
-    std::cout << "Landing Velocity < 5 m/s | PASS | ";
+  if (landing_velocity < 0.5) {
+    std::cout << "Landing Velocity < 0.5 m/s | PASS | ";
   } else {
-    std::cout << "Landing Velocity < 5 m/s | FAIL | ";
+    std::cout << "Landing Velocity < 0.5 m/s | FAIL | ";
     pass = pass * 0;
   }
   std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
@@ -76,7 +74,7 @@ void burnStartTimeCalc(Vehicle &State) {
   double velocity = State.vz;
   double h = 0;
 
-  double mass, thrust;
+  double mass, thrust, acceleration;
 
   // Piecewise functions for F15 thrust curve
   for (double i = 0.148; i < 3.450; i = i + dt) {
@@ -92,11 +90,14 @@ void burnStartTimeCalc(Vehicle &State) {
     else if ((i > 3.382) && (i < 3.46))
       thrust = -195.78 * i + 675.11;
 
-    velocity = (((thrust / mass) + g) * dt) + velocity;
-    h = (((thrust / mass) + g) * dt) + h;
+    acceleration = (thrust / mass) + g;
+    velocity = integral(acceleration, velocity, State.stepSize);
+    h = integral(velocity, h, State.stepSize);
   }
+
   State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
   State.burnVelocity = velocity;               // terminal velocity
+  std::cout << State.z << std::endl;
 
   double burnStartTime = State.burnVelocity / -g;
   State.simTime = (State.burntime + burnStartTime) * 1000;
@@ -172,6 +173,15 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
     State.vx = integral(State.ax, PrevState.vx, State.stepSize);
     State.vy = integral(State.ay, PrevState.vy, State.stepSize);
     State.vz = integral(State.az, PrevState.vz, State.stepSize);
+    /*
+        // Xe
+        State.x = 0.5 * State.ax * pow(State.stepSize, 2) +
+                  State.vx * State.stepSize + PrevState.x;
+        State.y = 0.5 * State.ay * pow(State.stepSize, 2) +
+                  State.vy * State.stepSize + PrevState.y;
+        State.z = 0.5 * State.az * pow(State.stepSize, 2) +
+                  State.vz * State.stepSize + PrevState.z;
+    */
 
     // Xe
     State.x = integral(State.vx, PrevState.x, State.stepSize);