added changes to account for imus that cancel gravity, added a yaml for real world with some calib for imu bias

Author: AhmedBorch

navigation/eskf/config/eskf_params.yaml

@@ -20,4 +20,8 @@
       publish_tf: true
       publish_pose: true
       publish_twist: true
-      publish_rate_ms: 1
+      publish_rate_ms: 5
+      add_gravity_to_imu: false
+      initial_gyro_bias: [0.0, 0.0, 0.0]
+      initial_accel_bias: [0.0, 0.0, 0.0]
+      publish_biases: false

navigation/eskf/config/eskf_params_real_world.yaml

@@ -0,0 +1,31 @@
+/**:
+  eskf_node:
+    ros__parameters:
+      diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001]
+      diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+      transform:
+        imu_frame_r: [ 1.0,  0.0,  0.0,
+                    0.0, 1.0,  0.0,
+                    0.0,  0.0, 1.0 ]
+        imu_frame_t: [ 0.0, 0.0, 0.0 ]
+
+        dvl_frame_r: [ 0.0, -1.0,  0.0,
+                        1.0,  0.0,  0.0,
+                        0.0,  0.0,  1.0 ]
+
+        dvl_frame_t: [ 0.4, 0.0, 0.2 ]
+
+        depth_frame_t: [ 0.0, 0.0, 0.0 ]
+      use_tf_transforms: false
+      publish_tf: true
+      publish_pose: true
+      publish_twist: true
+      publish_rate_ms: 5
+      add_gravity_to_imu: true
+      initial_gyro_bias: [0.0, 0.0, 0.0]
+      initial_accel_bias: [3.53e-2, 0.0, -0.0533]
+      publish_biases: true
+
+# USED SENSORS
+  # IMU: Kongsberg mini MRU
+  # DVL & Depth: Nucleus 1000

navigation/eskf/include/eskf/eskf.hpp

@@ -27,6 +27,8 @@ class ESKF {
 
     inline double get_nis() const { return nis_; }
 
+    inline Eigen::Vector3d get_gravity() const { return g_; }
+
    private:
     // @brief Predict the nominal state
     // @param imu_meas: IMU measurement

navigation/eskf/include/eskf/eskf_ros.hpp

@@ -8,6 +8,7 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <memory>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -81,6 +82,12 @@ class ESKFNode : public rclcpp::Node {
 
     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr nis_depth_pub_;
 
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr
+        accel_bias_pub_;
+
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr
+        gyro_bias_pub_;
+
     // Member variable for the ESKF instance
 
     std::chrono::milliseconds time_step_{1};
@@ -121,6 +128,8 @@ class ESKFNode : public rclcpp::Node {
     bool publish_tf_{false};
     bool publish_pose_{false};
     bool publish_twist_{false};
+    bool publish_biases_{false};
+    bool add_gravity_to_imu_{false};
 
     // hold the transfer from Sensor -> Base Link
     Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity();

navigation/eskf/include/eskf/typedefs.hpp

@@ -99,6 +99,8 @@ struct EskfParams {
     Eigen::Matrix12d Q = Eigen::Matrix12d::Zero();
     Eigen::Matrix15d P = Eigen::Matrix15d::Zero();
     Eigen::Vector3d g_{0.0, 0.0, -9.81};
+    Eigen::Vector3d initial_gyro_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero();
 };
 struct ImuMeasurement {
     Eigen::Vector3d accel = Eigen::Vector3d::Zero();

navigation/eskf/launch/eskf.launch.py

@@ -1,19 +1,19 @@
 import os
-from os import path
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 from auv_setup.launch_arg_common import (
     declare_drone_and_namespace_args,
     resolve_drone_and_namespace,
 )
 
-eskf_params = path.join(
-    get_package_share_directory("eskf"), "config", "eskf_params.yaml"
-)
+# eskf_params = path.join(
+#     get_package_share_directory("eskf"), "config", "eskf_params_real_world.yaml"
+# )
 
 
 def launch_setup(context, *args, **kwargs):
@@ -25,6 +25,18 @@ def launch_setup(context, *args, **kwargs):
         "robots",
         f"{drone}.yaml",
     )
+
+    use_sim = LaunchConfiguration('use_sim').perform(context).lower() == 'true'
+
+    if use_sim:
+        param_file_name = "eskf_params.yaml"
+    else:
+        param_file_name = "eskf_params_real_world.yaml"
+
+    eskf_params = os.path.join(
+        get_package_share_directory("eskf"), "config", param_file_name
+    )
+
     drone_env_params = os.path.join(
         get_package_share_directory("auv_setup"),
         "config",
@@ -49,7 +61,13 @@ def launch_setup(context, *args, **kwargs):
 
 
 def generate_launch_description():
-    # This function defines WHAT to do, but doesn't execute the logic yet
+    sim_arg = DeclareLaunchArgument(
+        'use_sim',
+        default_value='false',
+        description='Set to "false" to load real-world hardware parameters.',
+    )
     return LaunchDescription(
-        declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)]
+        [sim_arg]
+        + declare_drone_and_namespace_args()
+        + [OpaqueFunction(function=launch_setup)]
     )

navigation/eskf/src/eskf.cpp

@@ -19,6 +19,9 @@ ESKF::ESKF(const EskfParams& params) : Q_(params.Q) {
 
     // Initialize Nominal Quaternion to Identity
     current_nom_state_.quat = Eigen::Quaterniond::Identity();
+    // Initialize nominal bias values
+    current_nom_state_.gyro_bias = params.initial_gyro_bias;
+    current_nom_state_.accel_bias = params.initial_accel_bias;
 }
 
 std::pair<Eigen::Matrix15d, Eigen::Matrix15d> ESKF::van_loan_discretization(
@@ -205,7 +208,7 @@ Eigen::VectorXd SensorDepth::innovation(const StateQuat& state) const {
     return innovation;
 }
 
-Eigen::MatrixXd SensorDepth::jacobian(const StateQuat& state) const {
+Eigen::MatrixXd SensorDepth::jacobian(const StateQuat& /*state*/) const {
     Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 15);
     H(0, 2) = 1.0;
     return H;

