diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 000000000..c431c235f --- /dev/null +++ b/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() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml new file mode 100644 index 000000000..98be8e789 --- /dev/null +++ b/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] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..d9b7d4fa0 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,104 @@ +#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 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 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 diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..c3d09e820 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,88 @@ +#ifndef ESKF_ROS_HPP +#define 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(); + + 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::SharedPtr imu_sub_; + + rclcpp::Subscription::SharedPtr dvl_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + rclcpp::Publisher::SharedPtr nis_pub_; + + rclcpp::Publisher::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_; + + rclcpp::Time last_imu_time_; + + bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_; +}; + +#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..4fcaed412 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -0,0 +1,18 @@ +#ifndef ESKF_UTILS_HPP +#define ESKF_UTILS_HPP + +#include +#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 diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 000000000..47dfe06e1 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,123 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF_TYPEDEFS_H +#define ESKF_TYPEDEFS_H + +#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; +} // namespace Eigen + +template +Eigen::Matrix createDiagonalMatrix( + 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::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); + + 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 diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py new file mode 100644 index 000000000..84284f804 --- /dev/null +++ b/navigation/eskf/launch/eskf.launch.py @@ -0,0 +1,22 @@ +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", + 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..d3d8dc416 --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,22 @@ + + + + eskf + 1.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..e0e7c7a98 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,251 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" +#include "iostream" + +ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} + +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix18d& A_c, + const Eigen::Matrix18x12d& G_c, + const double dt) { + Eigen::Matrix18d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix36d vanLoanMat = Eigen::Matrix36d::Zero(); + + vanLoanMat.topLeftCorner<18, 18>() = -A_c; + vanLoanMat.topRightCorner<18, 18>() = GQG_T; + vanLoanMat.bottomRightCorner<18, 18>() = A_c.transpose(); + + Eigen::Matrix36d vanLoanExp = (vanLoanMat * dt).exp(); + + Eigen::Matrix18d V1 = vanLoanExp.bottomRightCorner<18, 18>().transpose(); + Eigen::Matrix18d V2 = vanLoanExp.topRightCorner<18, 18>(); + + Eigen::Matrix18d A_d = V1; + Eigen::Matrix18d GQG_d = A_d * V2; + + return {A_d, GQG_d}; +} + +Eigen::Matrix4x3d ESKF::calculate_q_delta() { + Eigen::Matrix4x3d q_delta_theta = Eigen::Matrix4x3d::Zero(); + double qw = current_nom_state_.quat.w(); + double qx = current_nom_state_.quat.x(); + double qy = current_nom_state_.quat.y(); + double qz = current_nom_state_.quat.z(); + + q_delta_theta << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + + q_delta_theta *= 0.5; + return q_delta_theta; +} +Eigen::Matrix3x19d ESKF::calculate_hx() { + Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); + + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + + Eigen::Vector3d v_n = current_nom_state_.vel; + + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + + double qw = q.w(); + double qx = q.x(); + double qy = q.y(); + double qz = q.z(); + + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + + Eigen::Vector3d eps(qx, qy, qz); + + Eigen::Matrix3d dR_deta = 2 * qw * I3 - 2 * skew(eps); + + Eigen::Vector3d e1_vec(1, 0, 0), e2_vec(0, 1, 0), e3_vec(0, 0, 1); + + Eigen::Matrix3d dR_dqx = + -2 * qx * I3 + + 2 * (e1_vec * eps.transpose() + eps * e1_vec.transpose()) - + 2 * qw * skew(e1_vec); + + Eigen::Matrix3d dR_dqy = + -2 * qy * I3 + + 2 * (e2_vec * eps.transpose() + eps * e2_vec.transpose()) - + 2 * qw * skew(e2_vec); + + Eigen::Matrix3d dR_dqz = + -2 * qz * I3 + + 2 * (e3_vec * eps.transpose() + eps * e3_vec.transpose()) - + 2 * qw * skew(e3_vec); + + Eigen::Matrix dR_dq; + dR_dq.col(0) = dR_deta * v_n; + dR_dq.col(1) = dR_dqx * v_n; + dR_dq.col(2) = dR_dqy * v_n; + dR_dq.col(3) = dR_dqz * v_n; + + Hx.block<3, 4>(0, 6) = dR_dq; + + return Hx; +} + +Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { + Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); + x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + x_delta.block<4, 3>(6, 6) = calculate_q_delta(); + x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); + + Eigen::Matrix3x18d H = calculate_hx() * x_delta; + return H; +} + +Eigen::Matrix3x1d ESKF::calculate_h() { + Eigen::Matrix3x1d 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) + + current_nom_state_.gravity; + 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_.gyro_bias = current_nom_state_.gyro_bias; + current_nom_state_.accel_bias = current_nom_state_.accel_bias; + current_nom_state_.gravity = current_nom_state_.gravity; +} + +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::Matrix18d A_c = Eigen::Matrix18d::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::Matrix18x12d G_c = Eigen::Matrix18x12d::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::Matrix18d 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::NEEDS() { + Eigen::Vector18d error_state = current_nom_state_.nees_error(ground_truth_); + + // Use SVD-based pseudo-inverse for better numerical stability + Eigen::JacobiSVD svd( + current_error_state_.covariance, + Eigen::ComputeThinU | Eigen::ComputeThinV); + const double epsilon = 1e-10; // Threshold for singular values + Eigen::VectorXd singular_values = svd.singularValues(); + Eigen::VectorXd singular_values_inv(singular_values.size()); + + for (int i = 0; i < singular_values.size(); ++i) { + if (singular_values(i) > epsilon) { + singular_values_inv(i) = 1.0 / singular_values(i); + } else { + singular_values_inv(i) = 0.0; + } + } + + Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * + svd.matrixU().transpose(); + + NEES_ = error_state.transpose() * cov_inv * error_state; +} + +void ESKF::measurement_update(const dvl_measurement& dvl_meas) { + Eigen::Matrix3x18d H = calculate_h_jacobian(); + Eigen::Matrix18d P = current_error_state_.covariance; + Eigen::Matrix3d R = dvl_meas.cov; + + Eigen::Matrix3d S = H * P * H.transpose() + R; + Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); + Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + NIS(innovation, S); + current_error_state_.set_from_vector(K * innovation); + + Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations + + NEEDS(); +} + +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; + current_nom_state_.gravity = + current_nom_state_.gravity + current_error_state_.gravity; + + Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); + + current_error_state_.covariance = + G * current_error_state_.covariance * G.transpose(); + current_error_state_.set_from_vector(Eigen::Vector18d::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 dvl_measurement& dvl_meas) { + measurement_update(dvl_meas); + injection_and_reset(); + + return {current_nom_state_, current_error_state_}; +} diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp new file mode 100644 index 000000000..196fa7916 --- /dev/null +++ b/navigation/eskf/src/eskf_node.cpp @@ -0,0 +1,9 @@ +#include "eskf/eskf_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + spdlog::info("Starting ESKF Node"); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 000000000..54daadb4c --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,184 @@ +#include "eskf/eskf_ros.hpp" +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +ESKFNode::ESKFNode() : Node("eskf_node") { + 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("ESKF Node Initialized"); +} + +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); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + "/orca/pose", qos_sensor_data, + std::bind(&ESKFNode::pose_callback, this, std::placeholders::_1)); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + "/orca/twist", qos_sensor_data, + std::bind(&ESKFNode::twist_callback, this, std::placeholders::_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( + 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); + nees_pub_ = create_publisher("dvl/needs", 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::Matrix18d P = createDiagonalMatrix<18>(diag_p_init); + + error_state_.covariance = P; +} + +void ESKFNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + g_truth_.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, + msg->pose.pose.position.z; + g_truth_.quat.w() = msg->pose.pose.orientation.w; + g_truth_.quat.x() = msg->pose.pose.orientation.x; + g_truth_.quat.y() = msg->pose.pose.orientation.y; + g_truth_.quat.z() = msg->pose.pose.orientation.z; +} + +void ESKFNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + g_truth_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; +} + +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 stonefish_ros2::msg::DVL::SharedPtr msg) { + dvl_meas_.vel << msg->velocity.x, msg->velocity.y, msg->velocity.z; + dvl_meas_.cov << 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001; + + // msg->velocity_covariance[0], msg->velocity_covariance[1], + // msg->velocity_covariance[2], + // msg->velocity_covariance[3], msg->velocity_covariance[4], + // msg->velocity_covariance[5], + // msg->velocity_covariance[6], msg->velocity_covariance[7], + // msg->velocity_covariance[8]; + + // Set biases and gravity as float values + float gyro_bias_x = 0.00001; + float gyro_bias_y = 0.00001; + float gyro_bias_z = 0.00001; + + float accel_bias_x = 0.00001; + float accel_bias_y = 0.00001; + float accel_bias_z = 0.00001; + + float gravity_x = 0.0; + float gravity_y = 0.0; + float gravity_z = -9.81; + + g_truth_.gyro_bias << gyro_bias_x, gyro_bias_y, gyro_bias_z; + g_truth_.accel_bias << accel_bias_x, accel_bias_y, accel_bias_z; + g_truth_.gravity << gravity_x, gravity_y, gravity_z; + + eskf_->ground_truth_ = g_truth_; + + std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); + + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->NIS_; + nis_pub_->publish(nis_msg); + + std_msgs::msg::Float64 nees_msg; + nees_msg.data = eskf_->NEES_; + nees_pub_->publish(nees_msg); +} + +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(); + + odom_msg.header.stamp = this->now(); + odom_pub_->publish(odom_msg); +} diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp new file mode 100644 index 000000000..88d04c3d5 --- /dev/null +++ b/navigation/eskf/src/eskf_utils.cpp @@ -0,0 +1,39 @@ + +#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(const double& value) { + return value * value; +} +double ssa(const 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::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/navigation/ukf_okid/CMakeLists.txt b/navigation/ukf_okid/CMakeLists.txt new file mode 100644 index 000000000..901ca044b --- /dev/null +++ b/navigation/ukf_okid/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(ukf_python) + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + ukf_python/ukf_ros.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py new file mode 100644 index 000000000..5d075259f --- /dev/null +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + ukf_node = Node( + package="ukf_python", + executable="ukf_ros.py", + name="ukf_node", + ) + + return LaunchDescription([ukf_node]) diff --git a/navigation/ukf_okid/package.xml b/navigation/ukf_okid/package.xml new file mode 100644 index 000000000..1c1b2b4cb --- /dev/null +++ b/navigation/ukf_okid/package.xml @@ -0,0 +1,22 @@ + + + + ukf_python + 1.0.0 + Uscented Kalman filter for AUV model + talha + MIT + + ament_cmake_python + + rclpy + geometry_msgs + nav_msgs + vortex_msgs + python-control-pip + std_msgs + + + ament_cmake + + diff --git a/navigation/ukf_okid/ukf_python/__init__.py b/navigation/ukf_okid/ukf_python/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py new file mode 100644 index 000000000..e50c65c3a --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -0,0 +1,131 @@ +import numpy as np +from ukf_okid_class import ( + MeasModel, + StateQuat, + covariance_measurement, + covariance_set, + cross_covariance, + mean_measurement, + mean_set, + okid_process_model, +) + + +class UKF: + def __init__(self, process_model: okid_process_model, x_0, P_0, Q, G): + self.x = x_0 + self.P = P_0 + self.Q = Q + self.G = G + self.process_model = process_model + self.sigma_points_list = None + self.measurement_updated = MeasModel() + self.y_i = None + self.weight = None + self.delta = self.generate_delta_matrix(len(x_0.as_vector()) - 1) + self.cross_correlation = None + + def generate_delta_matrix(self, n: float) -> np.ndarray: + """Generates the weight matrix used in the TUKF sigma point generation. + + Parameters: + n (int): The state dimension. + + Returns: + delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + """ + delta = np.zeros((n, 2 * n)) + k = 0.01 # Tuning parameter to ensure pos def + + for i in range(2 * n): + for j in range(n // 2): + delta[2 * j + 1, i] = np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) + delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) + + if (n % 2) == 1: + delta[n - 1, i] = (-1) ** i + return delta + + def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: + """Functions that generate the sigma points for the UKF.""" + n = len(current_state.covariance) + + S = np.linalg.cholesky(current_state.covariance + self.Q) + + self.sigma_points_list = [StateQuat() for _ in range(2 * n)] + + for index, state in enumerate(self.sigma_points_list): + delta_x = S @ self.delta[:, index] + state.fill_dynamic_states(current_state.as_vector(), delta_x) + + return self.sigma_points_list + + def unscented_transform(self, current_state: StateQuat) -> StateQuat: + """The unscented transform function generates the priori state estimate.""" + self.sigma_points(current_state) + n = len(current_state.covariance) + + self.y_i = [StateQuat() for _ in range(2 * n)] + + for i, state in enumerate(self.sigma_points_list): + self.process_model.model_prediction(state) + self.process_model.state_vector_prev = state + self.y_i[i] = self.process_model.euler_forward() + + state_estimate = StateQuat() + x = mean_set(self.y_i) + + state_estimate.fill_states(x) + state_estimate.covariance = covariance_set(self.y_i, x) + return state_estimate + + def measurement_update( + self, current_state: StateQuat, measurement: MeasModel + ) -> None: + """Function that updates the state estimate with a measurement. + + Hopefully this is the DVL or GNSS + """ + n = len(current_state.covariance) + z_i = [MeasModel() for _ in range(2 * n)] + + for i, state in enumerate(self.sigma_points_list): + z_i[i] = measurement.H(state) + + self.measurement_updated.measurement = mean_measurement(z_i) + + self.measurement_updated.covariance = covariance_measurement( + z_i, self.measurement_updated.measurement + ) + + self.cross_correlation = cross_covariance( + self.y_i, + current_state.as_vector(), + z_i, + self.measurement_updated.measurement, + ) + + def posteriori_estimate( + self, + current_state: StateQuat, + measurement: MeasModel, + ) -> StateQuat: + """Calculates the posteriori estimate using measurement and the prior estimate.""" + nu_k = MeasModel() + nu_k.measurement = ( + measurement.measurement - self.measurement_updated.measurement + ) + nu_k.covariance = self.measurement_updated.covariance + measurement.covariance + + K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) + + posteriori_estimate = StateQuat() + + posteriori_estimate.fill_states_different_dim( + current_state.as_vector(), np.dot(K_k, nu_k.measurement) + ) + posteriori_estimate.covariance = current_state.covariance - np.dot( + K_k, np.dot(nu_k.covariance, np.transpose(K_k)) + ) + + return posteriori_estimate diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py new file mode 100644 index 000000000..0b329cb84 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -0,0 +1,724 @@ +from dataclasses import dataclass, field + +import numpy as np + + +@dataclass +class okid: + """A class to represent the parameters for the OKID algorithm.""" + + inertia: np.ndarray = field( + default_factory=lambda: np.array( + [0.68, 0.0, 0.0, 0.0, 3.32, 0.0, 0.0, 0.0, 3.34] + ) + ) + added_mass: np.ndarray = field( + default_factory=lambda: np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + ) + damping_linear: np.ndarray = field( + default_factory=lambda: np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + ) + + def fill(self, state: np.ndarray) -> None: + """Fills the okid_params object with values from a numpy array.""" + self.inertia = state[0:9] + self.added_mass = state[9:15] + self.damping_linear = state[15:] + + def as_vector(self) -> np.ndarray: + """Returns the okid_params as a numpy array.""" + return np.concatenate([self.inertia, self.added_mass, self.damping_linear]) + + def __add__(self, other: 'okid') -> 'okid': + """Defines the addition operation between two okid_params objects.""" + result = okid() + result.inertia = self.inertia + other.inertia + result.added_mass = self.added_mass + other.added_mass + result.damping_linear = self.damping_linear + other.damping_linear + return result + + def __sub__(self, other: 'okid') -> 'okid': + """Defines the subtraction operation between two okid_params objects.""" + result = okid() + result.inertia = self.inertia - other.inertia + result.added_mass = self.added_mass - other.added_mass + result.damping_linear = self.damping_linear - other.damping_linear + return result + + def __sub__(self, other: np.ndarray) -> 'okid': + """Defines sub between okid_params and np.ndarray.""" + result = okid() + result.inertia = self.inertia - other[0:9] + result.added_mass = self.added_mass - other[9:15] + result.damping_linear = self.damping_linear - other[15:] + return result + + def __rmul__(self, scalar: float) -> 'okid': + """Defines the multiplication operation between a scalar and okid_params object.""" + result = okid() + result.inertia = scalar * self.inertia + result.added_mass = scalar * self.added_mass + result.damping_linear = scalar * self.damping_linear + return result + + +@dataclass +class StateQuat: + """A class to represent the state to be estimated by the UKF.""" + + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) + velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + okid_params: okid = field(default_factory=okid) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) + + def as_vector(self) -> np.ndarray: + """Returns the StateVector as a numpy array.""" + return np.concatenate( + [ + self.position, + self.orientation, + self.velocity, + self.angular_velocity, + self.okid_params.as_vector(), + ] + ) + + def dynamic_part(self) -> np.ndarray: + """Returns the dynamic part of the state vector.""" + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity] + ) + + def nu(self) -> np.ndarray: + """Calculates the nu vector.""" + return np.concatenate([self.velocity, self.angular_velocity]) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = self.orientation + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) + return R + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array.""" + self.position = state[0:3] + self.orientation = state[3:7] + self.velocity = state[7:10] + self.angular_velocity = state[10:13] + self.okid_params.fill(state[13:]) + + def fill_dynamic_states(self, state: np.ndarray, state_euler: np.ndarray) -> None: + """Fills only the dynamic part of the state vector with the values from a numpy array.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_super_product( + state[3:7], euler_to_quat(state_euler[3:6]) + ) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] + + def fill_states_different_dim( + self, state: np.ndarray, state_euler: np.ndarray + ) -> None: + """Fills states when the state vector has different dimensions than the default state vector.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_super_product( + state[3:7], euler_to_quat(state_euler[3:6]) + ) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] + self.okid_params.fill(state[13:] + state_euler[12:]) + + def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: + """Subtracts two StateQuat objects, returning the difference with Euler angles.""" + new_array = np.zeros(len(self.as_vector()) - 1) + new_array[:3] = self.position - other.position + new_array[3:6] = error_ori + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + new_array[12:] = self.okid_params.as_vector() - other.okid_params.as_vector() + + return new_array + + def __add__(self, other: 'StateQuat') -> 'StateQuat': + """Adds two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position + other.position + new_state.orientation = quaternion_super_product( + self.orientation, other.orientation + ) + new_state.velocity = self.velocity + other.velocity + new_state.angular_velocity = self.angular_velocity + other.angular_velocity + new_state.okid_params = self.okid_params + other.okid_params + + return new_state + + def __sub__(self, other: 'StateQuat') -> 'StateQuat': + """Subtracts two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position - other.position + new_state.orientation = quaternion_error(self.orientation, other.orientation) + new_state.velocity = self.velocity - other.velocity + new_state.angular_velocity = self.angular_velocity - other.angular_velocity + new_state.okid_params = self.okid_params - other.okid_params + + return new_state.as_vector() + + def __rmul__(self, scalar: float) -> 'StateQuat': + """Multiplies the StateQuat object by a scalar.""" + new_state = StateQuat() + new_state.position = scalar * self.position + new_state.orientation = quat_norm(scalar * self.orientation) + new_state.velocity = scalar * self.velocity + new_state.angular_velocity = scalar * self.angular_velocity + new_state.okid_params = scalar * self.okid_params + + return new_state + + def insert_weights(self, weights: np.ndarray) -> np.ndarray: + """Inserts the weights into the covariance matrix.""" + new_state = StateQuat() + new_state.position = self.position - weights[:3] + new_state.orientation = quaternion_error( + self.orientation, euler_to_quat(weights[3:6]) + ) + new_state.velocity = self.velocity - weights[6:9] + new_state.angular_velocity = self.angular_velocity - weights[9:12] + new_state.okid_params = self.okid_params - weights[12:] + + return new_state.as_vector() + + def add_without_quaternions(self, other: 'StateQuat') -> None: + """Adds elements into the state vector without considering the quaternions.""" + self.position += other.position + self.velocity += other.velocity + self.angular_velocity += other.angular_velocity + self.okid_params += other.okid_params + + +@dataclass +class MeasModel: + """A class defined for a general measurement model.""" + + measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + + def H(self, state: StateQuat) -> 'MeasModel': + """Calculates the measurement matrix.""" + H = np.zeros((3, 13)) + H[:, 7:10] = np.eye(3) + z_i = MeasModel() + z_i.measurement = np.dot(H, state.dynamic_part()) + return z_i + + def __add__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the addition operation between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement + other.measurement + return result + + def __rmul__(self, scalar: float) -> 'MeasModel': + """Defines multiplication between scalar value and MeasModel object.""" + result = MeasModel() + result.measurement = scalar * self.measurement + return result + + def __sub__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the subtraction between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement - other.measurement + return result + + +@dataclass +class process_model: + """A class defined for a general process model.""" + + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 0.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.0 + integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array( + [ + [ + 1 - 2 * e_2**2 - 2 * e_3**2, + 2 * e_1 * e_2 - 2 * nu * e_3, + 2 * e_1 * e_3 + 2 * nu * e_2, + ], + [ + 2 * e_1 * e_2 + 2 * nu * e_3, + 1 - 2 * e_1**2 - 2 * e_3**2, + 2 * e_2 * e_3 - 2 * nu * e_1, + ], + [ + 2 * e_1 * e_3 - 2 * nu * e_2, + 2 * e_2 * e_3 + 2 * nu * e_1, + 1 - 2 * e_1**2 - 2 * e_2**2, + ], + ] + ) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array( + [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] + ) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) + return D_l + D_nl + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot( + self.T(), self.state_vector.angular_velocity + ) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( + self.Control_input + - np.dot(self.Crb(), self.state_vector.nu()) + - np.dot(self.D(), self.state_vector.nu()) + ) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> StateQuat: + """Calculates the forward Euler integration.""" + self.state_vector.position = ( + self.state_vector_prev.position + self.state_vector_dot.position * self.dt + ) + self.state_vector.orientation = quat_norm( + self.state_vector_prev.orientation + + self.state_vector_dot.orientation * self.dt + ) + self.state_vector.velocity = ( + self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + ) + self.state_vector.angular_velocity = ( + self.state_vector_prev.angular_velocity + + self.state_vector_dot.angular_velocity * self.dt + ) + return self.state_vector + + +@dataclass +class okid_process_model: + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 30.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.01 + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array( + [ + [ + 1 - 2 * e_2**2 - 2 * e_3**2, + 2 * e_1 * e_2 - 2 * nu * e_3, + 2 * e_1 * e_3 + 2 * nu * e_2, + ], + [ + 2 * e_1 * e_2 + 2 * nu * e_3, + 1 - 2 * e_1**2 - 2 * e_3**2, + 2 * e_2 * e_3 - 2 * nu * e_1, + ], + [ + 2 * e_1 * e_3 - 2 * nu * e_2, + 2 * e_2 * e_3 + 2 * nu * e_1, + 1 - 2 * e_1**2 - 2 * e_2**2, + ], + ] + ) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array( + [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] + ) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + + return D_l + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + """ + separate out the different okid values + """ + self.inertia = state.okid_params.inertia.reshape((3, 3)) + self.added_mass = state.okid_params.added_mass + self.damping_linear = state.okid_params.damping_linear + + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot( + self.T(), self.state_vector.angular_velocity + ) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( + self.Control_input + - np.dot(self.Crb(), self.state_vector.nu()) + - np.dot(self.D(), self.state_vector.nu()) + ) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> None: + """Calculates the forward Euler integration.""" + self.state_vector.position = ( + self.state_vector_prev.position + self.state_vector_dot.position * self.dt + ) + self.state_vector.orientation = quat_norm( + self.state_vector_prev.orientation + + self.state_vector_dot.orientation * self.dt + ) + self.state_vector.velocity = ( + self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + ) + self.state_vector.angular_velocity = ( + self.state_vector_prev.angular_velocity + + self.state_vector_dot.angular_velocity * self.dt + ) + return self.state_vector + + +def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: + """Converts Euler angles to a quaternion.""" + psi, theta, phi = euler_angles + c_psi = np.cos(psi / 2) + s_psi = np.sin(psi / 2) + c_theta = np.cos(theta / 2) + s_theta = np.sin(theta / 2) + c_phi = np.cos(phi / 2) + s_phi = np.sin(phi / 2) + + quat = np.array( + [ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi, + ] + ) + + return quat + + +def quat_to_euler(quat: np.ndarray) -> np.ndarray: + """Converts a quaternion to Euler angles.""" + nu, eta_1, eta_2, eta_3 = quat + + phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) + theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2**2 + eta_3**2)) + + return np.array([phi, theta, psi]) + + +def quat_norm(quat: np.ndarray) -> np.ndarray: + """Function that normalizes a quaternion.""" + quat = quat / np.linalg.norm(quat) + + return quat + + +def skew_symmetric(vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + + +def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) + + eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) + + q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) + + return q_new + + +def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """Calculates the error between two quaternions.""" + quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) + + error_quat = quaternion_super_product(quat_1, quat_2_inv) + + return error_quat + + +def iterative_quaternion_mean_statequat( + state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 +) -> np.ndarray: + """Computes the iterative mean of quaternion orientations from StateQuat objects. + + Args: + state_list: List of StateQuat objects + tol: Convergence tolerance + max_iter: Maximum iterations + + Returns: + Mean quaternion as numpy array + """ + sigma_quats = [state.orientation for state in state_list] + n = len(state_list) + + mean_q = sigma_quats[0].copy() + + for _ in range(max_iter): + weighted_error_vectors = [] + for i, q in enumerate(sigma_quats): + mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) + e = quaternion_super_product(q, mean_q_conj) + + e0_clipped = np.clip(e[0], -1.0, 1.0) + angle = 2 * np.arccos(e0_clipped) + if np.abs(angle) < 1e-8: + error_vec = np.zeros(3) + else: + error_vec = (angle / np.sin(angle / 2)) * e[1:4] + weighted_error_vectors.append(error_vec) + + error_avg = (1 / n) * np.sum(weighted_error_vectors, axis=0) + if np.linalg.norm(error_avg) < tol: + break + + error_norm = np.linalg.norm(error_avg) + if error_norm > 0: + delta_q = np.array( + [ + np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm)), + ] + ) + else: + delta_q = np.array([1.0, 0.0, 0.0, 0.0]) + + mean_q = quaternion_super_product(delta_q, mean_q) + mean_q = quat_norm(mean_q) + + return mean_q + + +def mean_set(set_points: list[StateQuat]) -> np.ndarray: + """Function calculates the mean vector of a set of points. + + Args: + set_points (list[StateQuat]): List of StateQuat objects + + Returns: + np.ndarray: The mean vector + """ + n = len(set_points) + mean_value = StateQuat() + + for state in set_points: + mean_value.add_without_quaternions(state) + + mean_value.position = (1 / n) * mean_value.position + mean_value.velocity = (1 / n) * mean_value.velocity + mean_value.angular_velocity = (1 / n) * mean_value.angular_velocity + mean_value.okid_params = (1 / n) * mean_value.okid_params + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points) + + return mean_value.as_vector() + + +def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: + """Function that calculates the mean of a set of points.""" + n = len(set_points) + mean_value = MeasModel() + + for state in set_points: + mean_value = mean_value + state + + mean_value = (1 / n) * mean_value + + return mean_value.measurement + + +def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: + """Function that calculates the covariance of a set of points.""" + n = len(set_points) + covariance = np.zeros(set_points[0].covariance.shape) + + mean_quat = StateQuat() + mean_quat.fill_states(mean) + + mean_q = mean_quat.orientation + + for state in set_points: + q = state.orientation + diff_q = quaternion_error(q, mean_q) + + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) + angle = 2.0 * np.arccos(e0_clipped) + if abs(angle) < 1e-8: + e_vec = np.zeros(3) + else: + e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] + + covariance += np.outer( + state.subtract(mean_quat, e_vec), state.subtract(mean_quat, e_vec) + ) + + covariance = (1 / (n)) * covariance + + return covariance + + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: + """Function that calculates the covariance of a set of points.""" + n = len(set_points) + co_size = len(set_points[0].measurement) + covariance = np.zeros((co_size, co_size)) + + mean_meas = MeasModel() + mean_meas.measurement = mean + + for state in set_points: + temp_state = state - mean_meas + covariance += np.outer(temp_state.measurement, temp_state.measurement) + + covariance = (1 / n) * covariance + + return covariance + + +def cross_covariance( + set_y: list[StateQuat], + mean_y: np.ndarray, + set_z: list[MeasModel], + mean_z: np.ndarray, +) -> np.ndarray: + """Calculates the cross covariance between the measurement and state prediction.""" + n = len(set_y) + + cross_covariance = np.zeros((len(mean_y) - 1, len(mean_z))) + mean_quat = StateQuat() + mean_quat.fill_states(mean_y) + + mean_q = mean_quat.orientation + + for i in range(n): + q = set_y[i].orientation + diff_q = quaternion_error(q, mean_q) + + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) + angle = 2.0 * np.arccos(e0_clipped) + if abs(angle) < 1e-8: + e_vec = np.zeros(3) + else: + e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] + + cross_covariance += np.outer( + set_y[i].subtract(mean_quat, e_vec), set_z[i].measurement - mean_z + ) + + cross_covariance = (1 / n) * cross_covariance + + return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_ros.py b/navigation/ukf_okid/ukf_python/ukf_ros.py new file mode 100755 index 000000000..a40c2f7c3 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_ros.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +import numpy as np +import rclpy +from geometry_msgs.msg import TwistWithCovarianceStamped, WrenchStamped +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from ukf_okid import UKF +from ukf_okid_class import MeasModel, StateQuat, process_model + + +class UKFNode(Node): + def __init__(self): + super().__init__("UKFNode") + + best_effort_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + + # subscribers + self.dvl_subscriber = self.create_subscription( + TwistWithCovarianceStamped, + "/orca/twist", + self.dvl_callback, + qos_profile=best_effort_qos, + ) + + self.control_input = self.create_subscription( + WrenchStamped, + "/orca/wrench_input", + self.control_callback, + qos_profile=best_effort_qos, + ) + + self.odom_publish = self.create_publisher( + Odometry, "/orca/odometry", qos_profile=best_effort_qos + ) + dt = self.declare_parameter("dt", 0.01).get_parameter_value().double_value + self.control_timer = self.create_timer(dt, self.odom_publisher) + + self.current_state = StateQuat() + x0 = np.zeros(13) + x0[3] = 1.0 # quaternion: [1, 0, 0, 0] + P0 = np.eye(12) * 0.5 + self.ukf_model = process_model() + self.ukf_model.dt = 0.01 + self.ukf_model.mass_interia_matrix = np.array( + [ + [30.0, 0.0, 0.0, 0.0, 0.0, 0.6], + [0.0, 30.0, 0.0, 0.0, -0.6, 0.3], + [0.0, 0.0, 30.0, 0.6, 0.3, 0.0], + [0.0, 0.0, 0.6, 0.68, 0.0, 0.0], + [0.0, -0.6, 0.3, 0.0, 3.32, 0.0], + [0.6, 0.3, 0.0, 0.0, 0.0, 3.34], + ] + ) + self.ukf_model.m = 30.0 + self.ukf_model.r_b_bg = np.array([0.01, 0.0, 0.02]) + self.ukf_model.inertia = np.diag([0.68, 3.32, 3.34]) + self.ukf_model.damping_linear = np.array([0.01] * 6) + # self.ukf_model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + Q = np.diag([0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + + self.ukf = UKF(self.ukf_model, x0, P0, Q) + self.ukf_flagg = False + + def dvl_callback(self, msg: TwistWithCovarianceStamped): + # unpack msg + dvl_measurement = MeasModel() + # Print received DVL data to console + # self.get_logger().info(f"DVL data received: x={msg.twist.twist.linear.x}, y={msg.twist.twist.linear.y}, z={msg.twist.twist.linear.z}") + dvl_measurement.measurement = np.array( + [ + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ] + ) + dvl_measurement.covariance = np.array( + [ + [ + msg.twist.covariance[0], + msg.twist.covariance[1], + msg.twist.covariance[3], + ], + [ + msg.twist.covariance[6], + msg.twist.covariance[7], + msg.twist.covariance[8], + ], + [ + msg.twist.covariance[12], + msg.twist.covariance[13], + msg.twist.covariance[14], + ], + ] + ) + + self.ukf.measurement_update(self.current_state, dvl_measurement) + self.current_state = self.ukf.posteriori_estimate( + self.current_state, dvl_measurement + ) + self.ukf_flagg = True + + def control_callback(self, msg: WrenchStamped): + # unpack message + control_array = np.array( + [ + msg.wrench.force.x, + msg.wrench.force.y, + msg.wrench.force.z, + msg.wrench.torque.x, + msg.wrench.torque.y, + msg.wrench.torque.z, + ] + ) + self.ukf_model.Control_input = control_array + + def odom_publisher(self): + msg = Odometry() + + if self.ukf_flagg == False: + self.current_state = self.ukf.unscented_transform(self.current_state) + else: + self.ukf_flagg = False + + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "odom" + msg.child_frame_id = "base_link" + + msg.pose.pose.position.x = self.current_state.position[0] + msg.pose.pose.position.y = self.current_state.position[1] + msg.pose.pose.position.z = self.current_state.position[2] + msg.pose.pose.orientation.w = self.current_state.orientation[0] + msg.pose.pose.orientation.x = self.current_state.orientation[1] + msg.pose.pose.orientation.y = self.current_state.orientation[2] + msg.pose.pose.orientation.z = self.current_state.orientation[3] + msg.twist.twist.linear.x = self.current_state.velocity[0] + msg.twist.twist.linear.y = self.current_state.velocity[1] + msg.twist.twist.linear.z = self.current_state.velocity[2] + msg.twist.twist.angular.x = self.current_state.angular_velocity[0] + msg.twist.twist.angular.y = self.current_state.angular_velocity[1] + msg.twist.twist.angular.z = self.current_state.angular_velocity[2] + + self.odom_publish.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + ukf_node = UKFNode() + rclpy.spin(ukf_node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py new file mode 100644 index 000000000..d80427364 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -0,0 +1,458 @@ +import matplotlib.pyplot as plt +import numpy as np + +# Import your classes and functions. +# Adjust the import paths as necessary based on your module organization. +from ukf_okid_class import ( + MeasModel, + StateQuat, + covariance_measurement, + covariance_set, + cross_covariance, + mean_measurement, + mean_set, + quat_norm, + quat_to_euler, + quaternion_super_product, +) + + +# For testing, define a function to create a StateQuat with small perturbations. +def create_statequat( + base_vector, + position_perturbation, + orientation_perturbation, + velocity_perturbation, + angular_velocity_perturbation, +): + """Creates a StateQuat object. + - base_vector: 1D numpy array for base state (13 elements: + position (3), quaternion (4), velocity (3), angular_velocity (3)) + - ..._perturbation: small perturbation vector to be added to each respective component. + Returns a StateQuat. + """ + state = StateQuat() + # Base state + state.position = base_vector[0:3] + position_perturbation + # For orientation, perturb by adding a small rotation: + base_quat = base_vector[3:7] + noise_angle = np.linalg.norm(orientation_perturbation) + if noise_angle < 1e-8: + noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) + else: + noise_axis = orientation_perturbation / noise_angle + noise_quat = np.concatenate( + ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) + ) + state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) + state.velocity = base_vector[7:10] + velocity_perturbation + state.angular_velocity = base_vector[10:13] + angular_velocity_perturbation + + # For the augmented parameters (OKID parameters), set a 21-element vector: + # 9 for inertia, 6 for added_mass, and 6 for damping_linear. + state.okid_params.fill(np.concatenate((np.zeros(9), np.zeros(6), np.zeros(6)))) + + # Set a default covariance (33x33 for the extended state) + state.covariance = np.eye(33) * 0.01 + return state + + +# Test functions for state statistics +def test_state_statistics(): + # Define a base state vector (13 elements: position, quaternion, velocity, angular_velocity) + base_vector = np.zeros(13) + base_vector[0:3] = np.array([1.0, 2.0, 3.0]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion + base_vector[7:10] = np.array([0.1, 0.2, 0.3]) + base_vector[10:13] = np.array([0.01, 0.02, 0.03]) + + # Create a list of StateQuat objects with small random perturbations. + np.random.seed(42) + state_list = [] + num_states = 10 + for _ in range(num_states): + pos_noise = np.random.normal(0, 0.05, 3) + ori_noise = np.random.normal(0, 0.01, 3) + vel_noise = np.random.normal(0, 0.02, 3) + ang_vel_noise = np.random.normal(0, 0.005, 3) + state_list.append( + create_statequat( + base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise + ) + ) + + # Compute the state mean using mean_set. + mean_state_vec = mean_set(state_list) + print("Computed mean state vector:") + print(mean_state_vec) + + # Compute the covariance of the states. + cov_state = covariance_set(state_list, mean_state_vec) + print("Computed state covariance matrix:") + print(cov_state) + + # Check symmetry of the covariance: + asym_error = np.linalg.norm(cov_state - cov_state.T) + print("Covariance symmetry error (should be near 0):", asym_error) + + # Check eigenvalues for positive semidefiniteness: + eigvals = np.linalg.eigvals(cov_state) + print("Eigenvalues of state covariance:") + print(eigvals) + + +def test_measurement_statistics(): + # Create a list of measurement objects (MeasModel) with measurements in R^3. + np.random.seed(24) + meas_list = [] + num_meas = 10 + base_meas = np.array([1.0, 2.0, 3.0]) + for _ in range(num_meas): + noise = np.random.normal(0, 0.1, 3) + meas = MeasModel() + meas.measurement = base_meas + noise + meas_list.append(meas) + + # Compute the measurement mean. + mean_meas = mean_measurement(meas_list) + print("Computed measurement mean:") + print(mean_meas) + + # Compute the measurement covariance. + cov_meas = covariance_measurement(meas_list, mean_meas) + print("Computed measurement covariance:") + print(cov_meas) + + # Check symmetry and eigenvalues. + asym_error = np.linalg.norm(cov_meas - cov_meas.T) + print("Measurement covariance symmetry error:", asym_error) + eigvals = np.linalg.eigvals(cov_meas) + print("Eigenvalues of measurement covariance:") + print(eigvals) + + +def test_cross_covariance(): + # Create a set of StateQuat and corresponding MeasModel objects. + np.random.seed(99) + num = 10 + state_list = [] + meas_list = [] + base_vector = np.zeros(13) + base_vector[0:3] = np.array([0.5, 1.0, -0.5]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) + base_vector[7:10] = np.array([0.05, 0.1, 0.15]) + base_vector[10:13] = np.array([0.005, 0.01, 0.015]) + + for _ in range(num): + pos_noise = np.random.normal(0, 0.02, 3) + ori_noise = np.random.normal(0, 0.005, 3) + vel_noise = np.random.normal(0, 0.01, 3) + ang_vel_noise = np.random.normal(0, 0.002, 3) + state = create_statequat( + base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise + ) + state_list.append(state) + + # Generate a measurement from each state (e.g., state velocity plus noise). + meas = MeasModel() + meas.measurement = state.velocity + np.random.normal(0, 0.01, 3) + meas_list.append(meas) + + # Compute the state mean and measurement mean as vectors. + mean_state_vec = mean_set(state_list) + mean_meas = mean_measurement(meas_list) + + cross_cov = cross_covariance(state_list, mean_state_vec, meas_list, mean_meas) + print("Computed cross-covariance between state and measurement:") + print(cross_cov) + + +import time + +from ukf_okid import UKF + +# Import your classes and functions. +from ukf_okid_class import ( + okid_process_model, + process_model, +) # Your process model classes + + +############################################ +# Helper function to create a StateQuat with perturbations. +############################################ +def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise): + """Create a StateQuat object from a base vector (13 elements: + position (3), quaternion (4), velocity (3), angular_velocity (3)) + plus additive noise on each component. + + For the OKID parameters, we assume a 21-element vector: + - first 9: inertia, + - next 6: added_mass, + - last 6: damping_linear. + """ + state = StateQuat() + state.position = base_vector[0:3] + pos_noise + base_quat = base_vector[3:7] + noise_angle = np.linalg.norm(ori_noise) + if noise_angle < 1e-8: + noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) + else: + noise_axis = ori_noise / noise_angle + noise_quat = np.concatenate( + ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) + ) + state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) + state.velocity = base_vector[7:10] + vel_noise + state.angular_velocity = base_vector[10:13] + ang_vel_noise + + # Set OKID parameters to exactly 21 elements (9,6,6) + state.okid_params.fill( + np.concatenate( + ( + np.array([0.0, 0.0, 0.3, 0.0, 0.0, 3.3, 0.0, 0.0, 3.3]), + np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + ) + ) + ) + # Set an initial covariance (33x33) for the full augmented state. + state.covariance = np.eye(33) * 0.02 + return state + + +############################################ +# Full Filter Simulation Test +############################################ +def run_ukf_simulation(): + dt = 0.01 # Time step for simulation [s] + simulation_time = 10 # Total simulation time in seconds + num_steps = int(simulation_time / dt) + + # Define a base state vector (13 elements: pos, quat, vel, ang_vel) + base_vector = np.zeros(13) + base_vector[0:3] = np.array([0.0, 0.0, 0.0]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion + base_vector[7:10] = np.array([0.1, 0.0, 0.0]) # small velocity in x + base_vector[10:13] = np.array([0.0, 0.0, 0.0]) + + # Define initial covariance for state (33x33) + P0 = np.eye(33) + P0[0:3, 0:3] = np.eye(3) * 0.01 # position + P0[3:6, 3:6] = np.eye(3) * 0.01 # orientation error (quaternion) + P0[6:9, 6:9] = np.eye(3) * 0.01 # velocity + P0[9:12, 9:12] = np.eye(3) * 0.01 # angular velocity + P0[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters + + # Define process noise covariance Q (33x33) + Q = np.zeros((33, 33)) + Q[0:3, 0:3] = np.eye(3) * 0.001 # for position + Q[3:6, 3:6] = ( + np.eye(3) * 0.001 + ) # for orientation error (represented with Euler angles) + Q[6:9, 6:9] = np.eye(3) * 0.001 # for velocity + Q[9:12, 9:12] = np.eye(3) * 0.001 # for angular velocity + Q[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters + + G = np.zeros((33, 12)) + G[0:3, 0:3] = np.eye(3) + G[3:6, 3:6] = np.eye(3) + G[6:9, 6:9] = np.eye(3) + G[9:12, 9:12] = np.eye(3) + + # Measurement noise covariance R (3x3), assume measurement is velocity + R = np.eye(3) * 0.01 + + # Create a simulation process model and an independent UKF process model. + sim_model = process_model() + sim_model.dt = dt + sim_model.mass_interia_matrix = np.array( + [ + [30.0, 0.0, 0.0, 0.0, 0.0, 0.6], + [0.0, 30.0, 0.0, 0.0, -0.6, 0.3], + [0.0, 0.0, 30.0, 0.6, 0.3, 0.0], + [0.0, 0.0, 0.6, 0.68, 0.0, 0.0], + [0.0, -0.6, 0.3, 0.0, 3.32, 0.0], + [0.6, 0.3, 0.0, 0.0, 0.0, 3.34], + ] + ) + sim_model.m = 30.0 + sim_model.r_b_bg = np.array([0.01, 0.0, 0.02]) + sim_model.inertia = np.diag([0.68, 3.32, 3.34]) + sim_model.damping_linear = np.array([0.1] * 6) + sim_model.added_mass = np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # UKF process model copy: + ukf_model = okid_process_model() + ukf_model.dt = dt + ukf_model.mass_interia_matrix = sim_model.mass_interia_matrix.copy() + ukf_model.m = sim_model.m + ukf_model.r_b_bg = sim_model.r_b_bg.copy() + ukf_model.inertia = sim_model.inertia.copy() + ukf_model.damping_linear = sim_model.damping_linear.copy() + ukf_model.added_mass = sim_model.added_mass.copy() + + # Initialize true state and filter state. + true_state = create_statequat( + base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) + ) + true_state.covariance = P0.copy() + + filter_state = create_statequat( + base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) + ) + filter_state.covariance = P0.copy() + + # Initialize measurement model (for example, measuring velocity only) + meas_model = MeasModel() + meas_model.covariance = R.copy() + + # Initialize UKF. + ukf = UKF(ukf_model, true_state, P0.copy(), Q.copy(), G.copy()) + + # Arrays to store time histories. + pos_true_hist = np.zeros((num_steps, 3)) + pos_est_hist = np.zeros((num_steps, 3)) + vel_true_hist = np.zeros((num_steps, 3)) + vel_est_hist = np.zeros((num_steps, 3)) + euler_true_hist = np.zeros((num_steps, 3)) + euler_est_hist = np.zeros((num_steps, 3)) + time_array = np.linspace(0, simulation_time, num_steps) + + # Control input function (example: oscillatory in all directions) + def control_input(t): + return np.array( + [ + 2 * np.sin(t), + 2 * np.sin(t + 0.5), + 2 * np.sin(t + 1.0), + 0.2 * np.cos(t), + 0.2 * np.cos(t + 0.5), + 0.2 * np.cos(t + 1.0), + ] + ) + + # Set previous states. + sim_model.state_vector_prev = true_state + sim_model.state_vector = true_state + ukf_model.state_vector_prev = filter_state + ukf_model.state_vector = filter_state + + # Lists for timing diagnostics. + ukf_transform_times = [] + ukf_update_times = [] + + # Simulation loop. + for i in range(num_steps): + t_current = i * dt + + # Update control inputs. + sim_model.Control_input = control_input(t_current) + ukf_model.Control_input = control_input(t_current) + + # Propagate true state using the simulation model. + sim_model.model_prediction(true_state) + true_state = sim_model.euler_forward() + + # Create a measurement from true state. + # Here we assume we measure velocity plus noise. + meas_noise = np.random.normal(0, 0.01, 3) + meas_model.measurement = true_state.velocity + meas_noise + + # UKF prediction: unscented transform. + start = time.time() + filter_state = ukf.unscented_transform(filter_state) + ukf_transform_times.append(time.time() - start) + + # UKF measurement update every few steps. + if i % 5 == 0: + try: + start = time.time() + ukf.measurement_update(filter_state, meas_model) + filter_state = ukf.posteriori_estimate(filter_state, meas_model) + ukf_update_times.append(time.time() - start) + except np.linalg.LinAlgError: + # If matrix is not PD, add jitter. + filter_state.covariance += ( + np.eye(filter_state.covariance.shape[0]) * 1e-6 + ) + + # Store true and estimated state for diagnostics. + pos_true_hist[i, :] = true_state.position + pos_est_hist[i, :] = filter_state.position + vel_true_hist[i, :] = true_state.velocity + vel_est_hist[i, :] = filter_state.velocity + # Convert quaternion to Euler angles for visualization. + # Assumes you have a function quat_to_euler. + euler_true_hist[i, :] = quat_to_euler(true_state.orientation) + euler_est_hist[i, :] = quat_to_euler(filter_state.orientation) + + # Update previous states. + sim_model.state_vector_prev = true_state + ukf_model.state_vector_prev = filter_state + + # Print timing diagnostics. + print("Average unscented transform time:", np.mean(ukf_transform_times)) + print("Average measurement update time:", np.mean(ukf_update_times)) + + # Compute error metrics. + pos_error = np.linalg.norm(pos_true_hist - pos_est_hist, axis=1) + vel_error = np.linalg.norm(vel_true_hist - vel_est_hist, axis=1) + euler_error = np.linalg.norm(euler_true_hist - euler_est_hist, axis=1) + print("Average position error:", np.mean(pos_error)) + print("Average velocity error:", np.mean(vel_error)) + print("Average orientation (Euler) error:", np.mean(euler_error)) + + # Plot estimated vs true trajectory (positions). + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_array, pos_true_hist[:, 0], label="True X") + plt.plot(time_array, pos_est_hist[:, 0], label="Est X", linestyle="--") + plt.legend() + plt.title("Position X") + + plt.subplot(3, 1, 2) + plt.plot(time_array, pos_true_hist[:, 1], label="True Y") + plt.plot(time_array, pos_est_hist[:, 1], label="Est Y", linestyle="--") + plt.legend() + plt.title("Position Y") + + plt.subplot(3, 1, 3) + plt.plot(time_array, pos_true_hist[:, 2], label="True Z") + plt.plot(time_array, pos_est_hist[:, 2], label="Est Z", linestyle="--") + plt.legend() + plt.title("Position Z") + plt.tight_layout() + plt.show() + + # Plot errors. + plt.figure(figsize=(10, 4)) + plt.plot(time_array, pos_error, label="Position Error") + plt.plot(time_array, vel_error, label="Velocity Error") + plt.plot(time_array, euler_error, label="Euler Angle Error") + plt.legend() + plt.title("Error Metrics over Time") + plt.xlabel("Time (s)") + plt.ylabel("Error magnitude") + plt.show() + + +# You can also test the individual statistics functions separately: +def run_diagnostics(): + print("Testing state mean and covariance computation:") + # Call your pre-written tests: + # (Assuming these functions—test_state_statistics, test_measurement_statistics, test_cross_covariance—are defined above) + test_state_statistics() + print("\nTesting measurement mean and covariance computation:") + test_measurement_statistics() + print("\nTesting cross-covariance computation:") + test_cross_covariance() + + +if __name__ == '__main__': + # First, run the diagnostics on the mean/covariance functions. + run_diagnostics() + + # Then run the full UKF simulation test. + print("\nRunning full UKF simulation test:") + run_ukf_simulation() diff --git a/navigation/ukf_okid/ukf_python/ukf_test_2.py b/navigation/ukf_okid/ukf_python/ukf_test_2.py new file mode 100644 index 000000000..d3a75313b --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -0,0 +1,42 @@ +import numpy as np + +# Define process noise covariance Q (33x33) +Q = np.zeros((12, 12)) +Q[0:3, 0:3] = np.eye(3) * 0.003 # for position +Q[3:6, 3:6] = np.eye(3) * 0.003 # for orientation error (represented with Euler angles) +Q[6:9, 6:9] = np.eye(3) * 0.002 # for velocity +Q[9:12, 9:12] = np.eye(3) * 0.003 # for angular velocity + +G = np.zeros((33, 12)) +G[0:3, 0:3] = np.eye(3) +G[3:6, 3:6] = np.eye(3) +G[6:9, 6:9] = np.eye(3) +G[9:12, 9:12] = np.eye(3) + +GG = G @ Q @ G.T + + +def fancy_print_matrix(matrix, name="Matrix", precision=4): + """Print a matrix with fancy formatting. + + Args: + matrix: numpy array to print + name: name of the matrix to display + precision: number of decimal places to show + """ + print(f"\n{'=' * 50}") + print(f" {name} [{matrix.shape[0]}x{matrix.shape[1]}]") + print(f"{'=' * 50}") + + # Set numpy print options + with np.printoptions(precision=precision, suppress=True, linewidth=100): + # Print each row with custom formatting + for i in range(matrix.shape[0]): + row = ' '.join([f"{x:8.{precision}f}" for x in matrix[i]]) + print(f" {i:2d} | {row}") + + print(f"{'=' * 50}\n") + + +# Example usage: +fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py new file mode 100644 index 000000000..7bf7cd4e3 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -0,0 +1,35 @@ +import numpy as np +from ukf_okid_class import StateQuat + + +def print_StateQuat_list( + state_list: list[StateQuat], name="StateQuat List", print_covariance=True +): + """Custom print function to print a list of StateQuat objects in a formatted form.""" + print(f"{name}:") + for i, state in enumerate(state_list): + print(f"Index {i}:") + print_StateQuat(state, f"StateQuat {i}", print_covariance) + + +def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): + """Custom print function to print StateQuat objects in a formatted form.""" + print(f"{name}:") + print(f" Position: {state.position}") + print(f" Orientation: {state.orientation}") + print(f" Velocity: {state.velocity}") + print(f" Angular Velocity: {state.angular_velocity}") + print(f" okid state: {state.okid_params}") + # print(f" okid_params: {state.okid_params}") + if print_covariance: + print_matrix(state.covariance, "Covariance") + + +def print_matrix(matrix, name="Matrix"): + """Custom print function to print matrices in a formatted form.""" + print(f"{name}: {matrix.shape}") + if isinstance(matrix, np.ndarray): + for row in matrix: + print(" ".join(f"{val:.2f}" for val in row)) + else: + print(matrix)