Fixed wrapping issue.

rosflight · Nov 6, 2023 · 71758a6 · 71758a6
1 parent fe1fe9d
commit 71758a6
Showing 1 changed file with 15 additions and 7 deletions.
diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp
@@ -9,6 +9,11 @@ float radians(float degrees)
   return M_PI*degrees/180.0;
 }
 
+double wrap(double fixed_heading, double wrapped_heading) {
+    // wrapped_heading - number_of_times_to_wrap * 2pi
+    return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI;
+}
+
 estimator_example::estimator_example() :
   estimator_base(),
   xhat_a_(Eigen::Vector2f::Zero()),
@@ -237,17 +242,19 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
 
     P_p_ = (A_d_*P_p_*A_d_.transpose() + Q_p_*pow(params.Ts/N_, 2));
   }
-
-    while(xhat_p_(3) > radians(180.0f)) xhat_p_(3) = xhat_p_(3) - radians(360.0f);
-    while(xhat_p_(3) < radians(-180.0f)) xhat_p_(3) = xhat_p_(3) + radians(360.0f);
+
+    xhat_p_(3) = wrap(0.0, xhat_p_(3));
+    // while(xhat_p_(3) > radians(180.0f)) xhat_p_(3) = xhat_p_(3) - radians(360.0f);
+    // while(xhat_p_(3) < radians(-180.0f)) xhat_p_(3) = xhat_p_(3) + radians(360.0f);
     if(xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f))
     {
         RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi");
         xhat_p_(3) = 0;
     }
 
-    while(xhat_p_(6) > radians(180.0f)) xhat_p_(6) = xhat_p_(6) - radians(360.0f);
-    while(xhat_p_(6) < radians(-180.0f)) xhat_p_(6) = xhat_p_(6) + radians(360.0f);
+    xhat_p_(6) = wrap(0.0, xhat_p_(6));
+    // while(xhat_p_(6) > radians(180.0f)) xhat_p_(6) = xhat_p_(6) - radians(360.0f);
+    // while(xhat_p_(6) < radians(-180.0f)) xhat_p_(6) = xhat_p_(6) + radians(360.0f);
     if(xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f))
     {
         RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi");
@@ -290,8 +297,9 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
     //wrap course measurement
     float gps_course = fmodf(input.gps_course, radians(360.0f));
 
-    while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
-    while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f);
+    xhat_p_(3) = wrap(xhat_p_(3), gps_course);
+    // while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
+    // while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f);
     h_p_ = xhat_p_(3);
 
     C_p_ = Eigen::VectorXf::Zero(7);