fixed major bug in heading estimation (missing unwrap of correction wrt

current state)
ctu-mrs · Oct 5, 2023 · 7a2888f · 7a2888f
1 parent 00f50d4
commit 7a2888f
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 7 deletions.
diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
@@ -11,6 +11,7 @@
 #include <mrs_lib/dynamic_reconfigure_mgr.h>
 #include <mrs_lib/gps_conversions.h>
 #include <mrs_lib/mutex.h>
+#include <mrs_lib/geometry/cyclic.h>
 
 #include <mrs_msgs/RtkGps.h>
 #include <mrs_msgs/EstimatorCorrection.h>
@@ -144,6 +145,8 @@ class Correction {
   void                                                     callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);
 
   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
+  measurement_t                                               prev_hdg_measurement_;
+  bool                                                        got_first_hdg_measurement_ = false;
 
   mrs_lib::SubscribeHandler<sensor_msgs::Range> sh_range_;
   std::optional<measurement_t>                  getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg);
@@ -769,7 +772,15 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
         case StateId_t::POSITION: {
           measurement_t measurement;
           try {
-            measurement(0) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+            // obtain heading from orientation
+            measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+            // unwrap heading wrt previous measurement
+            if (got_first_hdg_measurement_) {
+              measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+            } else {
+              got_first_hdg_measurement_ = true;
+            }
+            prev_hdg_measurement_ = measurement;
             return measurement;
           }
           catch (...) {
@@ -877,7 +888,15 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
         case StateId_t::POSITION: {
           measurement_t measurement;
           try {
-            measurement(0) = mrs_lib::AttitudeConverter(msg->pose.orientation).getHeading();
+            // obtain heading from orientation
+            measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.orientation).getHeading();
+            // unwrap heading wrt previous measurement
+            if (got_first_hdg_measurement_) {
+              measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+            } else {
+              got_first_hdg_measurement_ = true;
+            }
+            prev_hdg_measurement_ = measurement;
             return measurement;
           }
           catch (...) {

diff --git a/src/estimators/heading/hdg_generic.cpp b/src/estimators/heading/hdg_generic.cpp
@@ -471,25 +471,34 @@ void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_i
     return;
   }
 
+  // copy measurement as we might need to modify it (unwrap)
+  z_t meas = z;
+
   // we do not want to perform corrections until the estimator is initialized
   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
     return;
   }
 
+  statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+
   // for position state check the innovation
   if (H_idx == POSITION) {
+
+    // unwrap the hdg measurement wrt current state
+    meas(POSITION) = mrs_lib::geometry::radians::unwrap(meas(POSITION), sc.x(POSITION));
+
     std::scoped_lock lock(mtx_innovation_);
 
-    innovation_(0) = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(z(0)), mrs_lib::geometry::radians(getState(POSITION)));
+    innovation_(0) = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(meas(0)), mrs_lib::geometry::radians(sc.x(POSITION)));
 
     if (fabs(innovation_(0)) > pos_innovation_limit_) {
       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - hdg: %.2f", getPrintName().c_str(), innovation_(0));
       innovation_ok_ = false;
       changeState(ERROR_STATE);
     }
+
   }
 
-  statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
   try {
     // Apply the correction step
     {
@@ -498,10 +507,9 @@ void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_i
       H_(H_idx) = 1;
       lkf_->H   = H_;
       if (is_repredictor_enabled_) {
-
-        lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+        lkf_rep_->addMeasurement(meas, R_t::Ones() * R, meas_stamp, models_[H_idx]);
       } else {
-        sc = lkf_->correct(sc, z, R_t::Ones() * R);
+        sc = lkf_->correct(sc, meas, R_t::Ones() * R);
       }
     }
     innovation_ok_ = true;