Error-state Kalman Filter implementation

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

@@ -14,6 +14,7 @@ jobs:
       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,77 @@
+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()
+
+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(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
+  src/eskf_utils.cpp
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+  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
+    diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145]
+    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, 0.001, 0.001, 0.001]
+    imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0,  0.0, -1.0,  0.0,  0.0]

navigation/eskf/include/eskf/eskf.hpp

@@ -0,0 +1,88 @@
+#ifndef ESKF_HPP
+#define ESKF_HPP
+
+#include <eigen3/Eigen/Dense>
+#include <utility>
+#include "eskf/typedefs.hpp"
+#include "typedefs.hpp"
+
+class ESKF {
+   public:
+    ESKF(const eskf_params& params);
+
+    // @brief Update the nominal state and error state
+    // @param imu_meas: IMU measurement
+    // @param dt: Time step
+    // @return Updated nominal state and error state
+    std::pair<state_quat, state_euler> imu_update(
+        const imu_measurement& imu_meas,
+        const double dt);
+
+    // @brief Update the nominal state and error state
+    // @param dvl_meas: DVL measurement
+    // @return Updated nominal state and error state
+    std::pair<state_quat, state_euler> dvl_update(
+        const dvl_measurement& dvl_meas);
+
+    // Normalized Innovation Squared
+    double 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 imu_measurement& 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 imu_measurement& 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
+    // @param dvl_meas: DVL measurement
+    void measurement_update(const dvl_measurement& dvl_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::Matrix18d, Eigen::Matrix18d> van_loan_discretization(
+        const Eigen::Matrix18d& A_c,
+        const Eigen::Matrix18x12d& G_c,
+        const double dt);
+
+    // @brief Calculate the measurement matrix jakobian
+    // @return Measurement matrix
+    Eigen::Matrix3x19d calculate_hx();
+
+    // @brief Calculate the full measurement matrix
+    // @return Measurement matrix
+    Eigen::Matrix3x18d calculate_h_jacobian();
+
+    // @brief Calculate the measurement
+    // @return Measurement
+    Eigen::Vector3d calculate_h();
+
+    // Process noise covariance matrix
+    Eigen::Matrix12d Q_{};
+
+    // Member variable for the current error state
+    state_euler current_error_state_{};
+
+    // Member variable for the current nominal state
+    state_quat current_nom_state_{};
+};
+
+#endif  // ESKF_HPP

navigation/eskf/include/eskf/eskf_ros.hpp

@@ -0,0 +1,74 @@
+#ifndef ESKF_ROS_HPP
+#define ESKF_ROS_HPP
+
+#include <chrono>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#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::Float64>::SharedPtr nis_pub_;
+
+    std::chrono::milliseconds time_step;
+
+    rclcpp::TimerBase::SharedPtr odom_pub_timer_;
+
+    state_quat nom_state_{};
+
+    state_euler error_state_{};
+
+    imu_measurement imu_meas_{};
+
+    dvl_measurement dvl_meas_{};
+
+    eskf_params eskf_params_{};
+
+    std::unique_ptr<ESKF> eskf_;
+
+    bool first_imu_msg_received_ = false;
+
+    Eigen::Matrix3d R_imu_eskf_{};
+
+    rclcpp::Time last_imu_time_{};
+};
+
+#endif  // ESKF_ROS_HPP

navigation/eskf/include/eskf/eskf_utils.hpp

@@ -0,0 +1,38 @@
+#ifndef ESKF_UTILS_HPP
+#define ESKF_UTILS_HPP
+
+#include <cmath>
+#include "eigen3/Eigen/Dense"
+#include "eskf/typedefs.hpp"
+
+// @brief Compute the skew-symmetric matrix of a vector
+// @param v: Input vector
+// @return Skew-symmetric matrix
+Eigen::Matrix3d skew(const Eigen::Vector3d& v);
+
+// @brief Square a value
+// @param value: Input value
+// @return Squared value
+double sq(double value);
+
+// @brief Normalize an angle to the range [-pi, pi]
+// @param angle: Input angle in radians
+// @return Normalized angle in radians
+double ssa(double angle);
+
+// @brief Calculate the transformation matrix using a quaternion
+// @param quat: Input quaternion
+// @return Transformation matrix
+Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat);
+
+// @brief Convert a rotation vector to a quaternion
+// @param vector: Input rotation vector
+// @return Corresponding quaternion
+Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector);
+
+// @brief Convert Euler angles to a quaternion
+// @param euler: Input Euler angles (roll, pitch, yaw) in radians
+// @return Corresponding quaternion
+Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler);
+
+#endif  // ESKF_UTILS_HPP