diff --git a/.github/workflows/ros-node-tests.yml b/.github/workflows/ros-node-tests.yml index 4200df85d..1ae75e093 100644 --- a/.github/workflows/ros-node-tests.yml +++ b/.github/workflows/ros-node-tests.yml @@ -10,6 +10,11 @@ on: - cron: '0 1 * * *' # Runs daily at 01:00 UTC jobs: call_reusable_workflow: + strategy: + 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..0028693bf --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,84 @@ +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(vortex_utils REQUIRED) +find_package(vortex_utils_ros 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 + vortex_utils_ros + 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..507c86de9 --- /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.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + 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] + imu_frame: [-1.0, 0.0, 0.0, -1.0, 0.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..66fe42c0d --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,106 @@ +#ifndef ESKF__ESKF_HPP_ +#define ESKF__ESKF_HPP_ + +#include +#include +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKF { + public: + explicit 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 SensorDVL& dvl_meas); + + inline StateQuat get_nominal_state() const { return current_nom_state_; } + + inline StateEuler get_error_state() const { return current_error_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 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_{}; + + // 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_{}; + + // 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 StateQuat& current_nom_state_); + +// Jacobian of h(x) with respect to the error state --> H +Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_); + +// Jacobian of h(x) with respect to the nominal state --> Hx +Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_); + +double compute_nis(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + +#include "eskf.tpp" // including template implementation + +#endif // ESKF__ESKF_HPP_ diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp new file mode 100644 index 000000000..c02d7d4f7 --- /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_ = compute_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 +} diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..31218274f --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,67 @@ +#ifndef ESKF__ESKF_ROS_HPP_ +#define ESKF__ESKF_ROS_HPP_ + +#include +#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 cov_pub_; + + rclcpp::Publisher::SharedPtr nis_pub_; + + std::chrono::milliseconds time_step; + + rclcpp::TimerBase::SharedPtr odom_pub_timer_; + + std::unique_ptr eskf_; + + bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_{}; + + rclcpp::Time last_imu_time_{}; +}; + +#endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 000000000..56b6a224b --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,127 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF__TYPEDEFS_HPP_ +#define ESKF__TYPEDEFS_HPP_ + +#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 Vector12d; +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 createDiagonalMatrix( + const std::vector& diag) { + return Eigen::Map>(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(); + + StateQuat() = 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; + } + + 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; + 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::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 EskfParams { + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + Eigen::Matrix15d P = Eigen::Matrix15d::Zero(); +}; +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(); +}; + +template +concept SensorModelConcept = requires(const T& meas, const StateQuat& state) { + { meas.innovation(state) } -> std::convertible_to; + { meas.jacobian(state) } -> std::convertible_to; + { meas.noise_covariance() } -> std::convertible_to; +}; // NOLINT(readability/braces) + +struct SensorDVL { + Eigen::Vector3d measurement; + Eigen::Matrix3d measurement_noise; + Eigen::VectorXd innovation(const StateQuat& state) const; + Eigen::MatrixXd jacobian(const StateQuat& state) const; + Eigen::MatrixXd noise_covariance() const; +}; + +#endif // ESKF__TYPEDEFS_HPP_ 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..f8472c188 --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,24 @@ + + + + eskf + 2.0.0 + Error-state Kalman filter + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + vortex_utils + vortex_utils_ros + + + ament_cmake + + diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp new file mode 100644 index 000000000..1073a50cf --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,186 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/typedefs.hpp" + +double compute_nis(const Eigen::Vector3d& innovation, + const Eigen::Matrix3d& S) { + Eigen::Matrix3d S_inv = S.inverse(); + return innovation.transpose() * S_inv * innovation; +} + +ESKF::ESKF(const EskfParams& 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 StateQuat& 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 * vortex::utils::math::get_skew_symmetric_matrix(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 StateQuat& 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) = + vortex::utils::math::get_transformation_matrix_attitude_quat( + 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 StateQuat& 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 ImuMeasurement& 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 * dt * dt * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + + current_nom_state_.quat = + (current_nom_state_.quat * + vortex::utils::math::eigen_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 ImuMeasurement& 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 * vortex::utils::math::get_skew_symmetric_matrix(acc); + A_c.block<3, 3>(6, 6) = + -vortex::utils::math::get_skew_symmetric_matrix(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(); + + 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); + + StateEuler next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; +} + +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 * + vortex::utils::math::eigen_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()); +} + +void ESKF::imu_update(const ImuMeasurement& imu_meas, const double dt) { + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); +} + +void ESKF::dvl_update(const SensorDVL& dvl_meas) { + measurement_update(dvl_meas); + injection_and_reset(); +} + +// DVL sensor model implementations + +Eigen::VectorXd SensorDVL::innovation(const StateQuat& state) const { + Eigen::Vector3d innovation = this->measurement - calculate_h(state); + return innovation; +} + +Eigen::MatrixXd SensorDVL::jacobian(const StateQuat& state) const { + Eigen::Matrix3x15d H = calculate_h_jacobian(state); + return H; +} + +Eigen::MatrixXd SensorDVL::noise_covariance() const { + return this->measurement_noise; +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 000000000..63fe6e2aa --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,198 @@ +#include "eskf/eskf_ros.hpp" +#include +#include +#include +#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() { + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + 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); + +#ifndef NDEBUG + nis_pub_ = create_publisher( + "dvl/nis", vortex::utils::qos_profiles::reliable_profile()); +#endif + + cov_pub_ = create_publisher( + "eskf/covariance", 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(); + + if (diag_Q_std.size() != 12) { + throw std::runtime_error("diag_Q_std must have length 12"); + } + + Eigen::Matrix12d Q = Eigen::Map(diag_Q_std.data()) + .array() + .square() + .matrix() + .asDiagonal(); + + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + if (diag_p_init.size() != 15) { + throw std::runtime_error("diag_p_init must have length 15"); + } + Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); + + EskfParams eskf_params{.Q = Q, .P = P}; + + eskf_ = std::make_unique(eskf_params); +} + +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); + + ImuMeasurement imu_measurement{}; + imu_measurement.accel = R_imu_eskf_ * raw_accel; + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z); + + // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; + imu_measurement.gyro = raw_gyro; + + eskf_->imu_update(imu_measurement, dt); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + SensorDVL dvl_sensor; + 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]; + + eskf_->dvl_update(dvl_sensor); + +#ifndef NDEBUG + // Publish NIS in Debug mode + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis(); + nis_pub_->publish(nis_msg); +#endif +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + StateQuat nom_state = eskf_->get_nominal_state(); + + 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); + + // publish the covariance matrix + std_msgs::msg::Float64MultiArray msg; + + msg.layout.dim.resize(2); + msg.layout.dim[0].label = "rows"; + msg.layout.dim[0].size = 15; + msg.layout.dim[0].stride = 15 * 15; + msg.layout.dim[1].label = "cols"; + msg.layout.dim[1].size = 15; + msg.layout.dim[1].stride = 15; + + // get error state covariance + StateEuler error_state_ = eskf_->get_error_state(); + + // Flatten Eigen matrix into a std::vector + msg.data.resize(15 * 15); + msg.data.assign(error_state_.covariance.data(), + error_state_.covariance.data() + 15 * 15); + + // Publish covariance + cov_pub_->publish(msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) 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."