eskf with sensor concept, less states, and latest fixes from dev/auv-navigation-eskf

.github/workflows/ros-node-tests.yml

@@ -10,6 +10,11 @@ on:
     - cron: '0 1 * * *' # Runs daily at 01:00 UTC
 jobs:
   call_reusable_workflow:
+    strategy:
+      matrix:
+        test_script:
+          - "tests/ros_node_tests/dp_node_test.sh"
+          - "tests/ros_node_tests/eskf_node_test.sh"
     uses: vortexntnu/vortex-ci/.github/workflows/reusable-ros2-simulator-test.yml@main
     with:
       vcs_repos_file: "tests/dependencies.repos"

navigation/eskf/CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 3.8)
+project(eskf)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
+
+if(NOT DEFINED EIGEN3_INCLUDE_DIR)
+  set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS})
+endif()
+
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/eskf.cpp
+  src/eskf_ros.cpp
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+  spdlog
+  fmt
+)
+
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "ESKFNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

navigation/eskf/config/eskf_params.yaml

@@ -0,0 +1,8 @@
+eskf_node:
+  ros__parameters:
+    imu_topic: /imu/data_raw
+    dvl_topic: /orca/twist
+    odom_topic: odom_ESKF
+    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]
+    imu_frame: [-1.0, 0.0, 0.0, -1.0, 0.0,  0.0, -1.0,  0.0,  0.0]

navigation/eskf/include/eskf/eskf.hpp

@@ -0,0 +1,106 @@
+#ifndef ESKF__ESKF_HPP_
+#define ESKF__ESKF_HPP_
+
+#include <eigen3/Eigen/Dense>
+#include <utility>
+#include "eskf/typedefs.hpp"
+#include "typedefs.hpp"
+
+class ESKF {
+   public:
+    explicit ESKF(const EskfParams& params);
+
+    // @brief Update the nominal state and error state
+    // @param imu_meas: IMU measurement
+    // @param dt: Time step
+    void imu_update(const ImuMeasurement& imu_meas, const double dt);
+
+    // @brief Update the nominal state and error state
+    // @param dvl_meas: DVL measurement
+    void dvl_update(const SensorDVL& dvl_meas);
+
+    inline StateQuat get_nominal_state() const { return current_nom_state_; }
+
+    inline StateEuler get_error_state() const { return current_error_state_; }
+
+    inline double get_nis() const { return nis_; }
+
+   private:
+    // @brief Predict the nominal state
+    // @param imu_meas: IMU measurement
+    // @param dt: Time step
+    // @return Predicted nominal state
+    void nominal_state_discrete(const ImuMeasurement& imu_meas,
+                                const double dt);
+
+    // @brief Predict the error state
+    // @param imu_meas: IMU measurement
+    // @param dt: Time step
+    // @return Predicted error state
+    void error_state_prediction(const ImuMeasurement& imu_meas,
+                                const double dt);
+
+    // @brief Calculate the NIS
+    // @param innovation: Innovation vector
+    // @param S: Innovation covariance matrix
+    void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S);
+
+    // @brief Update the error state using a generic sensor measurement model
+    // @tparam SensorT Type of the sensor model (must satisfy
+    // SensorModelConcept)
+    // @param meas Sensor measurement instance
+    template <SensorModelConcept SensorT>
+    void measurement_update(const SensorT& meas);
+
+    // @brief Inject the error state into the nominal state and reset the error
+    void injection_and_reset();
+
+    // @brief Van Loan discretization
+    // @param A_c: Continuous state transition matrix
+    // @param G_c: Continuous input matrix
+    // @return Discrete state transition matrix and discrete input matrix
+    std::pair<Eigen::Matrix15d, Eigen::Matrix15d> van_loan_discretization(
+        const Eigen::Matrix15d& A_c,
+        const Eigen::Matrix15x12d& G_c,
+        const double dt);
+
+    // Process noise covariance matrix
+    Eigen::Matrix12d Q_{};
+
+    // Normalized Innovation Squared
+    double nis_{};
+
+    // Member variable for the current error state
+    StateEuler current_error_state_{};
+
+    // Member variable for the current nominal state
+    StateQuat current_nom_state_{};
+
+    // gravity
+    Eigen::Vector3d g_{0.0, 0.0, 9.82};
+
+    // accelometer noise parameters
+    float accm_std_{0.0};
+    float accm_bias_std_{0.0};
+    float accm_bias_p_{1e-16};
+
+    // gyroscope noise parameters
+    float gyro_std_{0.0};
+    float gyro_bias_std_{0.0};
+    float gyro_bias_p_{1e-16};
+};
+
+// Measurement in world frame --> h(x)
+Eigen::Vector3d calculate_h(const StateQuat& current_nom_state_);
+
+// Jacobian of h(x) with respect to the error state --> H
+Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_);
+
+// Jacobian of h(x) with respect to the nominal state --> Hx
+Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_);
+
+double compute_nis(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S);
+
+#include "eskf.tpp"  // including template implementation
+
+#endif  // ESKF__ESKF_HPP_

navigation/eskf/include/eskf/eskf.tpp

@@ -0,0 +1,25 @@
+
+
+template <SensorModelConcept SensorT>
+void ESKF::measurement_update(const SensorT& meas) {
+    Eigen::VectorXd innovation = meas.innovation(current_nom_state_);
+    Eigen::MatrixXd H = meas.jacobian(current_nom_state_);
+    Eigen::MatrixXd R = meas.noise_covariance();
+
+    Eigen::MatrixXd P = current_error_state_.covariance;
+
+    Eigen::MatrixXd S = H * P * H.transpose() + R;
+    Eigen::MatrixXd K = P * H.transpose() * S.inverse();
+
+#ifndef NDEBUG
+    nis_ = compute_nis(innovation, S);
+#endif
+
+    current_error_state_.set_from_vector(K * innovation);
+
+    Eigen::MatrixXd I_KH =
+        Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * H;
+    current_error_state_.covariance =
+        I_KH * P * I_KH.transpose() +
+        K * R * K.transpose();  // Used joseph form for more stable calculations
+}

navigation/eskf/include/eskf/eskf_ros.hpp

@@ -0,0 +1,67 @@
+#ifndef ESKF__ESKF_ROS_HPP_
+#define ESKF__ESKF_ROS_HPP_
+
+#include <chrono>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <memory>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "eskf/eskf.hpp"
+#include "eskf/typedefs.hpp"
+#include "spdlog/spdlog.h"
+
+class ESKFNode : public rclcpp::Node {
+   public:
+    explicit ESKFNode(
+        const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+   private:
+    // @brief Callback function for the imu topic
+    // @param msg: Imu message containing the imu data
+    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+
+    // @brief Callback function for the dvl topic
+    // @param msg: TwistWithCovarianceStamped message containing the dvl data
+    void dvl_callback(
+        const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+
+    // @brief Publish the odometry message
+    void publish_odom();
+
+    // @brief Set the subscriber and publisher for the node
+    void set_subscribers_and_publisher();
+
+    // @brief Set the parameters for the eskf
+    void set_parameters();
+
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+    rclcpp::Subscription<
+        geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_;
+
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+
+    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cov_pub_;
+
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr nis_pub_;
+
+    std::chrono::milliseconds time_step;
+
+    rclcpp::TimerBase::SharedPtr odom_pub_timer_;
+
+    std::unique_ptr<ESKF> eskf_;
+
+    bool first_imu_msg_received_ = false;
+
+    Eigen::Matrix3d R_imu_eskf_{};
+
+    rclcpp::Time last_imu_time_{};
+};
+
+#endif  // ESKF__ESKF_ROS_HPP_