hopefully fixed uninitialized innovation

ctu-mrs · Jan 26, 2024 · 8797ea6 · 8797ea6
1 parent 9f89a4e
commit 8797ea6
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 0 deletions.
diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
@@ -1193,6 +1193,8 @@ void Correction<n_measurements>::callbackVector(const geometry_msgs::Vector3Stam
   auto res = getCorrectionFromVector(msg);
   if (res) {
     applyCorrection(res.value(), msg->header.stamp);
+  } else {
+    ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Vector3Stamped msg", getPrintName().c_str());
   }
 }
 /*//}*/
@@ -1241,6 +1243,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
             measurement = res.value();
             return measurement;
           } else {
+            ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain untilted Z velocity", getPrintName().c_str());
             return {};
           }
           break;

diff --git a/src/estimators/altitude/alt_generic.cpp b/src/estimators/altitude/alt_generic.cpp
@@ -29,6 +29,9 @@ void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
   H_ <<
     1, 0, 0;
 
+  innovation_ << 
+    0, 0, 0;
+
   // clang-format on
 
   // | --------------- initialize parameter loader -------------- |

diff --git a/src/estimators/heading/hdg_generic.cpp b/src/estimators/heading/hdg_generic.cpp
@@ -29,6 +29,10 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
   H_ <<
     1, 0;
 
+  innovation_ << 
+    0, 0;
+
+
   // clang-format on
 
   // | --------------- initialize parameter loader -------------- |

diff --git a/src/estimators/heading/hdg_passthrough.cpp b/src/estimators/heading/hdg_passthrough.cpp
@@ -21,6 +21,10 @@ void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<Commo
   hdg_state_      = states_t::Zero();
   hdg_covariance_ = covariance_t::Zero();
 
+  innovation_ << 
+    0, 0;
+
+
   // | --------------- param loader initialization --------------- |
 
   if (is_core_plugin_) {

diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp
@@ -29,6 +29,10 @@ void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
     1, 0, 0, 0, 0, 0,
     0, 1, 0, 0, 0, 0;
 
+  innovation_ << 
+    0, 0, 0, 0, 0, 0;
+
+
   // clang-format on
 
   // | --------------- initialize parameter loader -------------- |
-Original file line number
+Diff line change
@@ Expand Up @@
       H_ <<
 , 0, 0;
+      innovation_ <<
+, 0, 0;
       // clang-format on
       // | --------------- initialize parameter loader -------------- |
@@ Expand Down @@