UKF update (#751)

* Pulling UKF changes from Noetic

* Adding a reset service to make tests more deterministic

Author: ayrton04

include/robot_localization/filter_utilities.hpp

@@ -59,24 +59,6 @@ namespace robot_localization
 namespace filter_utilities
 {
 
-/**
- * @brief Utility method keeping RPY angles in the range [-pi, pi]
- * @param[in] rotation - The rotation to bind
- * @return the bounded value
- */
-inline double clampRotation(double rotation)
-{
-  while (rotation > PI) {
-    rotation -= TAU;
-  }
-
-  while (rotation < -PI) {
-    rotation += TAU;
-  }
-
-  return rotation;
-}
-
 /**
  * @brief Utility method for appending tf2 prefixes cleanly
  * @param[in] tf_prefix - the tf2 prefix to append

include/robot_localization/ros_filter.hpp

@@ -279,6 +279,13 @@ class RosFilter : public rclcpp::Node
   //!
   void initialize();
 
+  //! @brief Service callback for resetting the filter to its initial state. Parameters are unused.
+  //!
+  void resetSrvCallback(
+    const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<std_srvs::srv::Empty::Request>,
+    const std::shared_ptr<std_srvs::srv::Empty::Response>);
+
   //! @brief Callback method for manually setting/resetting the internal pose
   //! estimate
   //! @param[in] msg - The ROS stamped pose with covariance message to take in
@@ -553,6 +560,10 @@ class RosFilter : public rclcpp::Node
   //!
   rclcpp::Duration history_length_;
 
+  //! @brief The sensor timeout value that gets passed to the core filter
+  //!
+  rclcpp::Duration sensor_timeout_;
+
   //! @brief tf frame name for the robot's body frame
   //!
   std::string base_link_frame_id_;
@@ -586,6 +597,14 @@ class RosFilter : public rclcpp::Node
   //!
   Eigen::VectorXd latest_control_;
 
+  //! @brief The process noise covariance matrix that gets passed to the core filter
+  //!
+  Eigen::MatrixXd process_noise_covariance_;
+
+  //! @brief The initial estimate error covariance matrix that gets passed to the core filter
+  //!
+  Eigen::MatrixXd initial_estimate_error_covariance_;
+
   //! @brief Message that contains our latest transform (i.e., state)
   //!
   //! We use the vehicle's latest state in a number of places, and often
@@ -745,6 +764,10 @@ class RosFilter : public rclcpp::Node
   //!
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr enable_filter_srv_;
 
+  //! @brief Service that resets the filter to its initial state
+  //!
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv_;
+
   //! @brief Transform buffer for managing coordinate transforms
   //!
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

include/robot_localization/ukf.hpp

@@ -93,6 +93,18 @@ class Ukf : public FilterBase
     const rclcpp::Duration & delta) override;
 
 protected:
+  /**
+   * @brief  Computes the weighted covariance and sigma points
+   */
+  void generateSigmaPoints();
+
+  /**
+   * @brief  Carries out the predict step for the posteriori state of a sigma point
+   * @param[in,out] sigma_point - The sigma point (state vector) to project
+   * @param[in] delta - The time step over which to project
+   */
+  void projectSigmaPoint(Eigen::VectorXd & sigma_point, const rclcpp::Duration & delta);
+
   /**
    * @brief  The UKF sigma points
    *

src/filter_base.cpp

@@ -35,6 +35,8 @@
 #include <robot_localization/filter_common.hpp>
 #include <robot_localization/filter_utilities.hpp>
 
+#include <angles/angles.h>
+
 #include <iomanip>
 #include <iostream>
 #include <limits>
@@ -420,12 +422,9 @@ inline double FilterBase::computeControlAcceleration(
 
 void FilterBase::wrapStateAngles()
 {
-  state_(StateMemberRoll) =
-    filter_utilities::clampRotation(state_(StateMemberRoll));
-  state_(StateMemberPitch) =
-    filter_utilities::clampRotation(state_(StateMemberPitch));
-  state_(StateMemberYaw) =
-    filter_utilities::clampRotation(state_(StateMemberYaw));
+  state_(StateMemberRoll) = angles::normalize_angle(state_(StateMemberRoll));
+  state_(StateMemberPitch) = angles::normalize_angle(state_(StateMemberPitch));
+  state_(StateMemberYaw) = angles::normalize_angle(state_(StateMemberYaw));
 }
 
 bool FilterBase::checkMahalanobisThreshold(

src/filter_utilities.cpp

@@ -92,7 +92,7 @@ std::ostream & operator<<(std::ostream & os, const std::vector<int> & vec)
   os << "[";
   for (size_t dim = 0; dim < vec.size(); ++dim) {
     os << std::setiosflags(std::ios::left) << std::setw(3) <<
-    (vec[dim] ? "t" : "f");
+      (vec[dim] ? "t" : "f");
   }
   os << "]\n";
 

src/navsat_transform.cpp

@@ -36,6 +36,7 @@
 #include <robot_localization/navsat_transform.hpp>
 #include <robot_localization/ros_filter_utilities.hpp>
 
+#include <angles/angles.h>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -676,9 +677,9 @@ void NavSatTransform::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
       // Apply the offset (making sure to bound them), and throw them in a
       // vector
       tf2::Vector3 rpy_angles(
-        filter_utilities::clampRotation(roll - roll_offset),
-        filter_utilities::clampRotation(pitch - pitch_offset),
-        filter_utilities::clampRotation(yaw - yaw_offset));
+        angles::normalize_angle(roll - roll_offset),
+        angles::normalize_angle(pitch - pitch_offset),
+        angles::normalize_angle(yaw - yaw_offset));
 
       // Now we need to rotate the roll and pitch by the yaw offset value.
       // Imagine a case where an IMU is mounted facing sideways. In that case

src/ros_filter.cpp

@@ -36,6 +36,7 @@
 #include <robot_localization/ros_filter_utilities.hpp>
 #include <robot_localization/ukf.hpp>
 
+#include <angles/angles.h>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <rcl/time.h>
 #include <rclcpp/qos.hpp>
@@ -75,7 +76,10 @@ RosFilter<T>::RosFilter(const rclcpp::NodeOptions & options)
   frequency_(30.0),
   gravitational_acceleration_(9.80665),
   history_length_(0ns),
+  sensor_timeout_(0ns),
   latest_control_(),
+  process_noise_covariance_(STATE_SIZE, STATE_SIZE),
+  initial_estimate_error_covariance_(STATE_SIZE, STATE_SIZE),
   last_diag_time_(0, 0, RCL_ROS_TIME),
   last_published_stamp_(0, 0, RCL_ROS_TIME),
   predict_to_current_time_(false),
@@ -149,6 +153,22 @@ void RosFilter<T>::reset()
 
   // reset filter to uninitialized state
   filter_.reset();
+
+  // Restore filter parameters that we got from the ROS parameter server
+  filter_.setSensorTimeout(sensor_timeout_);
+  filter_.setProcessNoiseCovariance(process_noise_covariance_);
+  filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_);
+}
+
+template<typename T>
+void RosFilter<T>::resetSrvCallback(
+  const std::shared_ptr<rmw_request_id_t>,
+  const std::shared_ptr<std_srvs::srv::Empty::Request>,
+  const std::shared_ptr<std_srvs::srv::Empty::Response>)
+{
+  RCLCPP_INFO(this->get_logger(), "Received a request to reset filter.");
+
+  reset();
 }
 
 template<typename T>
@@ -852,8 +872,9 @@ void RosFilter<T>::loadParams()
 
   predict_to_current_time_ = this->declare_parameter<bool>("predict_to_current_time", false);
 
-  double sensor_timeout = this->declare_parameter("sensor_timeout", 1.0 / frequency_);
-  filter_.setSensorTimeout(rclcpp::Duration::from_seconds(sensor_timeout));
+  sensor_timeout_ =
+    rclcpp::Duration::from_seconds(this->declare_parameter("sensor_timeout", 1.0 / frequency_));
+  filter_.setSensorTimeout(sensor_timeout_);
 
   // Determine if we're in 2D mode
   two_d_mode_ = this->declare_parameter("two_d_mode", false);
@@ -879,7 +900,7 @@ void RosFilter<T>::loadParams()
   reset_on_time_jump_ = this->declare_parameter("reset_on_time_jump", false);
 
   // Determine if we're using a control term
-  double control_timeout = sensor_timeout;
+  double control_timeout = sensor_timeout_.seconds();
   std::vector<bool> control_update_vector;
   std::vector<double> acceleration_limits;
   std::vector<double> acceleration_gains;
@@ -1049,6 +1070,13 @@ void RosFilter<T>::loadParams()
       &RosFilter::enableFilterSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
+  // Create a service for manually setting/resetting pose
+  reset_srv_ =
+    this->create_service<std_srvs::srv::Empty>(
+    "reset", std::bind(
+      &RosFilter<T>::resetSrvCallback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
   // Create a service for toggling processing new measurements while still
   // publishing
   toggle_filter_processing_srv_ =
@@ -1205,7 +1233,7 @@ void RosFilter<T>::loadParams()
         "Subscribed to " <<
           odom_topic << " (" << odom_topic_name << ")\n\t" <<
           odom_topic_name << "_differential is " <<
-        (differential ? "true" : "false") << "\n\t" << odom_topic_name <<
+          (differential ? "true" : "false") << "\n\t" << odom_topic_name <<
           "_pose_rejection_threshold is " << pose_mahalanobis_thresh <<
           "\n\t" << odom_topic_name << "_twist_rejection_threshold is " <<
           twist_mahalanobis_thresh << "\n\t" << odom_topic_name <<
@@ -1325,7 +1353,7 @@ void RosFilter<T>::loadParams()
         "Subscribed to " <<
           pose_topic << " (" << pose_topic_name << ")\n\t" <<
           pose_topic_name << "_differential is " <<
-        (differential ? "true" : "false") << "\n\t" << pose_topic_name <<
+          (differential ? "true" : "false") << "\n\t" << pose_topic_name <<
           "_rejection_threshold is " << pose_mahalanobis_thresh <<
           "\n\t" << pose_topic_name << " update vector is " <<
           pose_update_vec);
@@ -1631,14 +1659,14 @@ void RosFilter<T>::loadParams()
         "Subscribed to " <<
           imu_topic << " (" << imu_topic_name << ")\n\t" <<
           imu_topic_name << "_differential is " <<
-        (differential ? "true" : "false") << "\n\t" << imu_topic_name <<
+          (differential ? "true" : "false") << "\n\t" << imu_topic_name <<
           "_pose_rejection_threshold is " << pose_mahalanobis_thresh <<
           "\n\t" << imu_topic_name << "_twist_rejection_threshold is " <<
           twist_mahalanobis_thresh << "\n\t" << imu_topic_name <<
           "_linear_acceleration_rejection_threshold is " <<
           accel_mahalanobis_thresh << "\n\t" << imu_topic_name <<
           "_remove_gravitational_acceleration is " <<
-        (remove_grav_acc ? "true" : "false") << "\n\t" <<
+          (remove_grav_acc ? "true" : "false") << "\n\t" <<
           imu_topic_name << " pose update vector is " << pose_update_vec <<
           "\t" << imu_topic_name << " twist update vector is " <<
           twist_update_vec << "\t" << imu_topic_name <<
@@ -1726,8 +1754,7 @@ void RosFilter<T>::loadParams()
 
   // Load up the process noise covariance (from the launch file/parameter
   // server)
-  Eigen::MatrixXd process_noise_covariance(STATE_SIZE, STATE_SIZE);
-  process_noise_covariance.setZero();
+  process_noise_covariance_.setZero();
   std::vector<double> process_noise_covar_flat;
 
   this->declare_parameter("process_noise_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY);
@@ -1739,22 +1766,21 @@ void RosFilter<T>::loadParams()
 
     for (int i = 0; i < STATE_SIZE; i++) {
       for (int j = 0; j < STATE_SIZE; j++) {
-        process_noise_covariance(i, j) =
+        process_noise_covariance_(i, j) =
           process_noise_covar_flat[i * STATE_SIZE + j];
       }
     }
 
     RF_DEBUG(
       "Process noise covariance is:\n" <<
-        process_noise_covariance << "\n");
+        process_noise_covariance_ << "\n");
 
-    filter_.setProcessNoiseCovariance(process_noise_covariance);
+    filter_.setProcessNoiseCovariance(process_noise_covariance_);
   }
 
   // Load up the process noise covariance (from the launch file/parameter
   // server)
-  Eigen::MatrixXd initial_estimate_error_covariance(STATE_SIZE, STATE_SIZE);
-  initial_estimate_error_covariance.setZero();
+  initial_estimate_error_covariance_.setZero();
   std::vector<double> estimate_error_covar_flat;
 
   this->declare_parameter("initial_estimate_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY);
@@ -1766,16 +1792,16 @@ void RosFilter<T>::loadParams()
 
     for (int i = 0; i < STATE_SIZE; i++) {
       for (int j = 0; j < STATE_SIZE; j++) {
-        initial_estimate_error_covariance(i, j) =
+        initial_estimate_error_covariance_(i, j) =
           estimate_error_covar_flat[i * STATE_SIZE + j];
       }
     }
 
     RF_DEBUG(
       "Initial estimate error covariance is:\n" <<
-        estimate_error_covar_flat << "\n");
+        initial_estimate_error_covariance_ << "\n");
 
-    filter_.setEstimateErrorCovariance(initial_estimate_error_covariance);
+    filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_);
   }
 }
 
@@ -2928,9 +2954,9 @@ bool RosFilter<T>::preparePose(
       // 6b. Apply the offset (making sure to bound them), and throw them in a
       // vector
       tf2::Vector3 rpy_angles(
-        filter_utilities::clampRotation(roll - rollOffset),
-        filter_utilities::clampRotation(pitch - pitchOffset),
-        filter_utilities::clampRotation(yaw - yaw_offset));
+        angles::normalize_angle(roll - rollOffset),
+        angles::normalize_angle(pitch - pitchOffset),
+        angles::normalize_angle(yaw - yaw_offset));
 
       // 6c. Now we need to rotate the roll and pitch by the yaw offset value.
       // Imagine a case where an IMU is mounted facing sideways. In that case