navigation/eskf/src/eskf_ros.cpp

@@ -22,6 +22,7 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options)
     if (!frame_prefix_.empty() && frame_prefix_.back() == '/') {
         frame_prefix_.pop_back();
     }
+    spdlog::info("frame_prefix set to '{}'", frame_prefix_);
 
     publish_tf_ = this->declare_parameter<bool>("publish_tf");
     if (publish_tf_) {
@@ -32,6 +33,8 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options)
     publish_pose_ = this->declare_parameter<bool>("publish_pose");
     publish_twist_ = this->declare_parameter<bool>("publish_twist");
 
+    publish_biases_ = this->declare_parameter<bool>("publish_biases");
+
     // Declare these here so they appear in `ros2 param list` from startup,
     // even though they are read in complete_initialization().
     this->declare_parameter<int>("publish_rate_ms");
@@ -95,6 +98,15 @@ void ESKFNode::set_subscribers_and_publisher() {
                                                             qos_sensor_data);
     }
 
+    if (publish_biases_) {
+        accel_bias_pub_ =
+            this->create_publisher<geometry_msgs::msg::Vector3Stamped>(
+                "eskf/accel_bias", qos_sensor_data);
+        gyro_bias_pub_ =
+            this->create_publisher<geometry_msgs::msg::Vector3Stamped>(
+                "eskf/gyro_bias", qos_sensor_data);
+    }
+
 #ifndef NDEBUG
     nis_dvl_pub_ = create_publisher<std_msgs::msg::Float64>(
         "eskf/nis_dvl", vortex::utils::qos_profiles::reliable_profile());
@@ -111,6 +123,11 @@ void ESKFNode::set_parameters() {
         R_imu_eskf_ = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
             R_imu_correction.data());
 
+        std::vector<double> T_imu_correction =
+            this->declare_parameter<std::vector<double>>(
+                "transform.imu_frame_t");
+        T_imu_eskf_ = Eigen::Map<Eigen::Vector3d>(T_imu_correction.data());
+
         std::vector<double> R_dvl_correction =
             this->declare_parameter<std::vector<double>>(
                 "transform.dvl_frame_r");
