diff --git a/include/core-structs.h b/include/core-structs.h
index 57b51e3..6b4052f 100644
--- a/include/core-structs.h
+++ b/include/core-structs.h
@@ -7,6 +7,7 @@
 
 #include <optional>
 #include <Eigen/Dense>
+#include <math.h>
 
 /** A point (position only) in the 2D plane. */
 struct Point2D {
@@ -26,6 +27,7 @@ struct Pose2D {
     float y;           // y-coordinate (scaled in meters)
     float theta_rad;   // heading (radians) wrapped into [-pi, pi]
 
+
     /**
      * @brief overloaded copy assignment operator
      */
@@ -33,7 +35,7 @@ struct Pose2D {
         if (this != &other) {
             this->x = other.x;
             this->y = other.y;
-            this->theta_rad = other.theta_rad;
+            this->theta_rad = wrapAngle(other.theta_rad);
         }
         return *this;
     }
@@ -44,9 +46,29 @@ struct Pose2D {
     struct Pose2D& operator+=(const Eigen::Vector3f& rhs) {
        x += rhs[0];
        y += rhs[1];
-       theta_rad += rhs[2];
+       theta_rad = wrapAngle(theta_rad + rhs[2]);
        return *this;
     }
+
+    /**
+     * @brief wraps angle in radians into [-pi, pi]
+     */
+    static float wrapAngle( float angle_rad ){
+        while (angle_rad < -M_PI) {
+            angle_rad += 2.0 * M_PI;
+        }
+        while (angle_rad > M_PI) {
+            angle_rad -= 2.0 * M_PI;
+        }
+
+        if (std::fabs(angle_rad - M_PI) < 1e-4) {
+            angle_rad = M_PI;
+        } else if (std::fabs(angle_rad + M_PI) < 1e-4f) {
+            angle_rad = -M_PI;
+        }
+        
+        return angle_rad;
+    }
 };
 
 /**
diff --git a/src/FastSLAM/math-util_test.cpp b/src/FastSLAM/math-util_test.cpp
index 4b882d9..88d3749 100644
--- a/src/FastSLAM/math-util_test.cpp
+++ b/src/FastSLAM/math-util_test.cpp
@@ -171,3 +171,91 @@ TEST_CASE( "Test CDF table generation" ){
       REQUIRE( cdf.empty() );
    }
 }
+
+TEST_CASE( "Test wrap around angle" ){
+
+   struct Pose2D pose = {1, 0, 0};
+
+   SECTION( "positive overflow with sum operator" ){
+      Eigen::Vector3f del(0.0f, 0.0f, 3.0f * M_PI_2);
+      pose += del;
+      REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel((-M_PI_2), 0.001));
+   }       
+
+   SECTION( "negative overflow with sum operator" ){
+      Eigen::Vector3f del(0.0f, 0.0f, -4.0f * M_PI_2);
+      pose += del;
+      REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinAbs(-0.0, 0.001));
+   }  
+
+   SECTION( "theta is slightly over pi with sum operator" ){
+      Eigen::Vector3f del(0.0f, 0.0f, M_PI + 0.1);
+      pose += del;
+      REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel(-M_PI + 0.1, 0.001));
+   }  
+
+   SECTION( "theta is slightly under pi with sum operator" ){
+      Eigen::Vector3f del(0.0f, 0.0f, M_PI - 0.1);
+      pose += del;
+      REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel(M_PI - 0.1, 0.001));
+   }  
+
+   SECTION( "sum theta is -pi sum operator" ){
+      struct Pose2D add = {0.0f, 0.0f, -M_PI_2};
+      Eigen::Vector3f delta(0.0f, 0.0f, -M_PI_2);
+      add += delta;
+      REQUIRE_THAT(add.theta_rad, Catch::Matchers::WithinRel(M_PI, 0.001));
+   }   
+
+   SECTION( "sum theta is pi sum operator" ){
+      struct Pose2D add = {0.0f, 0.0f, M_PI_2};
+      Eigen::Vector3f delta(0.0f, 0.0f, M_PI_2);
+      add += delta;
+      REQUIRE_THAT(add.theta_rad, Catch::Matchers::WithinRel(-M_PI, 0.001));
+   }   
+
+   SECTION( "positive overflow with copy assignment operator" ){
+     pose.theta_rad = 3.0 * M_PI;
+     struct Pose2D copied = {0, 0, 0};
+     copied = pose;
+     REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel((-M_PI), 0.001));
+   } 
+   
+   SECTION( "negative overflow with copy assignment operator" ){
+     pose.theta_rad = -5.0f * M_PI_2;
+     struct Pose2D copied;
+     copied = pose;
+     REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI_2, 0.001));
+   } 
+
+   SECTION( "theta is slightly over pi with sum operator" ){
+     pose.theta_rad = M_PI + 0.01;
+     struct Pose2D copied;
+     copied = pose;
+     REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI + 0.01, 0.001));
+   } 
+   
+   SECTION( "theta is slightly under -pi with sum operator" ){
+     pose.theta_rad = -M_PI - 0.001;
+     struct Pose2D copied;
+     copied = pose;
+     REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(M_PI - 0.001, 0.001));
+   } 
+
+   SECTION( "copy asssignment with theta is pi" ){
+      pose.theta_rad = M_PI;
+      struct Pose2D copied = {0, 0, 0.0};
+      copied = pose;
+      REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI, 0.001));
+   }
+
+   SECTION( "copy asssignment with theta is -pi" ){
+      pose.theta_rad = -M_PI;
+      struct Pose2D copied = {0, 0, 0.0};
+      copied = pose;
+      REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(M_PI, 0.001));
+   }
+
+}
+
+