diff --git a/rotorpy/trajectories/hover_traj.py b/rotorpy/trajectories/hover_traj.py
index 475a991..a398a66 100644
--- a/rotorpy/trajectories/hover_traj.py
+++ b/rotorpy/trajectories/hover_traj.py
@@ -35,7 +35,8 @@ class HoverTraj(object):
         x_ddddot = np.zeros((3,))
         yaw    = 0
         yaw_dot = 0
+        yaw_ddot = 0
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
diff --git a/rotorpy/trajectories/lissajous_traj.py b/rotorpy/trajectories/lissajous_traj.py
index 5e58a58..b46c235 100644
--- a/rotorpy/trajectories/lissajous_traj.py
+++ b/rotorpy/trajectories/lissajous_traj.py
@@ -66,10 +66,12 @@ class TwoDLissajous(object):
         if self.yaw_bool:
             yaw = np.pi/4*np.sin(np.pi*t)
             yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
+            yaw_ddot = np.pi*np.pi*np.pi/4*np.cos(np.pi*t)
         else:
             yaw = 0
             yaw_dot = 0
+            yaw_ddot = 0
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
diff --git a/rotorpy/trajectories/minsnap.py b/rotorpy/trajectories/minsnap.py
index 6128dcf..a5e36bd 100644
--- a/rotorpy/trajectories/minsnap.py
+++ b/rotorpy/trajectories/minsnap.py
@@ -158,6 +158,7 @@ class MinSnap(object):
         self.x_dddot_poly  = np.zeros((m, 3, 5))
         self.x_ddddot_poly = np.zeros((m, 3, 4))
         self.yaw_dot_poly = np.zeros((m, 1, 7))
+        self.yaw_ddot_poly = np.zeros((m, 1, 6))
         
         # If two or more waypoints remain, solve min snap
         if self.points.shape[0] >= 2:
@@ -203,7 +204,8 @@ class MinSnap(object):
                     self.x_ddot_poly[i,j,:]   = np.polyder(self.x_poly[i,j,:], m=2)
                     self.x_dddot_poly[i,j,:]  = np.polyder(self.x_poly[i,j,:], m=3)
                     self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
-            self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1)
+                self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1)
+                self.yaw_ddot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=2)
 
         else:
             # Otherwise, there is only one waypoint so we just set everything = 0. 
@@ -236,10 +238,12 @@ class MinSnap(object):
         x_ddddot = np.zeros((3,))
         yaw    = 0
         yaw_dot = 0
+        yaw_ddot = 0
 
         if self.null:
             # If there's only one waypoint
             x    = self.points[0,:]
+            yaw = self.yaw[0]
         else:
             # Find interval index i and time within interval t.
             t = np.clip(t, self.t_keyframes[0], self.t_keyframes[-1])
@@ -258,9 +262,10 @@ class MinSnap(object):
 
             yaw = np.polyval(self.yaw_poly[i, 0, :], t)
             yaw_dot = np.polyval(self.yaw_dot_poly[i,0,:], t)
+            yaw_ddot = np.polyval(self.yaw_ddot_poly[i,0,:], t)
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
 
 if __name__=="__main__":
diff --git a/rotorpy/trajectories/polynomial_traj.py b/rotorpy/trajectories/polynomial_traj.py
index 45c6108..632cdfc 100644
--- a/rotorpy/trajectories/polynomial_traj.py
+++ b/rotorpy/trajectories/polynomial_traj.py
@@ -86,6 +86,7 @@ class Polynomial(object):
         x_ddddot = np.zeros((3,))
         yaw = 0
         yaw_dot = 0
+        yaw_ddot = 0
 
         # Find interval index i and time within interval t.
         t = np.clip(t, self.t_start[0], self.t_start[-1]+self.T[-1])
@@ -103,5 +104,5 @@ class Polynomial(object):
             x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
diff --git a/rotorpy/trajectories/speed_traj.py b/rotorpy/trajectories/speed_traj.py
index 8af9e94..e4889b0 100644
--- a/rotorpy/trajectories/speed_traj.py
+++ b/rotorpy/trajectories/speed_traj.py
@@ -70,9 +70,10 @@ class ConstantSpeed(object):
 
         yaw    = 0
         yaw_dot = 0
+        yaw_ddot = 0
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output
 
 if __name__=="__main__":
diff --git a/rotorpy/trajectories/traj_template.py b/rotorpy/trajectories/traj_template.py
index d2efd1f..049adc7 100644
--- a/rotorpy/trajectories/traj_template.py
+++ b/rotorpy/trajectories/traj_template.py
@@ -40,7 +40,8 @@ class TrajTemplate(object):
         x_ddddot = np.zeros((3,))
         yaw    = 0
         yaw_dot = 0
+        yaw_ddot = 0
 
         flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
-                        'yaw':yaw, 'yaw_dot':yaw_dot}
+                        'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
         return flat_output