diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
index a1cf9498..db4ca60e 100644
--- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
+++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
@@ -25,6 +25,8 @@
 
 #include <gtest/gtest.h>
 
+#include <ignition/gazebo/Link.hh>
+#include <ignition/gazebo/Model.hh>
 #include <ignition/gazebo/TestFixture.hh>
 #include <ignition/gazebo/Util.hh>
 #include <ignition/gazebo/World.hh>
@@ -86,6 +88,19 @@ class LrauvTestFixture : public ::testing::Test
         this->tethysPoses.push_back(
             ignition::gazebo::worldPose(modelEntity, _ecm));
         this->iterations++;
+
+        ignition::gazebo::Model model(modelEntity);
+        auto linkEntity = model.LinkByName(_ecm, "base_link");
+        EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity);
+
+        ignition::gazebo::Link link(linkEntity);
+        auto linkAngVel = link.WorldAngularVelocity(_ecm);
+        EXPECT_TRUE(linkAngVel.has_value());
+        tethysAngularVel.push_back(linkAngVel.value());
+
+        auto linkVel = link.WorldLinearVelocity(_ecm);
+        EXPECT_TRUE(linkVel.has_value());
+        tethysLinearVel.push_back(linkVel.value());
       });
     fixture->Finalize();
   }
@@ -268,6 +283,12 @@ class LrauvTestFixture : public ::testing::Test
   /// \brief All tethys world poses in order
   public: std::vector<ignition::math::Pose3d> tethysPoses;
 
+  /// \brief All tethys linear velocities in order
+  public: std::vector<ignition::math::Vector3d> tethysLinearVel;
+
+  /// \brief All tethys angular velocities in order
+  public: std::vector<ignition::math::Vector3d> tethysAngularVel;
+
   /// \brief All state messages in order
   public: std::vector<lrauv_ignition_plugins::msgs::LRAUVState> stateMsgs;
 
diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
index 8d3880dc..92ae52cb 100644
--- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
+++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
@@ -106,7 +106,7 @@ TEST_F(LrauvTestFixture, YoYoCircle)
     EXPECT_LT(-22.7, pose.Pos().Z()) << i;
     if (i > 4000)
     {
-      EXPECT_GT(0.4, pose.Pos().Z()) << i;
+      EXPECT_GT(0.5, pose.Pos().Z()) << i;
     }
 
 
@@ -117,11 +117,20 @@ TEST_F(LrauvTestFixture, YoYoCircle)
     // Trajectory projected onto the horizontal plane is roughly a circle
     ignition::math::Vector2d planarPos{pose.Pos().X(), pose.Pos().Y()};
 
-    //ignition::math::Vector2d center{-4.0, -23.0};
-    //planarPos -= center;
+    if (i > 6000)
+    {
+      // Once the vehicle achieves its full velocity the vehicle should have a
+      // nominal yaw rate of around 0.37-0.38rad/s. This means that the vehicle
+      // should keep spinning in a circle.
+      EXPECT_NEAR(tethysAngularVel[i].Z(), 0.037, 2e-2)
+        << i << " yaw rate: " << tethysAngularVel[i].Z();
+
+      // At the same time the roll rate should be near zero
+      EXPECT_NEAR(tethysAngularVel[i].Y(), 0, 1e-1) << i;
 
-    //double meanRadius{20.0};
-    //EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i << ", " << planarPos;
+      // And the linear velocity should be near 1m/s
+      EXPECT_NEAR(tethysLinearVel[i].Length(), 1.0, 2e-1) << i;
+    }
   }
 }