Error-state Kalman Filter implementation

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: /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,96 @@
+#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_;
+
+   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 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,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 <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:
+    // @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,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

navigation/eskf/include/eskf/typedefs.hpp

@@ -0,0 +1,126 @@
+/**
+ * @file typedefs.hpp
+ * @brief Contains the typedef and structs for the eskf.
+ */
+#ifndef ESKF_TYPEDEFS_H
+#define ESKF_TYPEDEFS_H
+
+#include <eigen3/Eigen/src/Core/Matrix.h>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+namespace Eigen {
+typedef Eigen::Matrix<double, 19, 1> Vector19d;
+typedef Eigen::Matrix<double, 18, 1> Vector18d;
+typedef Eigen::Matrix<double, 18, 18> Matrix18d;
+typedef Eigen::Matrix<double, 19, 19> Matrix19d;
+typedef Eigen::Matrix<double, 18, 12> Matrix18x12d;
+typedef Eigen::Matrix<double, 4, 3> Matrix4x3d;
+typedef Eigen::Matrix<double, 3, 19> Matrix3x19d;
+typedef Eigen::Matrix<double, 3, 18> Matrix3x18d;
+typedef Eigen::Matrix<double, 12, 12> Matrix12d;
+typedef Eigen::Matrix<double, 18, 18> Matrix18d;
+typedef Eigen::Matrix<double, 3, 1> Matrix3x1d;
+typedef Eigen::Matrix<double, 19, 18> Matrix19x18d;
+typedef Eigen::Matrix<double, 18, 3> Matrix18x3d;
+typedef Eigen::Matrix<double, 36, 36> Matrix36d;
+typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+typedef Eigen::Matrix<double, 9, 9> Matrix9d;
+typedef Eigen::Matrix<double, 15, 15> Matrix15d;
+typedef Eigen::Matrix<double, 15, 1> Vector15d;
+}  // namespace Eigen
+
+template <int N>
+Eigen::Matrix<double, N, N> createDiagonalMatrix(
+    const std::vector<double>& diag) {
+    return Eigen::Map<const Eigen::Matrix<double, N, 1>>(diag.data())
+        .asDiagonal();
+}
+
+struct state_quat {
+    Eigen::Vector3d pos = Eigen::Vector3d(5.58, 0.66, 0.12);
+    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond quat = Eigen::Quaterniond(0.98, -0.047, 0.028, -0.18);
+    Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gravity = Eigen::Vector3d::Zero();
+
+    state_quat() { gravity << 0, 0, 9.81; }
+
+    Eigen::Vector19d as_vector() const {
+        Eigen::Vector19d vec;
+        vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias,
+            accel_bias, gravity;
+        return vec;
+    }
+
+    Eigen::Vector18d nees_error(const state_quat& other) const {
+        Eigen::Vector18d vec;
+        Eigen::Vector3d euler_diff;
+
+        euler_diff = (quat * other.quat.inverse())
+                         .toRotationMatrix()
+                         .eulerAngles(0, 1, 2) +
+                     Eigen::Vector3d(-M_PI, M_PI, -M_PI);
+
+        vec << pos - other.pos, vel - other.vel, euler_diff,
+            gyro_bias - other.gyro_bias, accel_bias - other.accel_bias,
+            gravity - other.gravity;
+        return vec;
+    }
+
+    state_quat operator-(const state_quat& other) const {
+        state_quat diff;
+        diff.pos = pos - other.pos;
+        diff.vel = vel - other.vel;
+        diff.quat = quat * other.quat.inverse();
+        diff.gyro_bias = gyro_bias - other.gyro_bias;
+        diff.accel_bias = accel_bias - other.accel_bias;
+        diff.gravity = gravity - other.gravity;
+        return diff;
+    }
+};
+
+struct state_euler {
+    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
+    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
+    Eigen::Vector3d euler = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gravity = Eigen::Vector3d::Zero();
+
+    Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero();
+
+    Eigen::Vector18d as_vector() const {
+        Eigen::Vector18d vec;
+        vec << pos, vel, euler, gyro_bias, accel_bias, gravity;
+        return vec;
+    }
+
+    void set_from_vector(const Eigen::Vector18d& vec) {
+        pos = vec.block<3, 1>(0, 0);
+        vel = vec.block<3, 1>(3, 0);
+        euler = vec.block<3, 1>(6, 0);
+        gyro_bias = vec.block<3, 1>(9, 0);
+        accel_bias = vec.block<3, 1>(12, 0);
+        gravity = vec.block<3, 1>(15, 0);
+    }
+};
+
+struct imu_measurement {
+    Eigen::Vector3d accel = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gyro = Eigen::Vector3d::Zero();
+};
+
+struct dvl_measurement {
+    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
+    Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
+};
+
+struct eskf_params {
+    double temp = 0.0;
+    Eigen::Matrix12d Q = Eigen::Matrix12d::Zero();
+    double dt = 0.0;
+};
+
+#endif  // ESKF_TYPEDEFS_H