diff --git a/.github/workflows/ros-node-tests.yml b/.github/workflows/ros-node-tests.yml index c85fbe1f7..8ff052ded 100644 --- a/.github/workflows/ros-node-tests.yml +++ b/.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" diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 000000000..ec4feea05 --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,81 @@ +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(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() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml new file mode 100644 index 000000000..2293ae1e1 --- /dev/null +++ b/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.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] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..1f0ba2ec2 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,103 @@ +#ifndef ESKF_HPP +#define ESKF_HPP + +#include +#include +#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 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 dvl_update( + const sensor_dvl& 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 using a generic sensor measurement model + // @tparam SensorT Type of the sensor model (must satisfy SensorModelConcept) + // @param meas Sensor measurement instance + template + 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 van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, + const double dt); + + + // 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_{}; + + // 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 state_quat& current_nom_state_); + +// Jacobian of h(x) with respect to the error state --> H +Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_); + +// Jacobian of h(x) with respect to the nominal state --> Hx +Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_); + +#include "eskf.tpp" // including template implementation + +#endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp new file mode 100644 index 000000000..81e01ba8e --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -0,0 +1,25 @@ + + +template +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(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 +} \ No newline at end of file diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..9220ae7bf --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,74 @@ +#ifndef ESKF_ROS_HPP +#define ESKF_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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::SharedPtr imu_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + rclcpp::Publisher::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_{}; + + sensor_dvl dvl_sensor_{}; + + eskf_params eskf_params_{}; + + std::unique_ptr eskf_; + + bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_{}; + + rclcpp::Time last_imu_time_{}; +}; + +#endif // ESKF_ROS_HPP diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp new file mode 100644 index 000000000..0fe06ee7a --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -0,0 +1,38 @@ +#ifndef ESKF_UTILS_HPP +#define ESKF_UTILS_HPP + +#include +#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 diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 000000000..1a9963df5 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,141 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF_TYPEDEFS_H +#define ESKF_TYPEDEFS_H + +#include +#include +#include +#include + +namespace Eigen { +typedef Eigen::Matrix Vector19d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix19d; +typedef Eigen::Matrix Matrix18x12d; +typedef Eigen::Matrix Matrix4x3d; +typedef Eigen::Matrix Matrix3x19d; +typedef Eigen::Matrix Matrix3x18d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix3x1d; +typedef Eigen::Matrix Matrix19x18d; +typedef Eigen::Matrix Matrix18x3d; +typedef Eigen::Matrix Matrix36d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix9d; +typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Vector15d; +typedef Eigen::Matrix Vector16d; +typedef Eigen::Matrix Matrix15x12d; +typedef Eigen::Matrix Matrix16x15d; +typedef Eigen::Matrix Matrix3x15d; +typedef Eigen::Matrix Matrix3x16d; +typedef Eigen::Matrix Matrix30d; +} // namespace Eigen + +template +Eigen::Matrix create_diagonal_matrix( + const std::vector& diag) { + return Eigen::Map>(diag.data()) + .asDiagonal(); +} +struct state_quat { + 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); + + state_quat() = default; + + Eigen::Vector16d as_vector() const { + Eigen::Vector16d vec{}; + vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, + accel_bias; + 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; + 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::Matrix15d covariance = Eigen::Matrix15d::Zero(); + + Eigen::Vector15d as_vector() const { + Eigen::Vector15d vec{}; + vec << pos, vel, euler, gyro_bias, accel_bias; + return vec; + } + + void set_from_vector(const Eigen::Vector15d& 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); + } +}; + +struct eskf_params { + double temp = 0.0; + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + double dt = 0.0; +}; + +struct imu_measurement { + Eigen::Vector3d accel = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); +}; + +template +concept SensorModelConcept = requires(const T &meas, const state_quat &state) +{ + { meas.innovation(state) } -> std::convertible_to; + { meas.jacobian(state) } -> std::convertible_to; + { meas.noise_covariance() } -> std::convertible_to; +}; + +struct sensor_dvl { + Eigen::Vector3d measurement; + Eigen::Matrix3d measurement_noise; + Eigen::VectorXd innovation(const state_quat &state) const; + Eigen::MatrixXd jacobian(const state_quat &state) const; + Eigen::MatrixXd noise_covariance() const; +}; + + +#endif // ESKF_TYPEDEFS_H diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py new file mode 100644 index 000000000..d7b3a8a4e --- /dev/null +++ b/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]) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml new file mode 100644 index 000000000..015e6f71c --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,22 @@ + + + + eskf + 2.0.0 + Error-state Kalman filter + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + + + ament_cmake + + diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp new file mode 100644 index 000000000..e08c0a90f --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,189 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" +#include "iostream" +#include + +ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} + +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, + const double dt) { + Eigen::Matrix15d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix30d vanLoanMat = Eigen::Matrix30d::Zero(); + + vanLoanMat.topLeftCorner<15, 15>() = -A_c; + vanLoanMat.topRightCorner<15, 15>() = GQG_T; + vanLoanMat.bottomRightCorner<15, 15>() = A_c.transpose(); + + Eigen::Matrix30d vanLoanExp = (vanLoanMat * dt).exp(); + + Eigen::Matrix15d V1 = vanLoanExp.bottomRightCorner<15, 15>().transpose(); + Eigen::Matrix15d V2 = vanLoanExp.topRightCorner<15, 15>(); + + Eigen::Matrix15d A_d = V1; + Eigen::Matrix15d GQG_d = A_d * V2; + + return {A_d, GQG_d}; +} + +Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_) { + Eigen::Matrix3x16d Hx = Eigen::Matrix3x16d::Zero(); + + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + + Eigen::Vector3d v_n = current_nom_state_.vel; + + // Correct derivative w.r.t velocity (nominal state: v_n) + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + + // Derivative w.r.t quaternion (nominal state: q) + // Compute partial derivative w.r.t quaternion directly: + double qw = q.w(); + Eigen::Vector3d q_vec(q.x(), q.y(), q.z()); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + + Eigen::Matrix dhdq; + dhdq.col(0) = 2 * (qw * v_n + q_vec.cross(v_n)); + dhdq.block<3, 3>(0, 1) = + 2 * (q_vec.dot(v_n) * I3 + q_vec * v_n.transpose() - + v_n * q_vec.transpose() - qw * skew(v_n)); + + // Assign quaternion derivative (3x4 block at columns 6:9) + Hx.block<3, 4>(0, 6) = dhdq; + + return Hx; +} + +Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_) { + Eigen::Matrix16x15d x_delta = Eigen::Matrix16x15d::Zero(); + x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + x_delta.block<4, 3>(6, 6) = calculate_T_q(current_nom_state_.quat); + x_delta.block<6, 6>(10, 9) = Eigen::Matrix6d::Identity(); + + Eigen::Matrix3x15d H = calculate_hx(current_nom_state_) * x_delta; + return H; +} + +Eigen::Vector3d calculate_h(const state_quat& current_nom_state_) { + Eigen::Vector3d h; + Eigen::Matrix3d R_bn = + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); + + h = R_bn * current_nom_state_.vel; + return h; +} + +void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, + const double dt) { + Eigen::Vector3d acc = + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + this->g_; + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + + current_nom_state_.quat = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.quat.normalize(); + + current_nom_state_.accel_bias = current_nom_state_.accel_bias*std::exp(accm_bias_p_*dt); + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias*std::exp(gyro_bias_p_*dt); +} + +void ESKF::error_state_prediction(const imu_measurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); + Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); + + Eigen::Matrix15d A_c = Eigen::Matrix15d::Zero(); + A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 6) = -R * skew(acc); + A_c.block<3, 3>(6, 6) = -skew(gyro); + A_c.block<3, 3>(3, 9) = -R; + A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); + // A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix15x12d G_c = Eigen::Matrix15x12d::Zero(); + G_c.block<3, 3>(3, 0) = -R; + G_c.block<3, 3>(6, 3) = -Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix15d A_d, GQG_d; + std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); + + state_euler next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; +} + +void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { + Eigen::Matrix3d S_inv = S.inverse(); + NIS_ = innovation.transpose() * S_inv * innovation; +} + +void ESKF::injection_and_reset() { + current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; + current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; + current_nom_state_.quat = + current_nom_state_.quat * + vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = + current_nom_state_.accel_bias + current_error_state_.accel_bias; + + Eigen::Matrix15d G = Eigen::Matrix15d::Identity(); + + current_error_state_.covariance = + G * current_error_state_.covariance * G.transpose(); + current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); +} + +std::pair ESKF::imu_update( + const imu_measurement& imu_meas, + const double dt) { + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); + + return {current_nom_state_, current_error_state_}; +} + +std::pair ESKF::dvl_update( + const sensor_dvl& dvl_meas) { + measurement_update(dvl_meas); + injection_and_reset(); + + return {current_nom_state_, current_error_state_}; +} + +// DVL sensor model implementations + +Eigen::VectorXd sensor_dvl::innovation(const state_quat &state) const { + + Eigen::Vector3d innovation = this->measurement - calculate_h(state); + return innovation; +} + +Eigen::MatrixXd sensor_dvl::jacobian(const state_quat &state) const { + Eigen::Matrix3x15d H = calculate_h_jacobian(state); + return H; +} + +Eigen::MatrixXd sensor_dvl::noise_covariance() const { + return this->measurement_noise; +} \ No newline at end of file diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 000000000..99e3c8cb5 --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,162 @@ +#include "eskf/eskf_ros.hpp" +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +auto start_message{R"( + ________ ______ ___ ____ ________ + |_ __ |.' ____ \ |_ ||_ _| |_ __ | + | |_ \_|| (___ \_| | |_/ / | |_ \_| + | _| _ _.____`. | __'. | _| + _| |__/ || \____) | _| | \ \_ _| |_ + |________| \______.'|____||____||_____| +)"}; + +ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) + : Node("eskf_node", options) { + time_step = std::chrono::milliseconds(1); + odom_pub_timer_ = this->create_wall_timer( + time_step, std::bind(&ESKFNode::publish_odom, this)); + + set_subscribers_and_publisher(); + + set_parameters(); + + spdlog::info(start_message); + #ifndef NDEBUG + spdlog::info("______________________Debug mode is enabled______________________"); + #endif +} + +void ESKFNode::set_subscribers_and_publisher() { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + this->declare_parameter("imu_topic"); + std::string imu_topic = this->get_parameter("imu_topic").as_string(); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + + this->declare_parameter("dvl_topic"); + std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_topic, qos_sensor_data, + std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + + this->declare_parameter("odom_topic"); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); + + nis_pub_ = create_publisher("dvl/nis", 10); +} + +void ESKFNode::set_parameters() { + std::vector R_imu_correction; + this->declare_parameter>("imu_frame"); + R_imu_correction = get_parameter("imu_frame").as_double_array(); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); + + std::vector diag_Q_std; + this->declare_parameter>("diag_Q_std"); + + diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); + + Eigen::Matrix12d Q; + Q.setZero(); + Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), + sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), + sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), + sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); + eskf_params_.Q = Q; + + eskf_ = std::make_unique(eskf_params_); + + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + Eigen::Matrix15d P = create_diagonal_matrix<15>(diag_p_init); + + error_state_.covariance = P; +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + rclcpp::Time current_time = msg->header.stamp; + + if (!first_imu_msg_received_) { + last_imu_time_ = current_time; + first_imu_msg_received_ = true; + return; + } + + double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; + last_imu_time_ = current_time; + + Eigen::Vector3d raw_accel(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + + imu_meas_.accel = R_imu_eskf_ * raw_accel; + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z); + + imu_meas_.gyro = R_imu_eskf_ * raw_gyro; + + std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + dvl_sensor_.measurement << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + + dvl_sensor_.measurement_noise << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_sensor_); + + #ifndef NDEBUG + // Publish NIS in Debug mode + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->NIS_; + nis_pub_->publish(nis_msg); + #endif +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + + odom_msg.pose.pose.position.x = nom_state_.pos.x(); + odom_msg.pose.pose.position.y = nom_state_.pos.y(); + odom_msg.pose.pose.position.z = nom_state_.pos.z(); + + odom_msg.pose.pose.orientation.w = nom_state_.quat.w(); + odom_msg.pose.pose.orientation.x = nom_state_.quat.x(); + odom_msg.pose.pose.orientation.y = nom_state_.quat.y(); + odom_msg.pose.pose.orientation.z = nom_state_.quat.z(); + + odom_msg.twist.twist.linear.x = nom_state_.vel.x(); + odom_msg.twist.twist.linear.y = nom_state_.vel.y(); + odom_msg.twist.twist.linear.z = nom_state_.vel.z(); + + // Add bias values to the angular velocity field of twist + odom_msg.twist.twist.angular.x = nom_state_.accel_bias.x(); + odom_msg.twist.twist.angular.y = nom_state_.accel_bias.y(); + odom_msg.twist.twist.angular.z = nom_state_.accel_bias.z(); + + // If you also want to include gyro bias, you could add it to the covariance + // matrix or publish a separate topic for biases + + odom_msg.header.stamp = this->now(); + odom_pub_->publish(odom_msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp new file mode 100644 index 000000000..02e8d9229 --- /dev/null +++ b/navigation/eskf/src/eskf_utils.cpp @@ -0,0 +1,52 @@ + +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +Eigen::Matrix3d skew(const Eigen::Vector3d& v) { + Eigen::Matrix3d S; + S << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; + return S; +} + +double sq(double value) { + return value * value; +} +double ssa(double angle) { + double result = fmod(angle + M_PI, 2 * M_PI); + double angle_ssa = result < 0 ? result + M_PI : result - M_PI; + return angle_ssa; +} + +Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat) { + Eigen::Matrix4x3d T_q = Eigen::Matrix4x3d::Zero(); + double qw = quat.w(); + double qx = quat.x(); + double qy = quat.y(); + double qz = quat.z(); + + T_q << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + + T_q *= 0.5; + return T_q; +} + +Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { + double angle = vector.norm(); + if (angle < 1e-8) { + return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + } else { + Eigen::Vector3d axis = vector / angle; + Eigen::Quaterniond quat = + Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + return quat.normalized(); + } +} + +Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); + q.normalize(); + return q; +} diff --git a/tests/ros_node_tests/eskf_node_test.sh b/tests/ros_node_tests/eskf_node_test.sh new file mode 100644 index 000000000..35ba6e91e --- /dev/null +++ b/tests/ros_node_tests/eskf_node_test.sh @@ -0,0 +1,39 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Testing that the ESKF node is able to start up and publish odom" + +# Load ROS 2 environment +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + kill -TERM -"$ESKF_PID" || true + exit 1 +} +trap cleanup ERR + +# Launch eskf node +setsid ros2 launch eskf eskf.launch.py & +ESKF_PID=$! +echo "Launched eskf with PID: $ESKF_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Check if eskf correctly publishes odom +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Terminate processes +kill -TERM -"$ESKF_PID" + +echo "Test completed successfully."