From 7a2888f6c71527c0bbe51a480d4cb384177f6fe0 Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Thu, 5 Oct 2023 16:30:02 +0200
Subject: [PATCH] fixed major bug in heading estimation (missing unwrap of
 correction wrt current state)

---
 .../estimators/correction.h                   | 23 +++++++++++++++++--
 src/estimators/heading/hdg_generic.cpp        | 18 +++++++++++----
 2 files changed, 34 insertions(+), 7 deletions(-)

diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
index ecd696d..2486f19 100644
--- 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
index dfc7403..12140ca 100644
--- 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;