diff --git a/include/Vehicle.h b/include/Vehicle.h
index e570cc9..41d41d5 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -29,7 +29,9 @@ struct Vehicle {
   double I11dot, I22dot, I33dot;
 
   int maxServo;
+  int maxServoRate;
   double xServoDegs, yServoDegs;
+  double xServoDegsDot, yServoDegsDot;
 
   double Kp, Ki, Kd;
   double yError, yPrevError;
diff --git a/include/outVector.h b/include/outVector.h
index 9b21427..f76d3ee 100644
--- a/include/outVector.h
+++ b/include/outVector.h
@@ -33,6 +33,8 @@ struct outVector {
 
   std::vector<double> PIDx = std::vector<double>(length, 0.0);
   std::vector<double> PIDy = std::vector<double>(length, 0.0);
+
+  std::vector<double> thrust = std::vector<double>(length, 0.0);
 };
 
 #endif
\ No newline at end of file
diff --git a/include/sim.h b/include/sim.h
index f5bb884..b0388b6 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -6,15 +6,16 @@
 void burnStartTimeCalc(struct Vehicle &);
 void thrustSelection(struct Vehicle &, int t);
 void pidController(struct Vehicle &, struct Vehicle &);
-void TVC(struct Vehicle &);
+void TVC(struct Vehicle &, struct Vehicle &);
 void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
 void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
 void write2CSV(struct outVector &, struct Vehicle &);
-double derivative(double x2, double x1, double dt);
-double integral(double x2, double x1, double dt);
+double derivative(double current, double previous, double step);
+double integral(double currentChange, double prevValue, double dt);
+double limit(double value, double upr, double lwr);
 
-// Any parameters that are constants should be declared here instead of buried
-// in code
+// Any parameters that are constants should be declared here instead of
+// buried in code
 double const dt = 0.01;
 double const g = -9.81;
 
@@ -32,7 +33,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
     vehicleDynamics(State, PrevState, t);
     thrustSelection(State, t);
     pidController(State, PrevState);
-    TVC(State);
+    TVC(State, PrevState);
     state2vec(State, PrevState, stateVector, t);
 
     t += State.stepSize;
@@ -246,20 +247,12 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
     State.PIDy = 0;
   }
 
-  // PID Force limiter X
-  if (State.PIDx > State.thrust)
-    State.PIDx = State.thrust;
-  else if (State.PIDx < -1 * State.thrust)
-    State.PIDx = -1 * State.thrust;
-
-  // PID Force limiter Y
-  if (State.PIDy > State.thrust)
-    State.PIDy = State.thrust;
-  else if (State.PIDy < -1 * State.thrust)
-    State.PIDy = -1 * State.thrust;
+  // PID Force Limiter
+  State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
+  State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
 }
 