@@ -152,9 +169,37 @@ void ESKFNode::set_parameters() {
 
     Eigen::Vector3d g_vec(0.0, 0.0, this->gravity);
 
-    EskfParams eskf_params{.Q = Q, .P = P, .g_ = g_vec};
+    std::vector<double> initial_gyro_bias =
+        this->declare_parameter<std::vector<double>>(
+            "initial_gyro_bias", std::vector<double>{0.0, 0.0, 0.0});
+    spdlog::info("initial_gyro_bias: [{}, {}, {}]", initial_gyro_bias[0],
+                 initial_gyro_bias[1], initial_gyro_bias[2]);
+    if (initial_gyro_bias.size() != 3) {
+        throw std::runtime_error("initial_gyro_bias must have length 3");
+    }
+
+    std::vector<double> initial_accel_bias =
+        this->declare_parameter<std::vector<double>>(
+            "initial_accel_bias", std::vector<double>{0.0, 0.0, 0.0});
+    spdlog::info("initial_accel_bias: [{}, {}, {}]", initial_accel_bias[0],
+                 initial_accel_bias[1], initial_accel_bias[2]);
+    if (initial_accel_bias.size() != 3) {
+        throw std::runtime_error("initial_accel_bias must have length 3");
+    }
+
+    EskfParams eskf_params{
+        .Q = Q,
+        .P = P,
+        .g_ = g_vec,
+        .initial_gyro_bias =
+            Eigen::Map<Eigen::Vector3d>(initial_gyro_bias.data()),
+        .initial_accel_bias =
+            Eigen::Map<Eigen::Vector3d>(initial_accel_bias.data())};
 
     eskf_ = std::make_unique<ESKF>(eskf_params);
+
+    add_gravity_to_imu_ = this->declare_parameter<bool>("add_gravity_to_imu");
+    spdlog::info("add_gravity_to_imu: {}", add_gravity_to_imu_);
 }
 
 void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
@@ -192,7 +237,14 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
 
     // a_corrected = a_meas - omega x (omega x T)
     Eigen::Vector3d centripetal_accel = omega.cross(omega.cross(T_imu_eskf_));
-    imu_measurement.accel = accel_aligned - centripetal_accel;
+    accel_aligned -= centripetal_accel;
+
+    if (add_gravity_to_imu_) {
+        Eigen::Matrix3d R = nom_state.quat.normalized().toRotationMatrix();
+        accel_aligned -= R.transpose() * eskf_->get_gravity();
+    }
+
+    imu_measurement.accel = accel_aligned;
 
     // save latest gyro readings (used for DVL correction and odom output)
     latest_gyro_measurement_ = imu_measurement.gyro;
@@ -334,13 +386,36 @@ void ESKFNode::publish_odom() {
     if (publish_tf_) {
         publish_tf(nom_state, current_time);
     }
+
+    if (publish_biases_) {
+        geometry_msgs::msg::Vector3Stamped accel_bias_msg;
+        accel_bias_msg.header.stamp = current_time;
+        accel_bias_msg.header.frame_id =
+            frame("base_link");  // Biases are in the body frame
+
+        accel_bias_msg.vector.x = nom_state.accel_bias.x();
+        accel_bias_msg.vector.y = nom_state.accel_bias.y();
+        accel_bias_msg.vector.z = nom_state.accel_bias.z();
+
+        accel_bias_pub_->publish(accel_bias_msg);
+
+        geometry_msgs::msg::Vector3Stamped gyro_bias_msg;
+        gyro_bias_msg.header = accel_bias_msg.header;
+
+        gyro_bias_msg.vector.x = nom_state.gyro_bias.x();
+        gyro_bias_msg.vector.y = nom_state.gyro_bias.y();
+        gyro_bias_msg.vector.z = nom_state.gyro_bias.z();
+
+        gyro_bias_pub_->publish(gyro_bias_msg);
+    }
 }
 
 void ESKFNode::lookup_static_transforms() {
     try {
         Tf_base_imu_ = tf2::transformToEigen(tf_buffer_->lookupTransform(
             frame("base_link"), frame("imu_link"), tf2::TimePointZero));
         R_imu_eskf_ = Tf_base_imu_.rotation();
+        T_imu_eskf_ = Tf_base_imu_.translation();
 
         Tf_base_dvl_ = tf2::transformToEigen(tf_buffer_->lookupTransform(
             frame("base_link"), frame("dvl_link"), tf2::TimePointZero));