Error state kalman filter

navigation/eskf/CMakeLists.txt

@@ -0,0 +1,63 @@
+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(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)
+find_package(stonefish_ros2 REQUIRED)
+
+if(NOT DEFINED EIGEN3_INCLUDE_DIR)
+  set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS})
+endif()
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(include)
+
+add_executable(eskf_node
+  src/eskf.cpp
+  src/eskf_ros.cpp
+  src/eskf_node.cpp
+  src/eskf_utils.cpp
+)
+
+ament_target_dependencies(eskf_node
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+  spdlog
+  fmt
+  stonefish_ros2
+)
+
+target_link_libraries(eskf_node
+  fmt::fmt
+)
+
+install(TARGETS
+  eskf_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY
+  config
+  launch
+  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: /dvl/sim
+    odom_topic: odom
+    diag_Q_std: [0.05, 0.05, 0.05, 0.00001, 0.00001, 0.00001, 0.000001, 0.000000001, 0.000000001, 0.000000001, 0.00000001, 0.00000001]
+    diag_p_init: [1.0, 1.0, 0.5, 0.5, 0.5, 1.0, 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,104 @@
+#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);
+
+    // NIS
+    double NIS_;
+
+    // NEEDS
+    double NEES_;
+
+    // ground truth
+    state_quat ground_truth_;
+
+   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);
+
+    void NEEDS();
+
+    // @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 delta quaternion matrix
+    // @param nom_state: Nominal state
+    // @return Delta quaternion matrix
+    Eigen::Matrix4x3d calculate_q_delta();
+
+    // @brief Calculate the measurement matrix jakobian
+    // @param nom_state: Nominal state
+    // @return Measurement matrix
+    Eigen::Matrix3x19d calculate_hx();
+
+    // @brief Calculate the full measurement matrix
+    // @param nom_state: Nominal state
+    // @return Measurement matrix
+    Eigen::Matrix3x18d calculate_h_jacobian();
+
+    // @brief Calculate the measurement
+    // @param nom_state: Nominal state
+    // @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,88 @@
+#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 <stonefish_ros2/msg/dvl.hpp>
+#include "eskf/eskf.hpp"
+#include "eskf/typedefs.hpp"
+#include "spdlog/spdlog.h"
+
+class ESKFNode : public rclcpp::Node {
+   public:
+    explicit ESKFNode();
+
+   private:
+    void pose_callback(
+        const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+    void twist_callback(
+        const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+
+    // @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 stonefish_ros2::msg::DVL::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<stonefish_ros2::msg::DVL>::SharedPtr dvl_sub_;
+
+    rclcpp::Subscription<
+        geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+
+    rclcpp::Subscription<
+        geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr nis_pub_;
+
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr nees_pub_;
+
+    std::chrono::milliseconds time_step;
+
+    rclcpp::TimerBase::SharedPtr odom_pub_timer_;
+
+    state_quat nom_state_;
+
+    state_quat g_truth_;
+
+    state_euler error_state_;
+
+    imu_measurement imu_meas_;
+
+    dvl_measurement dvl_meas_;
+
+    eskf_params eskf_params_;
+
+    std::unique_ptr<ESKF> eskf_;
+
+    rclcpp::Time last_imu_time_;
+
+    bool first_imu_msg_received_ = false;
+
+    Eigen::Matrix3d R_imu_eskf_;
+};
+
+#endif  // ESKF_ROS_HPP

navigation/eskf/include/eskf/eskf_utils.hpp

@@ -0,0 +1,18 @@
+#ifndef ESKF_UTILS_HPP
+#define ESKF_UTILS_HPP
+
+#include <cmath>
+#include "eigen3/Eigen/Dense"
+#include "eskf/typedefs.hpp"
+
+Eigen::Matrix3d skew(const Eigen::Vector3d& v);
+
+double sq(const double& value);
+
+double ssa(const double& angle);
+
+Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector);
+
+Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler);
+
+#endif  // ESKF_UTILS_HPP