-void TVC(Vehicle &State) {
+void TVC(Vehicle &State, Vehicle &PrevState) {
   if (State.thrust < 0.1) {
     // Define forces and moments for t = 0
     State.Fx = 0;
@@ -273,21 +266,29 @@ void TVC(Vehicle &State) {
   } else {
     // Convert servo position to degrees for comparison to max allowable
     State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
-
-    // Servo position limiter
-    if (State.xServoDegs > State.maxServo)
-      State.xServoDegs = State.maxServo;
-    else if (State.xServoDegs < -1 * State.maxServo)
-      State.xServoDegs = -1 * State.maxServo;
-
-    // Convert servo position to degrees for comparison to max allowable
     State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
 
-    // Servo position limiter
-    if (State.yServoDegs > State.maxServo)
-      State.yServoDegs = State.maxServo;
-    else if (State.yServoDegs < -1 * State.maxServo)
-      State.yServoDegs = -1 * State.maxServo;
+    // Limit Servo Position
+    State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
+    State.yServoDegs = limit(State.yServoDegs, State.maxServo, -State.maxServo);
+
+    // Servo Rate
+    State.xServoDegsDot =
+        derivative(State.xServoDegs, PrevState.xServoDegs, State.stepSize);
+    State.yServoDegsDot =
+        derivative(State.yServoDegs, PrevState.yServoDegs, State.stepSize);
+
+    // Limit Servo Rate
+    State.xServoDegsDot =
+        limit(State.xServoDegsDot, State.maxServoRate, -State.maxServoRate);
+    State.yServoDegsDot =
+        limit(State.yServoDegsDot, State.maxServoRate, -State.maxServoRate);
+
+    // Back to Degrees
+    State.xServoDegs =
+        integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
+    State.yServoDegs =
+        integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
 
     // Vector math to aqcuire thrust vector components
     State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
@@ -333,6 +334,8 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
   stateVector.PIDx[t] = State.PIDx;
   stateVector.PIDy[t] = State.PIDy;
 
+  stateVector.thrust[t] = State.thrust;
+
   // Set "prev" values for next timestep
   PrevState = State;
 }
@@ -353,40 +356,42 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
   // debugging
   outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
              "pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, "
-             "thrust, deriv"
+             "thrust"
           << std::endl;
 
   // writing to output file
   for (int t = 0; t < State.simTime; t += State.stepSize) {
     outfile << t << ", ";
 
-    outfile << stateVector.x[t] << ", ";
-    outfile << stateVector.y[t] << ", ";
-    outfile << stateVector.z[t] << ", ";
+    outfile << stateVector.x[t] << ",";
+    outfile << stateVector.y[t] << ",";
+    outfile << stateVector.z[t] << ",";
 
-    outfile << stateVector.vx[t] << ", ";
-    outfile << stateVector.vy[t] << ", ";
-    outfile << stateVector.vz[t] << ", ";
+    outfile << stateVector.vx[t] << ",";
+    outfile << stateVector.vy[t] << ",";
+    outfile << stateVector.vz[t] << ",";
 
-    outfile << stateVector.ax[t] << ", ";
-    outfile << stateVector.ay[t] << ", ";
-    outfile << stateVector.az[t] << ", ";
+    outfile << stateVector.ax[t] << ",";
+    outfile << stateVector.ay[t] << ",";
+    outfile << stateVector.az[t] << ",";
 
-    outfile << stateVector.yaw[t] * 180 / M_PI << ", ";
-    outfile << stateVector.pitch[t] * 180 / M_PI << ", ";
-    outfile << stateVector.roll[t] * 180 / M_PI << ", ";
+    outfile << stateVector.yaw[t] * 180 / M_PI << ",";
+    outfile << stateVector.pitch[t] * 180 / M_PI << ",";
+    outfile << stateVector.roll[t] * 180 / M_PI << ",";
 
-    outfile << stateVector.yawdot[t] * 180 / M_PI << ", ";
-    outfile << stateVector.pitchdot[t] * 180 / M_PI << ", ";
-    outfile << stateVector.rolldot[t] * 180 / M_PI << ", ";
+    outfile << stateVector.yawdot[t] * 180 / M_PI << ",";
+    outfile << stateVector.pitchdot[t] * 180 / M_PI << ",";
+    outfile << stateVector.rolldot[t] * 180 / M_PI << ",";
 
-    outfile << stateVector.servo1[t] << ", ";
-    outfile << stateVector.servo2[t] << ", ";
+    outfile << stateVector.servo1[t] << ",";
+    outfile << stateVector.servo2[t] << ",";
 
-    outfile << stateVector.thrustFiring[t] << ", ";
+    outfile << stateVector.thrustFiring[t] << ",";
 
-    outfile << stateVector.PIDx[t] << ", ";
-    outfile << stateVector.PIDy[t] << std::endl;
+    outfile << stateVector.PIDx[t] << ",";
+    outfile << stateVector.PIDy[t] << ",";
+
+    outfile << stateVector.thrust[t] << std::endl;
   }
 
   outfile.close();
@@ -401,3 +406,14 @@ double derivative(double current, double previous, double step) {
 double integral(double currentChange, double prevValue, double dt) {
   return (currentChange * dt / 1000) + prevValue;
 }
+
+double limit(double value, double upr, double lwr) {
+  if (value > upr)
+    value = upr;
+  else if (value < lwr)
+    value = lwr;
+  else
+    value = value;
+
+  return value;
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index c2f29fd..f3bf0d1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -28,9 +28,10 @@ int main() {
   State.vz = 0; // [m/s]
 
   // Initial YPR
-  State.yaw = 75 * M_PI / 180;  // [rad]
-  State.pitch = 35 * M_PI / 180; // [rad]
-  State.roll = 0 * M_PI / 180;  // [rad]
+
+  State.yaw = 75 * M_PI / 180;   // [rad]
+  State.pitch = 30 * M_PI / 180; // [rad]
+  State.roll = 0 * M_PI / 180;   // [rad]
 
   // Initial YPRdot
   State.yawdot = 1 * M_PI / 180;    // [rad/s]
@@ -38,7 +39,8 @@ int main() {
   State.rolldot = 0 * M_PI / 180;   // [rad/s]
 
   // Servo Limitation
-  State.maxServo = 15; // [degs]
+  State.maxServo = 7;   // [degs]
+  State.maxServoRate = 360; // [degs/sec]
 
   // Vehicle Properties
   State.massInitial = 1.2;       // [kg]