Error-state Kalman Filter implementation

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

@@ -16,6 +16,7 @@ jobs:
       setup_script: "tests/setup.sh"
       test_scripts: '[
         "tests/ros_node_tests/dp_node_test.sh",
+        "tests/ros_node_tests/eskf_node_test.sh",
         "tests/ros_node_tests/lqr_autopilot_node_test.sh",
         "tests/ros_node_tests/reference_filter_node_test.sh",
         "tests/ros_node_tests/los_node_test.sh",

navigation/eskf/CMakeLists.txt

@@ -0,0 +1,78 @@
+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(vortex_utils 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
+  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,87 @@
+#ifndef ESKF_HPP
+#define ESKF_HPP
+
+#include <eigen3/Eigen/Dense>
+#include <utility>
+#include "eskf/typedefs.hpp"
+#include "typedefs.hpp"
+
+class ESKF {
+   public:
+    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 DvlMeasurement& dvl_meas);
+
+    inline StateQuat get_nominal_state() const { return current_nom_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
+    // @param dvl_meas: DVL measurement
+    void measurement_update(const DvlMeasurement& 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_{};
+
+    // 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_{};
+};
+
+#endif  // ESKF_HPP

navigation/eskf/include/eskf/eskf_ros.hpp

@@ -0,0 +1,64 @@
+#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_;
+
+    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/typedefs.hpp

@@ -0,0 +1,125 @@
+/**
+ * @file typedefs.hpp
+ * @brief Contains the typedef and structs for the eskf.
+ */
+#ifndef ESKF_TYPEDEFS_H
+#define ESKF_TYPEDEFS_H
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <vector>
+
+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;
+typedef Eigen::Matrix<double, 12, 1> Vector12d;
+}  // 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 StateQuat {
+    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
+    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond quat = Eigen::Quaterniond::Identity();
+    Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81);
+
+    StateQuat() = default;
+
+    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 StateQuat& 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;
+    }
+
+    StateQuat operator-(const StateQuat& other) const {
+        StateQuat 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 StateEuler {
+    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(0, 0, 9.81);
+
+    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 ImuMeasurement {
+    Eigen::Vector3d accel = Eigen::Vector3d::Zero();
+    Eigen::Vector3d gyro = Eigen::Vector3d::Zero();
+};
+
+struct DvlMeasurement {
+    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
+    Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
+};
+
+struct EskfParams {
+    Eigen::Matrix12d Q = Eigen::Matrix12d::Zero();
+    Eigen::Matrix18d P = Eigen::Matrix18d::Zero();
+};
+
+#endif  // ESKF_TYPEDEFS_H

navigation/eskf/launch/eskf.launch.py

@@ -0,0 +1,23 @@
+from os import path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+eskf_params = path.join(
+    get_package_share_directory("eskf"), "config", "eskf_params.yaml"
+)
+
+
+def generate_launch_description():
+    eskf_node = Node(
+        package="eskf",
+        executable="eskf_node",
+        name="eskf_node",
+        # namespace="orca",
+        parameters=[
+            eskf_params,
+        ],
+        output="screen",
+    )
+    return LaunchDescription([eskf_node])