Fixing angular acceleration initialisation (#945)

Author: ayrton04

include/robot_localization/ros_filter.hpp

@@ -690,7 +690,7 @@ class RosFilter : public rclcpp::Node
 
   //! @brief Covariance of the calculated angular acceleration
   //!
-  Eigen::MatrixXd angular_acceleration_cov_;
+  Eigen::Matrix3d angular_acceleration_cov_;
 
   //! @brief Stores the first measurement from each topic for relative
   //! measurements

src/ros_filter.cpp

@@ -164,13 +164,21 @@ void RosFilter<T>::reset()
   filter_state_history_.clear();
   measurement_history_.clear();
 
+  angular_acceleration_.setZero();
+  angular_acceleration_cov_.setIdentity();
+  angular_acceleration_cov_ *= 0.01;
+
+  last_state_twist_rot_.setZero();
+
   // Also set the last set pose time, so we ignore all messages
   // that occur before it
   last_set_pose_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
   last_diag_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
   latest_control_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
   last_published_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
+  last_diff_time_ = this->now().seconds();
+
   // clear tf buffer to avoid TF_OLD_DATA errors
   tf_buffer_->clear();
 
@@ -731,6 +739,10 @@ void RosFilter<T>::integrateMeasurements(const rclcpp::Time & current_time)
         restored_measurement_count--;
       }
 
+      auto previous_state = filter_.getState();
+      auto previous_covar = filter_.getEstimateErrorCovariance();
+      auto last_measurement_time = filter_.getLastMeasurementTime();
+
       // This will call predict and, if necessary, correct
       filter_.processMeasurement(*(measurement.get()));
 
@@ -1777,9 +1789,6 @@ void RosFilter<T>::loadParams()
     }
   } while (more_params);
 
-  angular_acceleration_cov_.resize(ORIENTATION_SIZE, ORIENTATION_SIZE);
-  angular_acceleration_cov_.setZero();
-
   // Now that we've checked if IMU linear acceleration is being used, we can
   // determine our final control parameters
   if (use_control_ && std::accumulate(
@@ -2073,6 +2082,12 @@ void RosFilter<T>::initialize()
     this->get_clock()->wait_until_started();
   }
 
+  angular_acceleration_.setZero();
+  angular_acceleration_cov_.setIdentity();
+  angular_acceleration_cov_ *= 1e-6;
+
+  last_state_twist_rot_.setZero();
+
   diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(
     shared_from_this());
   diagnostic_updater_->setHardwareID("none");
@@ -2100,6 +2115,7 @@ void RosFilter<T>::initialize()
       &max_frequency_, 0.1, 10));
 
   last_diag_time_ = this->now();
+  last_diff_time_ = this->now().seconds();
 
   // Clear out the transforms
   world_base_link_trans_msg_.transform =