From 17a7660a5ff6899e29362faf772e88d6f5cd95dc Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 9 Feb 2025 19:47:52 +0100 Subject: [PATCH 01/19] feat: added the math and setup of the ESKF, need some changes --- navigation/eskf_python/CMakeLists.txt | 33 ++ navigation/eskf_python/README.md | 0 .../eskf_python/config/eskf_python.yaml | 3 + .../eskf_python/eskf_python/__init__.py | 0 .../eskf_python/eskf_python_filter.py | 511 ++++++++++++++++++ .../eskf_python/eskf_python_node.py | 103 ++++ navigation/eskf_python/launch/eskf.launch.py | 22 + navigation/eskf_python/package.xml | 23 + 8 files changed, 695 insertions(+) create mode 100644 navigation/eskf_python/CMakeLists.txt create mode 100644 navigation/eskf_python/README.md create mode 100644 navigation/eskf_python/config/eskf_python.yaml create mode 100644 navigation/eskf_python/eskf_python/__init__.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_filter.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_node.py create mode 100644 navigation/eskf_python/launch/eskf.launch.py create mode 100644 navigation/eskf_python/package.xml diff --git a/navigation/eskf_python/CMakeLists.txt b/navigation/eskf_python/CMakeLists.txt new file mode 100644 index 000000000..b4fc9118c --- /dev/null +++ b/navigation/eskf_python/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf_python) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + eskf_python/eskf_python_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_package() diff --git a/navigation/eskf_python/README.md b/navigation/eskf_python/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/eskf_python/config/eskf_python.yaml b/navigation/eskf_python/config/eskf_python.yaml new file mode 100644 index 000000000..0d80b90df --- /dev/null +++ b/navigation/eskf_python/config/eskf_python.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + eskf_python_node: diff --git a/navigation/eskf_python/eskf_python/__init__.py b/navigation/eskf_python/eskf_python/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py new file mode 100644 index 000000000..286747467 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -0,0 +1,511 @@ +from dataclasses import dataclass, field +from typing import tuple + +import numpy as np + + +@dataclass +class StateVector_quaternion: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(4) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + 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 euler_forward( + self, current_state: 'StateVector_quaternion', dt: float + ) -> 'StateVector_quaternion': + # Define the new state + new_state = StateVector_quaternion() + + # Define the state derivatives + new_state.position = current_state.position + self.position * dt + new_state.velocity = current_state.velocity + self.velocity * dt + new_state.orientation = current_state.orientation + self.orientation * dt + new_state.acceleration_bias = ( + current_state.acceleration_bias + self.acceleration_bias * dt + ) + new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt + + # Normalize the orientation quaternion + new_state.orientation /= np.linalg.norm(new_state.orientation) + + return new_state + + +@dataclass +class StateVector_euler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((15, 15)) + ) # Covariance matrix + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + + def copy_state(self, wanted_state: 'StateVector_euler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) # Measurement vector + measurement_matrix: np.ndarray = field( + default_factory=lambda: np.zeros((6, 15)) + ) # Measurement matrix + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) # Measurement noise matrix + + +class ErrorStateKalmanFilter: + def __init__( + self, + P_ab: np.ndarray, + P_wb: np.ndarray, + Q: np.ndarray, + lever_arm: np.array, + R: np.ndarray, + g: float, + dt: float, + ) -> None: + self.P_ab = P_ab + self.P_wb = P_wb + self.Q_process_noise = Q + self.lever_arm = lever_arm + self.R = R + self.g = np.array([0, 0, g]) + self.dt = dt + + def skew_symmetric(self, 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(self, 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. + """ + nu_0, eta_0_x, eta_0_y, eta_0_z = q1 + nu_1, eta_1_x, eta_1_y, eta_1_z = q2 + + eta_0 = np.array([[eta_1_x, eta_1_y, eta_1_z]]).T + eta_1 = np.array([[eta_0_x, eta_0_y, eta_0_z]]).T + + eta_new = ( + nu_1 * eta_0 + nu_0 * eta_1 + np.dot(self.skew_symmetric(eta_0), eta_1) + ) + nu_new = nu_0 * nu_1 - np.dot(eta_0.T, eta_1) + + q_new = np.array([nu_new, eta_new[0], eta_new[1], eta_new[2]]) + q_new /= np.linalg.norm(q_new) + + return q_new + + def van_loan_discretization( + self, A_c: np.ndarray, G_c: np.ndarray + ) -> tuple[np.ndarray, np.ndarray]: + """Calculates the Van Loan discretization of a continuous-time system. + + Args: + A_c (np.ndarray): The A matrix. + G_c (np.ndarray): The G matrix. + + Returns: + tuple: The A_d and GQG_d matrices. + """ + GQG_T = np.dot(np.dot(G_c, self.Q_process_noise), G_c.T) * self.dt + + matrix_exp = ( + np.block([[A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), A_c.T]]) + * self.dt + ) + + van_loan_matrix = np.linalg.expm(matrix_exp) + + V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] + V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] + + A_d = V1.T + GQG_d = A_d @ V2 + + return A_d, GQG_d + + def nominal_state_update( + self, current_state: StateVector_quaternion, imu_reading: np.ndarray + ) -> StateVector_quaternion: + """Updates the nominal state of the system. + + Args: + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated nominal state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + # Define the derivative of the state + current_state_dot = StateVector_quaternion() + + # Define the state derivates + current_state_dot.position = current_state.velocity + current_state_dot.velocity = ( + np.dot( + current_state.R_q(), + (imu_acceleration - current_state.acceleration_bias), + ) + + self.g + ) + + # Define the quaternion derivatives + current_state_dot.orientation = 0.5 * self.quaternion_super_product( + current_state.orientation, + np.array([0, imu_gyro[0], imu_gyro[1], imu_gyro[2]]), + ) + + # Define the bias + current_state_dot.acceleration_bias = ( + -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias + ) + current_state_dot.gyro_bias = ( + -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias + ) + + return current_state_dot.euler_forward(current_state, self.dt) + + def error_state_update( + self, + current_error_state: StateVector_euler, + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated error state. + """ + # Define the derivative of the state + next_error_state = StateVector_euler() + + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + A_c = np.zeros((15, 15)) + A_c[0:3, 3:6] = np.eye(3) + A_c[3:6, 6:9] = -np.dot( + current_state.R_q(), + self.skew_symmetric(imu_acceleration - current_state.acceleration_bias), + ) + A_c[6:9, 6:9] = -self.skew_symmetric(imu_gyro - current_state.gyro_bias) + A_c[3:6, 9:12] = -current_state.R_q() + A_c[6:9, 12:15] = -np.eye(3) + A_c[9:12, 9:12] = -self.P_ab * np.eye(3) + A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + + G_c = np.zeros((15, 12)) + G_c[3:6, 0:3] = -current_state.R_q() + G_c[6:9, 3:6] = -np.eye(3) + G_c[9:12, 6:9] = np.eye(3) + G_c[12:15, 9:12] = np.eye(3) + + # Van loan discretization + A_d, GQG_d = self.van_loan_discretization(A_c, G_c, self.dt) + + # Inserting the new state and covariance + next_error_state.copy_state(current_error_state) + next_error_state.covariance = ( + np.dot(np.dot(A_d, current_error_state.covariance), A_d.T) + GQG_d + ) + + return next_error_state + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + # Define the measurement matrix + H = np.zeros((3, 15)) + + # For now assume only velocity is measured + H[0:3, 3:6] = np.eye(3) + + return H + + def prediction_from_estimates( + self, + current_state: StateVector_quaternion, + current_error_state: StateVector_euler, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Predicts the measurement from the current state and error state. + + Args: + current_state (StateVector_quaternion): The current state of the system. + current_error_state (StateVector_euler): The current error state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + StateVector_euler: The predicted measurement. + """ + # Define the z_pred matrix + z_pred = MeasurementModel() + + # Define the z_pred values separately + z_pred_1 = np.hstack((current_state.position, current_state.velocity)) + z_pred_2 = np.hstack( + np.dot(current_state.R_q(), self.lever_arm), + np.dot( + current_state.R_q, + np.dot( + self.skew_symmetric(current_state.angular_velocity), self.lever_arm + ), + ), + ) + + # Combine the z_pred values + z_pred.measurement = z_pred_1 + z_pred_2 + + # Define the H matrix + z_pred.measurement_matrix = self.H() + R = self.R + z_pred.measurement_covariance = ( + np.dot( + np.dot(z_pred.measurement_matrix, current_error_state.covariance), + z_pred.measurement_matrix.T, + ) + + R + ) + + return z_pred + + def measurement_update( + self, + error_state_pred: StateVector_euler, + z_pred: MeasurementModel, + dvl_measure: np.array, + ) -> StateVector_euler: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + measurement (np.ndarray): The measurement. + + Returns: + np.ndarray: The updated error state. + """ + # Define new error state value + new_error_state = StateVector_euler() + + # Define the measurement matrix + innovation = dvl_measure - z_pred.measurement + H = z_pred.measurement_matrix + R = self.R + P = error_state_pred.covariance + S = z_pred.measurement_covariance + + # Kalman gain calculation + W = np.dot(P, np.linalg.solve(S, H).T) + new_error_state.fill_states(np.dot(W, innovation)) + + I_WH = np.eye(15) - np.dot(W, H) + new_error_state.covariance = np.dot(np.dot(I_WH, P), I_WH.T) + np.dot( + np.dot(W, R), W.T + ) + + return new_error_state + + def imu_update_states( + self, + current_pred_nom: StateVector_quaternion, + current_pred_err: StateVector_euler, + imu_readings: np.array, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Calculates the predicted state using the IMU readings. + + Args: + current_pred_nom (StateVector_quaternion): The current nominal state. + current_pred_err (StateVector_euler): The current error state. + imu_readings (np.array): The IMU readings. + + Returns: + tuple: The predicted nominal state and the predicted error state. + """ + pred_nom_state = self.nominal_state_update(current_pred_nom, imu_readings) + pred_err_state = self.error_state_update( + current_pred_err, current_pred_nom, imu_readings + ) + + return pred_nom_state, pred_err_state + + def dvl_update_states( + self, + current_pred_nom: StateVector_quaternion, + current_pred_err: StateVector_euler, + dvl_measure: np.array, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Calculates the predicted state using the DVL readings. + + Args: + current_pred_nom (StateVector_quaternion): The current nominal state. + current_pred_err (StateVector_euler): The current error state. + dvl_measure (np.array): The DVL readings. + + Returns: + tuple: The predicted nominal state and the predicted error state. + """ + z_pred = self.prediction_from_estimates( + current_pred_nom, current_pred_err, dvl_measure + ) + new_error_state = self.measurement_update(current_pred_err, z_pred, dvl_measure) + + return current_pred_nom, new_error_state + + def injection_and_reset( + self, next_state: StateVector_quaternion, next_error_state: StateVector_euler + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Injects the error state into the nominal state and resets the error state. + + Args: + next_state (StateVector_quaternion): The next nominal state. + next_error_state (StateVector_euler): The next error state. + + Returns: + tuple: The injected nominal state and the reset error state. + """ + # Define the new state + inj_state = StateVector_quaternion() + + # Injecting the error state + inj_state.position = next_state.position + next_error_state.position + inj_state.velocity = next_state.velocity + next_error_state.velocity + inj_state.orientation = self.quaternion_super_product( + next_state.orientation, + 0.5 + * np.array( + [ + 2, + next_error_state.orientation[0], + next_error_state.orientation[1], + next_error_state.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + next_state.acceleration_bias + next_error_state.acceleration_bias + ) + inj_state.gyro_bias = next_state.gyro_bias + next_error_state.gyro_bias + + # Resetting the error state + G = np.eye(15) + G[6:9, 6:9] = np.eye(3) - self.skew_symmetric( + 0.5 * next_error_state.orientation + ) + + next_error_state.covariance = np.dot( + np.dot(G, next_error_state.covariance), G.T + ) + next_error_state.fill_states(np.zeros(15)) + + return inj_state, next_error_state diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py new file mode 100644 index 000000000..25e5b6ca9 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data + +# NEED TO CHANGE THIS TO THE CORRECT PATH +from eskf_python.eskf_python_filter import ( + ErrorStateKalmanFilter, + MeasurementModel, + StateVector_euler, + StateVector_quaternion, +) + +qos_profile = QoSProfile( + depth=1, + history=qos_profile_sensor_data.history, + reliability=qos_profile_sensor_data.reliability, +) + + +class ESKalmanFilterNode(Node): + def __init__(self): + super().__init__("eskf_python_node") + + # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz + # TEMPORARILY ADDED FOR TESTING + self.imu_subscriber_ = self.create_subscription( + Odometry, '/orca/imu', self.state_callback, qos_profile=qos_profile + ) + + # This publisher will publish the estimtaed state of the vehicle + self.state_publisher_ = self.create_publisher( + Odometry, '/orca/odom', qos_profile=qos_profile + ) + + self.eskf_modual = ErrorStateKalmanFilter() + self.current_state_nom = StateVector_quaternion() + self.current_state_error = StateVector_euler() + self.measurement_pred = MeasurementModel() + + self.get_logger().info("hybridpath_controller_node started") + + def imu_callback(self, msg: Odometry): + self.get_logger().info(f"Received IMU message: {msg}") + + # Get the IMU data + + # SOME CONVERSION HERE TO SUITABLE TYPE + imu_data = "something" + + # Update the filter with the IMU data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.imu_update_states( + self.current_state_nom, self.current_state_error, imu_data + ) + ) + + # Publish the estimated state + """ + Some conversion function from the custom state type to odometry message + This needs to be worked on + """ + + def filter_callback(self): + """Callback function for the filter measurement update, + this will be called when the filter needs to be updated with the DVL data. + """ + self.get_logger().info("Filter callback, got DVL data") + + # Get the DVL data + dvl_data = "something" + + # Update the filter with the DVL data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.dvl_update_states( + self.current_state_nom, self.current_state_error, dvl_data + ) + ) + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.injection_and_reset( + self.current_state_nom, self.current_state_error + ) + ) + + # Publish the estimated state + """ + Some conversion function from the custom state type to odometry message + This needs to be worked on + """ + + +def main(args=None): + rclpy.init(args=args) + node = ESKalmanFilterNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/navigation/eskf_python/launch/eskf.launch.py b/navigation/eskf_python/launch/eskf.launch.py new file mode 100644 index 000000000..3cae83dce --- /dev/null +++ b/navigation/eskf_python/launch/eskf.launch.py @@ -0,0 +1,22 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + eskf_python_node = Node( + package='eskf_python', + executable='eskf_python_node.py', + name='eskf_python_node', + parameters=[ + os.path.join( + get_package_share_directory('eskf_python'), + 'config', + 'eskf_python.yaml', + ), + ], + output='screen', + ) + return LaunchDescription([eskf_python_node]) diff --git a/navigation/eskf_python/package.xml b/navigation/eskf_python/package.xml new file mode 100644 index 000000000..980653c40 --- /dev/null +++ b/navigation/eskf_python/package.xml @@ -0,0 +1,23 @@ + + + + eskf_python + 1.0.0 + This package provides the implementation of a error-state kalman filter in python + talhanc + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + + ament_cmake + + From dff90b70400a3709533f639795d62dbc55182156 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Mon, 10 Feb 2025 14:28:18 +0100 Subject: [PATCH 02/19] feat: added in the IMU msg and DVL msg, pluss ros2 setup --- .../eskf_python/eskf_python_filter.py | 12 +-- .../eskf_python/eskf_python_node.py | 74 ++++++++++++++----- 2 files changed, 56 insertions(+), 30 deletions(-) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 286747467..392c705c2 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -353,16 +353,8 @@ def prediction_from_estimates( z_pred = MeasurementModel() # Define the z_pred values separately - z_pred_1 = np.hstack((current_state.position, current_state.velocity)) - z_pred_2 = np.hstack( - np.dot(current_state.R_q(), self.lever_arm), - np.dot( - current_state.R_q, - np.dot( - self.skew_symmetric(current_state.angular_velocity), self.lever_arm - ), - ), - ) + z_pred_1 = current_state.velocity + z_pred_2 = 0 # Currently assuming no lever arm compensation # Combine the z_pred values z_pred.measurement = z_pred_1 + z_pred_2 diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index 25e5b6ca9..5b860582e 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -4,6 +4,9 @@ from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data +from sensor_msgs.msg import Imu, +import numpy as np +from geometry_msgs.msg import TwistWithCovarianceStamped # NEED TO CHANGE THIS TO THE CORRECT PATH from eskf_python.eskf_python_filter import ( @@ -25,9 +28,12 @@ def __init__(self): super().__init__("eskf_python_node") # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - # TEMPORARILY ADDED FOR TESTING self.imu_subscriber_ = self.create_subscription( - Odometry, '/orca/imu', self.state_callback, qos_profile=qos_profile + Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile + ) + + self.twist_dvl_subscriber_ = self.create_subscription( + TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile ) # This publisher will publish the estimtaed state of the vehicle @@ -39,16 +45,19 @@ def __init__(self): self.current_state_nom = StateVector_quaternion() self.current_state_error = StateVector_euler() self.measurement_pred = MeasurementModel() + self.odom_msg = Odometry() - self.get_logger().info("hybridpath_controller_node started") + self.get_logger().info("Error State Kalman Filter started") - def imu_callback(self, msg: Odometry): - self.get_logger().info(f"Received IMU message: {msg}") + def imu_callback(self, msg: Imu): # Get the IMU data - # SOME CONVERSION HERE TO SUITABLE TYPE - imu_data = "something" + imu_acceleartion = msg.linear_acceleration + imu_angular_velocity = msg.angular_velocity + + # Combine the IMU data + imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) # Update the filter with the IMU data self.current_state_nom, self.current_state_error = ( @@ -57,20 +66,34 @@ def imu_callback(self, msg: Odometry): ) ) - # Publish the estimated state - """ - Some conversion function from the custom state type to odometry message - This needs to be worked on - """ + # Inserting the nominal state into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x + self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y + self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z + + # Publish + self.state_publisher_.publish(self.odom_msg) - def filter_callback(self): + + + def filter_callback(self, msg: TwistWithCovarianceStamped): """Callback function for the filter measurement update, this will be called when the filter needs to be updated with the DVL data. """ self.get_logger().info("Filter callback, got DVL data") - # Get the DVL data - dvl_data = "something" + # Get the DVL data (linear velocity) + dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) # Update the filter with the DVL data self.current_state_nom, self.current_state_error = ( @@ -84,11 +107,22 @@ def filter_callback(self): ) ) - # Publish the estimated state - """ - Some conversion function from the custom state type to odometry message - This needs to be worked on - """ + # Inserting data into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + + # Publishing the data + self.state_publisher_.publish(self.odom_msg) + def main(args=None): From d428dfbff1bb10778d790fbc2da87156efe1d312 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 21 Feb 2025 17:07:58 +0100 Subject: [PATCH 03/19] feat: added ES-UKF filter --- .../eskf_python/eskf_python_filter.py | 4 +- navigation/sp_ukf_python/CMakeLists.txt | 33 ++ navigation/sp_ukf_python/README.md | 0 .../sp_ukf_python/config/sp_ukf_python.yaml | 3 + navigation/sp_ukf_python/launch/ukf.launch.py | 22 + navigation/sp_ukf_python/package.xml | 23 + .../sp_ukf_python/sp_ukf_python/__init__.py | 0 .../sp_ukf_python/sp_ukf_python.py | 440 ++++++++++++++++++ .../sp_ukf_python/sp_ukf_python_class.py | 280 +++++++++++ .../sp_ukf_python/sp_ukf_python_node.py | 137 ++++++ .../sp_ukf_python/sp_ukf_python_utils.py | 107 +++++ .../sp_ukf_python/sp_ukf_python/test_ukf.py | 218 +++++++++ 12 files changed, 1265 insertions(+), 2 deletions(-) create mode 100644 navigation/sp_ukf_python/CMakeLists.txt create mode 100644 navigation/sp_ukf_python/README.md create mode 100644 navigation/sp_ukf_python/config/sp_ukf_python.yaml create mode 100644 navigation/sp_ukf_python/launch/ukf.launch.py create mode 100644 navigation/sp_ukf_python/package.xml create mode 100644 navigation/sp_ukf_python/sp_ukf_python/__init__.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/test_ukf.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 392c705c2..edf39dcc0 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -2,7 +2,7 @@ from typing import tuple import numpy as np - +from scipy.linalg import expm @dataclass class StateVector_quaternion: @@ -212,7 +212,7 @@ def van_loan_discretization( * self.dt ) - van_loan_matrix = np.linalg.expm(matrix_exp) + van_loan_matrix = expm(matrix_exp) V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] diff --git a/navigation/sp_ukf_python/CMakeLists.txt b/navigation/sp_ukf_python/CMakeLists.txt new file mode 100644 index 000000000..a40f065cd --- /dev/null +++ b/navigation/sp_ukf_python/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(sp_ukf_python) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + sp_ukf_python/sp_ukf_python_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_package() diff --git a/navigation/sp_ukf_python/README.md b/navigation/sp_ukf_python/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/sp_ukf_python/config/sp_ukf_python.yaml b/navigation/sp_ukf_python/config/sp_ukf_python.yaml new file mode 100644 index 000000000..d3d18145d --- /dev/null +++ b/navigation/sp_ukf_python/config/sp_ukf_python.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + sp_ukf_python_node: diff --git a/navigation/sp_ukf_python/launch/ukf.launch.py b/navigation/sp_ukf_python/launch/ukf.launch.py new file mode 100644 index 000000000..fdd3f07e6 --- /dev/null +++ b/navigation/sp_ukf_python/launch/ukf.launch.py @@ -0,0 +1,22 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + sp_ukf_python_node = Node( + package='sp_ukf_python', + executable='sp_ukf_python_node.py', + name='sp_ukf_python_node', + parameters=[ + os.path.join( + get_package_share_directory('sp_ukf_python'), + 'config', + 'sp_ukf_python.yaml', + ), + ], + output='screen', + ) + return LaunchDescription([sp_ukf_python_node]) diff --git a/navigation/sp_ukf_python/package.xml b/navigation/sp_ukf_python/package.xml new file mode 100644 index 000000000..6aa4edbc0 --- /dev/null +++ b/navigation/sp_ukf_python/package.xml @@ -0,0 +1,23 @@ + + + + sp_ukf_python + 1.0.0 + This package provides the implementation of a sigma point based Unscented Error-state Kalman Filter + talhanc + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + + ament_cmake + + diff --git a/navigation/sp_ukf_python/sp_ukf_python/__init__.py b/navigation/sp_ukf_python/sp_ukf_python/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py new file mode 100644 index 000000000..6f9d1b2f6 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -0,0 +1,440 @@ +from dataclasses import dataclass +from typing import Tuple +from sp_ukf_python_class import StateVector_quaternion, StateVector_euler +from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from scipy.linalg import expm + +import numpy as np + +class ErrorStateUnscentedKalmanFilter: + def __init__( + self, + P_ab: float, + P_wb: float, + Q: np.ndarray, + lever_arm: np.array, + R: np.ndarray, + g: float, + dt: float, + ) -> None: + self.P_ab = P_ab + self.P_wb = P_wb + self.Q_process_noise = Q + self.lever_arm = lever_arm + self.R = R + self.g = np.array([0, 0, g]) + self.dt = dt + self.y_i = np.zeros((15, 2*15)) + + def mean_set(self, set: np.ndarray) -> np.ndarray: + """ + Calculates the mean of a set of values. + + Args: + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The mean of the set. + """ + # Define the number of columns + n = set.shape[0] + + # Calculate the mean value + mean_value = (1 / (2 * n)) * np.sum(set, axis=1) + + return mean_value + + def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + """ + Calculates the mean of a set of values. + + Args: + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The mean of the set. + """ + # Define the number of columns + n = set.shape[0] + mean_value = np.zeros(n) + + for i in range(2*n + 1): + mean_value += weight[i] * set[:, i] + + mean_value = (1 / (2 * n + 1)) * mean_value + + return mean_value + + def covariance_set(self, mean: np.ndarray, set: np.ndarray, mean_2: np.ndarray = None, set_2: np.ndarray = None) -> np.ndarray: + """ + Calculate the covarince of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The covariance of the set. + """ + + if mean_2 is not None: + + n = set.shape[0] + n2 = set_2.shape[0] + covariance_set = np.zeros((n, n2)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + vector_2 = StateVector_euler() + vector_2.position = set_2[:, i][:3] + vector_2.velocity = set_2[:, i][3:6] + vector_2.orientation = set_2[:, i][6:9] + vector_2.acceleration_bias = set_2[:, i][9:12] + vector_2.gyro_bias = set_2[:, i][12:] + W_i = vector - mean + W_i_2 = vector_2 - mean_2 + + covariance_set += (1 / (2*n)) * np.outer(W_i, W_i_2) + + return covariance_set + else: + + n = set.shape[0] + covariance_set = np.zeros((n, n)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + covariance_set += (1 / (2*n)) * np.outer(W_i, W_i) + + return covariance_set + + def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + """ + Calculate the covarince of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The covariance of the set. + """ + + n = set.shape[0] + covariance_set = np.zeros((n, n)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + covariance_set += weight[i] * np.outer(W_i, W_i) + + return covariance_set + + + + def generate_sigma_points(self, error_state: StateVector_euler, Q_process_noise) -> tuple[list[StateVector_euler], np.ndarray]: + """ + Generates the sigma points for the UKF + This is done using the Cholesky decomposition method + """ + + # Define n + n = len(error_state.covariance) + kappa = 3 - n + + # Computing S matrix using cholensky decomposition + S = np.linalg.cholesky(error_state.covariance + Q_process_noise) + + S_scaled = np.sqrt(n + kappa) * S + + weighted_points = np.concatenate((S_scaled , -S_scaled), axis=1) + + sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] + + sigma_points[0].fill_states(error_state.as_vector()) + for i in range(2*n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:,i]) + + W = np.zeros(2*n + 1) + W[0] = kappa / (n + kappa) + + for i in range(2*n): + W[i + 1] = 1 / (2 * (n + kappa)) + + return sigma_points, W + + def nominal_state_update( + self, current_state: StateVector_quaternion, imu_reading: np.ndarray + ) -> StateVector_quaternion: + """Updates the nominal state of the system. + + Args: + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated nominal state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + # Define the derivative of the state + current_state_dot = StateVector_quaternion() + + # Define the state derivates + current_state_dot.position = current_state.velocity + current_state_dot.velocity = ( + np.dot( + current_state.R_q(), + (imu_acceleration - current_state.acceleration_bias), + ) + + self.g + ) + + # Define the quaternion derivatives + current_state_dot.orientation = 0.5 * quaternion_super_product( + current_state.orientation, + np.array([0, imu_gyro[0] - current_state.gyro_bias[0], imu_gyro[1] - current_state.gyro_bias[1], imu_gyro[2] - current_state.gyro_bias[2]]), + ) + + # Define the bias + current_state_dot.acceleration_bias = ( + -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias + ) + current_state_dot.gyro_bias = ( + -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias + ) + + return current_state_dot.euler_forward(current_state, self.dt) + + def error_state_update( + self, + current_error_state: StateVector_euler, + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> np.ndarray: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated error state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + A_c = np.zeros((15, 15)) + A_c[0:3, 3:6] = np.eye(3) + A_c[3:6, 6:9] = -np.dot( + current_state.R_q(), + skew_symmetric(imu_acceleration - current_state.acceleration_bias), + ) + A_c[6:9, 6:9] = -skew_symmetric(imu_gyro - current_state.gyro_bias) + A_c[3:6, 9:12] = -current_state.R_q() + A_c[6:9, 12:15] = -np.eye(3) + A_c[9:12, 9:12] = -self.P_ab * np.eye(3) + A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + + # Exact matrix exponential + A_d = expm(A_c * self.dt) + + next_error_state = A_d @ current_error_state.as_vector() + + return next_error_state + + def unscented_transform(self, sigma_points: list[StateVector_euler], current_state: StateVector_quaternion, + imu_reading: np.ndarray,) -> StateVector_euler: + """ + Performs the Unscented Transform + This is the corresponding to a preditction step in the EKF + """ + + n = len(sigma_points[0].as_vector()) + + self.y_i = np.zeros((n, 2*n)) + + for i in range(2*n): + self.y_i[:, i] = self.error_state_update(sigma_points[i], current_state, imu_reading) + + error_state_estimate = StateVector_euler() + + x = self.mean_set(self.y_i) + + error_state_estimate.fill_states(x) + error_state_estimate.covariance = self.covariance_set(x, self.y_i) + + return error_state_estimate + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + # Define the measurement matrix + H = np.zeros((3, 15)) + + # For now assume only velocity is measured + H[0:3, 3:6] = np.eye(3) + + return H + + def measurement_update(self, sigma_points: list[StateVector_euler], current_error_state: StateVector_euler, dvl_data: np.ndarray, Weight: np.ndarray) -> StateVector_euler: + """ + Updates the state vector with the DVL data + """ + + H = self.H() + R = self.R + + n = len(sigma_points[0].as_vector()) + + Z_i = np.zeros((H.shape[0], 2 * n)) + + for i in range(2*n): + Z_i[:, i] = np.dot(H, sigma_points[i].as_vector()) + + z = self.weighted_mean_set(Z_i, Weight) + S = self.weighted_covariance_set(z, Z_i, Weight) + + x = self.mean_set(self.y_i) + + # Calculate the rest + innovation = dvl_data - z + + P_innovation = S + R + + P_xz = self.covariance_set(x, self.y_i, z, Z_i) + + # Kalman gain + K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) + + updated_error_state = StateVector_euler() + + # Update the state + updated_error_state.fill_states(x + np.dot(K_k, innovation)) + + # Update the covariance + updated_error_state.covariance = current_error_state.covariance - np.dot(K_k, np.dot(P_innovation, K_k.T)) + + return updated_error_state + + def imu_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, imu_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Updates the state vector with the IMU data + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + imu_data (np.ndarray): The IMU data + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + + """ + + # Update the nominal state + current_state_nom = self.nominal_state_update(current_state_nom, imu_data) + + # Generate the sigma points + sigma_points, _ = self.generate_sigma_points(current_state_error, self.Q_process_noise) + + # Update the error state + current_state_error = self.unscented_transform(sigma_points, current_state_nom, imu_data) + + return current_state_nom, current_state_error + + def dvl_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Update the error state given the DVL data + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + dvl_data (np.ndarray): The DVL data to update the state with + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + """ + + # Generate the sigma points + sigma_points, weight = self.generate_sigma_points(current_state_error, self.Q_process_noise) + + # Update the error state + current_state_error = self.measurement_update(sigma_points, current_state_error, dvl_data, weight) + + return current_state_nom, current_state_error + + def inject_and_reset(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Injects the error state into the nominal state and resets the error state + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + """ + + inj_state = StateVector_quaternion() + + inj_state.position = current_state_nom.position + current_state_error.position + inj_state.velocity = current_state_nom.velocity + current_state_error.velocity + inj_state.orientation = quaternion_super_product( + current_state_nom.orientation, + 0.5 + * np.array( + [ + 2, + current_state_error.orientation[0], + current_state_error.orientation[1], + current_state_error.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + current_state_nom.acceleration_bias + current_state_error.acceleration_bias + ) + inj_state.gyro_bias = current_state_nom.gyro_bias + current_state_error.gyro_bias + + + # Resetting the error state + G = np.eye(15) + G[6:9, 6:9] = np.eye(3) - skew_symmetric( + 0.5 * current_state_error.orientation + ) + + current_state_error.covariance = np.dot( + np.dot(G, current_state_error.covariance), G.T + ) + + current_state_error.fill_states(np.zeros(15)) + + + return inj_state, current_state_error + diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py new file mode 100644 index 000000000..629ce460e --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -0,0 +1,280 @@ +import numpy as np +from dataclasses import dataclass, field +from sp_ukf_python_utils import quaternion_super_product, quaternion_error, euler_rotation_quaternion, ssa + +@dataclass +class StateVector_quaternion: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(4) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + + def as_vector(self) -> np.ndarray: + """Calculates the state vector. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + if len(state) == 15: + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:10] + self.acceleration_bias = state[10:13] + self.gyro_bias = state[13:] + else: + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = euler_rotation_quaternion(state[6:9]) + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:] + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + 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 euler_forward( + self, current_state: 'StateVector_quaternion', dt: float + ) -> 'StateVector_quaternion': + # Define the new state + new_state = StateVector_quaternion() + + # Define the state derivatives + new_state.position = current_state.position + self.position * dt + new_state.velocity = current_state.velocity + self.velocity * dt + new_state.orientation = current_state.orientation + self.orientation * dt + new_state.acceleration_bias = ( + current_state.acceleration_bias + self.acceleration_bias * dt + ) + new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt + + # Normalize the orientation quaternion + new_state.orientation /= np.linalg.norm(new_state.orientation) + + return new_state + + def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: + """Subtracts two StateVector_quaternion objects. + + Args: + other (StateVector_quaternion): The other StateVector_quaternion object. + + Returns: + np.ndarray: The difference between the two StateVector_quaternion objects. + """ + position_diff = self.position - other.position + velocity_diff = self.velocity - other.velocity + orientation_diff = quaternion_error(self.orientation, other.orientation) + acceleration_bias_diff = self.acceleration_bias - other.acceleration_bias + gyro_bias_diff = self.gyro_bias - other.gyro_bias + + return np.concatenate( + [ + position_diff, + velocity_diff, + orientation_diff, + acceleration_bias_diff, + gyro_bias_diff, + ] + ) + + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': + """Adds a numpy array to this StateVector_quaternion. + + Args: + other (np.ndarray): The numpy array to add. + + Returns: + np.ndarray: The result of the addition. + """ + # Construct the quaternion from the array + add_to_position = other[:3] + add_to_orientation = euler_rotation_quaternion(other[6:10]) + + new_position = self.position + add_to_position + new_velcoity = self.velocity + other[3:6] + new_orientation = quaternion_super_product(self.orientation, add_to_orientation) + new_acceleration_bias = self.acceleration_bias + other[10:13] + new_gyro_bias = self.gyro_bias + other[13:] + + return np.concatenate( + [ + new_position, + new_velcoity, + new_orientation, + new_acceleration_bias, + new_gyro_bias, + ] + ) + + +@dataclass +class StateVector_euler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((15, 15)) + ) # Covariance matrix + + def as_vector(self) -> np.ndarray: + """Calculates the state estimate vector. + + Returns: + np.ndarray: The state estimate vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + + def copy_state(self, wanted_state: 'StateVector_euler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': + """Adds a numpy array to this StateVector_quaternion. + + Args: + other (np.ndarray): The numpy array to add. + + Returns: + np.ndarray: The result of the addition. + """ + + new_position = self.position + other[:3] + new_velcoity = self.velocity + other[3:6] + new_orientation = self.orientation + other[6:9] + new_acceleration_bias = self.acceleration_bias + other[9:12] + new_gyro_bias = self.gyro_bias + other[12:] + + return np.concatenate( + [ + new_position, + new_velcoity, + new_orientation, + new_acceleration_bias, + new_gyro_bias, + ] + ) + + def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': + """ + Subtracts two StateVector_euler objects. + + Args: + other (StateVector_euler): The other StateVector_euler object. + + Returns: + StateVector_euler: The difference between the two StateVector_euler objects. + """ + position_diff = self.position - other_state[:3] + velocity_diff = self.velocity - other_state[3:6] + orientation_diff = ssa(self.orientation - other_state[6:9]) + acceleration_bias_diff = self.acceleration_bias - other_state[9:12] + gyro_bias_diff = self.gyro_bias - other_state[12:] + + return np.concatenate( + (position_diff, velocity_diff, orientation_diff, acceleration_bias_diff, gyro_bias_diff) + ) + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) # Measurement vector + measurement_matrix: np.ndarray = field( + default_factory=lambda: np.zeros((6, 15)) + ) # Measurement matrix + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) # Measurement noise matrix \ No newline at end of file diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py new file mode 100644 index 000000000..103528ef2 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data +from sensor_msgs.msg import Imu, +import numpy as np +from geometry_msgs.msg import TwistWithCovarianceStamped + +# NEED TO CHANGE THIS TO THE CORRECT PATH +from eskf_python.eskf_python_filter import ( + ErrorStateKalmanFilter, + MeasurementModel, + StateVector_euler, + StateVector_quaternion, +) + +qos_profile = QoSProfile( + depth=1, + history=qos_profile_sensor_data.history, + reliability=qos_profile_sensor_data.reliability, +) + + +class ESKalmanFilterNode(Node): + def __init__(self): + super().__init__("sp_ukf_python_node") + + # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz + self.imu_subscriber_ = self.create_subscription( + Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile + ) + + self.twist_dvl_subscriber_ = self.create_subscription( + TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile + ) + + # This publisher will publish the estimtaed state of the vehicle + self.state_publisher_ = self.create_publisher( + Odometry, '/orca/odom', qos_profile=qos_profile + ) + + self.eskf_modual = ErrorStateKalmanFilter() + self.current_state_nom = StateVector_quaternion() + self.current_state_error = StateVector_euler() + self.measurement_pred = MeasurementModel() + self.odom_msg = Odometry() + + self.get_logger().info("Unscented Kalman Filter started") + + def imu_callback(self, msg: Imu): + + # Get the IMU data + + imu_acceleartion = msg.linear_acceleration + imu_angular_velocity = msg.angular_velocity + + # Combine the IMU data + imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) + + # Update the filter with the IMU data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.imu_update_states( + self.current_state_nom, self.current_state_error, imu_data + ) + ) + + # Inserting the nominal state into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x + self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y + self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z + + # Publish + self.state_publisher_.publish(self.odom_msg) + + + + def filter_callback(self, msg: TwistWithCovarianceStamped): + """Callback function for the filter measurement update, + this will be called when the filter needs to be updated with the DVL data. + """ + self.get_logger().info("Filter callback, got DVL data") + + # Get the DVL data (linear velocity) + dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + + # Update the filter with the DVL data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.dvl_update_states( + self.current_state_nom, self.current_state_error, dvl_data + ) + ) + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.injection_and_reset( + self.current_state_nom, self.current_state_error + ) + ) + + # Inserting data into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + + # Publishing the data + self.state_publisher_.publish(self.odom_msg) + + + +def main(args=None): + rclpy.init(args=args) + node = ESKalmanFilterNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py new file mode 100644 index 000000000..071180fc7 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py @@ -0,0 +1,107 @@ +import numpy as np + +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 = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new /= np.linalg.norm(q_new) + + 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 euler_rotation_quaternion(self, euler_angles: np.ndarray) -> np.ndarray: + """ + Converts An vector assumed to be rotation vector to quaternion + + Args: + euler_angles (np.ndarray): Rotation vector + + Returns: + np.ndarray: Quaternion representation of the rotation vector + """ + + angle = np.linalg.norm(euler_angles) + + if angle == 0: + axis = np.array([0, 0, 0]) + else: + axis = euler_angles / angle + + quaternion = np.zeros(4) + quaternion[0] = np.cos(angle / 2) + quaternion[1:] = np.sin(angle / 2) * axis + + return quaternion + +def quaternion_rotation_euler(self, quaternion: np.ndarray) -> np.ndarray: + """ + Converts a quaternion to an euler rotation vector + Used to generate the covarince matrix + + Args: + quaternion (np.ndarray): The quaternion to convert + + Returns: + np.ndarray: The euler rotation vector + """ + nu, eta_x, eta_y, eta_z = quaternion + + phi = np.arctan2(2 * (nu * eta_x + eta_y * eta_z), 1 - 2 * (eta_x ** 2 + eta_y ** 2)) + theta = -np.arcsin(2 * (eta_z * eta_x - nu * eta_y)) + psi = np.arctan2(2 * (nu * eta_z + eta_x * eta_y), 1 - 2 * (eta_y ** 2 + eta_z ** 2)) + + return np.array([phi, theta, psi]) + +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 ssa(angle: np.ndarray) -> np.ndarray: + """ + smallest signed angle between two angles + """ + ssa_vector = np.zeros(len(angle)) + + for i in range(len(angle)): + ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi + + return ssa_vector \ No newline at end of file diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py new file mode 100644 index 000000000..59e08aa55 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -0,0 +1,218 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D # for 3D plotting + +# (Assuming the following have been imported from your modules) +from sp_ukf_python_class import StateVector_quaternion, StateVector_euler +from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from sp_ukf_python import ErrorStateUnscentedKalmanFilter + +def quat_to_yaw(q: np.ndarray) -> float: + """ + Convert a quaternion (assumed [w, x, y, z]) with zero roll and pitch + into a yaw angle. + """ + return 2 * np.arctan2(q[3], q[0]) + +def run_ESUKF_simulation(): + # Simulation parameters + dt = 0.01 # time step [s] + T = 60.0 # total simulation time [s] + num_steps = int(T/dt) + g_val = 9.81 # gravitational acceleration + + # Define noise and covariance matrices + Q = np.diag([0.1]*15) # Process noise covariance (15x15) + R_meas = np.diag([0.08]*3) # DVL measurement noise (velocity noise) + P_ab = 0.005 # Accelerometer bias dynamics matrix + P_wb = 0.005 # Gyro bias dynamics matrix + lever_arm = np.array([0.0, 0.0, 0.0]) # Assume sensor is at the center of mass + + # Create ESUKF instance + esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) + + # Initialize true state (StateVector_quaternion) with no biases. + true_state = StateVector_quaternion() + true_state.position = np.array([20.0, 0.0, 0.0]) + true_state.velocity = np.array([0.0, 1.0, 0.0]) + true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation + true_state.acceleration_bias = np.zeros(3) + true_state.gyro_bias = np.zeros(3) + + # Initialize estimated (nominal) state with a small offset. + est_state_nom = StateVector_quaternion() + est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) + est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) + est_state_nom.orientation = true_state.orientation.copy() + est_state_nom.acceleration_bias = np.zeros(3) + est_state_nom.gyro_bias = np.zeros(3) + + # Initialize error state (StateVector_euler) as zero with some initial covariance. + est_state_error = StateVector_euler() + est_state_error.fill_states(np.zeros(15)) + est_state_error.covariance = 0.1 * np.eye(15) + + # Prepare histories for plotting + time_hist = [] + true_pos_hist = [] + est_pos_hist = [] + true_vel_hist = [] + est_vel_hist = [] + true_yaw_hist = [] + est_yaw_hist = [] + + # For the true trajectory, we simulate a circle in the horizontal plane. + R_circle = 20.0 # circle radius [m] + omega = 0.05 # angular speed [rad/s] + + t = 0.0 + for step in range(num_steps): + # --- True State Generation --- + # Circular trajectory: position = [R*cos(omega*t), R*sin(omega*t), 0] + pos_true = np.array([R_circle * np.cos(omega * t), + R_circle * np.sin(omega * t), + 0.0]) + # Velocity is the derivative of position. + vel_true = np.array([-R_circle * omega * np.sin(omega * t), + R_circle * omega * np.cos(omega * t), + 0.0]) + # Acceleration is the second derivative. + acc_true = np.array([-R_circle * omega**2 * np.cos(omega * t), + -R_circle * omega**2 * np.sin(omega * t), + 0.0]) + # Update the true state. + true_state.position = pos_true + true_state.velocity = vel_true + # Compute heading (yaw) tangent to the path. + yaw_true = np.arctan2(vel_true[1], vel_true[0]) + # For simplicity, assume roll and pitch are zero. + true_state.orientation = np.array([np.cos(yaw_true/2), 0.0, 0.0, np.sin(yaw_true/2)]) + # Biases remain zero for the true state. + + # --- Simulated IMU Measurements --- + # The nominal state propagation uses: + # velocity_dot = R_q() @ (imu_acc - bias) + g + # Therefore, the ideal accelerometer measurement is: + # imu_acc = R_true.T @ (acc_true - g_vector) + R_true = true_state.R_q() # rotation matrix from quaternion + imu_acc_ideal = np.dot(R_true.T, (acc_true - np.array([0.0, 0.0, g_val]))) + # Add noise (e.g., 0.1 m/s^2 std dev). + imu_acc_noise = np.random.normal(0.0, 0.1, 3) + imu_acc_meas = imu_acc_ideal + imu_acc_noise + + # For the gyro: the true angular velocity in body frame. + # For a circular path with constant yaw rate, the ideal gyro reading is: + imu_gyro_ideal = np.array([0.0, 0.0, omega]) + # Add noise (e.g., 0.01 rad/s std dev). + imu_gyro_noise = np.random.normal(0.0, 0.01, 3) + imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise + + # Combine to form the complete IMU measurement vector. + imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) + + # --- Simulated DVL Measurement --- + # DVL measures velocity (here assumed in the inertial frame). + dvl_noise = np.random.normal(0.0, 0.05, 3) + dvl_meas = vel_true + dvl_noise + + # --- Filter Updates --- + # 1. Propagate the nominal state with IMU data. + est_state_nom, est_state_error = esukf.imu_update_states(est_state_nom, est_state_error, imu_meas) + # 2. Incorporate DVL measurement. + est_state_nom, est_state_error = esukf.dvl_update_states(est_state_nom, est_state_error, dvl_meas) + # 3. Inject the error state into the nominal state and reset the error state. + est_state_nom, est_state_error = esukf.inject_and_reset(est_state_nom, est_state_error) + + # --- Store Histories --- + time_hist.append(t) + true_pos_hist.append(pos_true) + est_pos_hist.append(est_state_nom.position.copy()) + true_vel_hist.append(vel_true) + est_vel_hist.append(est_state_nom.velocity.copy()) + true_yaw_hist.append(yaw_true) + est_yaw_hist.append(quat_to_yaw(est_state_nom.orientation)) + + t += dt + + # Convert histories to NumPy arrays. + true_pos_hist = np.array(true_pos_hist) + est_pos_hist = np.array(est_pos_hist) + true_vel_hist = np.array(true_vel_hist) + est_vel_hist = np.array(est_vel_hist) + true_yaw_hist = np.array(true_yaw_hist) + est_yaw_hist = np.array(est_yaw_hist) + time_hist = np.array(time_hist) + + # --- Plotting Results --- + + # Plot positions (each axis separately) + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_hist, true_pos_hist[:, 0], label='True X') + plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated X') + plt.ylabel('X Position (m)') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time_hist, true_pos_hist[:, 1], label='True Y') + plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated Y') + plt.ylabel('Y Position (m)') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time_hist, true_pos_hist[:, 2], label='True Z') + plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated Z') + plt.xlabel('Time (s)') + plt.ylabel('Z Position (m)') + plt.legend() + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_hist, true_vel_hist[:, 0], label='True Vx') + plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vx') + plt.ylabel('Vx (m/s)') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time_hist, true_vel_hist[:, 1], label='True Vy') + plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Vy') + plt.ylabel('Vy (m/s)') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time_hist, true_vel_hist[:, 2], label='True Vz') + plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vz') + plt.xlabel('Time (s)') + plt.ylabel('Vz (m/s)') + plt.legend() + plt.tight_layout() + plt.show() + + # Plot heading (yaw) + plt.figure(figsize=(10, 4)) + plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') + plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') + plt.xlabel('Time (s)') + plt.ylabel('Yaw (deg)') + plt.legend() + plt.title('Heading Comparison') + plt.tight_layout() + plt.show() + + # Plot 3D Trajectory + fig = plt.figure(figsize=(8, 6)) + ax = fig.add_subplot(111, projection='3d') + ax.plot(true_pos_hist[:, 0], true_pos_hist[:, 1], true_pos_hist[:, 2], label='True Trajectory', linewidth=2) + ax.plot(est_pos_hist[:, 0], est_pos_hist[:, 1], est_pos_hist[:, 2], '--', label='Estimated Trajectory', linewidth=2) + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_zlabel('Z (m)') + ax.legend() + plt.title('3D Trajectory') + plt.show() + +if __name__ == '__main__': + run_ESUKF_simulation() From 382b0799e89b14836558aa8734e751168b94fdfa Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 27 Feb 2025 16:18:15 +0100 Subject: [PATCH 04/19] feat: added UKF fix injection step --- .../sp_ukf_python/sp_ukf_python.py | 377 ++++++++++-------- .../sp_ukf_python/sp_ukf_python_class.py | 33 +- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 295 +++++++++----- 3 files changed, 434 insertions(+), 271 deletions(-) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py index 6f9d1b2f6..8d2ff6265 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -1,10 +1,9 @@ -from dataclasses import dataclass -from typing import Tuple -from sp_ukf_python_class import StateVector_quaternion, StateVector_euler -from sp_ukf_python_utils import skew_symmetric, quaternion_super_product -from scipy.linalg import expm import numpy as np +from scipy.linalg import expm +from sp_ukf_python_class import StateVector_euler, StateVector_quaternion +from sp_ukf_python_utils import quaternion_super_product, skew_symmetric + class ErrorStateUnscentedKalmanFilter: def __init__( @@ -24,11 +23,10 @@ def __init__( self.R = R self.g = np.array([0, 0, g]) self.dt = dt - self.y_i = np.zeros((15, 2*15)) - + self.y_i = np.zeros((15, 2 * 15)) + def mean_set(self, set: np.ndarray) -> np.ndarray: - """ - Calculates the mean of a set of values. + """Calculates the mean of a set of values. Args: set (np.ndarray): The set of values. @@ -38,15 +36,14 @@ def mean_set(self, set: np.ndarray) -> np.ndarray: """ # Define the number of columns n = set.shape[0] - + # Calculate the mean value mean_value = (1 / (2 * n)) * np.sum(set, axis=1) return mean_value - + def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: - """ - Calculates the mean of a set of values. + """Calculates the mean of a set of values. Args: set (np.ndarray): The set of values. @@ -57,86 +54,94 @@ def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: # Define the number of columns n = set.shape[0] mean_value = np.zeros(n) - - for i in range(2*n + 1): + + for i in range(2 * n + 1): mean_value += weight[i] * set[:, i] - mean_value = (1 / (2 * n + 1)) * mean_value + mean_value = (1 / (2 * n + 1)) * mean_value return mean_value - - def covariance_set(self, mean: np.ndarray, set: np.ndarray, mean_2: np.ndarray = None, set_2: np.ndarray = None) -> np.ndarray: - """ - Calculate the covarince of a set of sigmapoints - + + def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: + """Calculate the covarince of a set of sigmapoints + Args: mean (np.ndarray): The mean of the set. set (np.ndarray): The set of values. - + Returns: np.ndarray: The covariance of the set. """ + n = set.shape[0] + covariance_set = np.zeros((n, n)) - if mean_2 is not None: - - n = set.shape[0] - n2 = set_2.shape[0] - covariance_set = np.zeros((n, n2)) - - for i in range(2*n): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - vector_2 = StateVector_euler() - vector_2.position = set_2[:, i][:3] - vector_2.velocity = set_2[:, i][3:6] - vector_2.orientation = set_2[:, i][6:9] - vector_2.acceleration_bias = set_2[:, i][9:12] - vector_2.gyro_bias = set_2[:, i][12:] - W_i = vector - mean - W_i_2 = vector_2 - mean_2 - - covariance_set += (1 / (2*n)) * np.outer(W_i, W_i_2) - - return covariance_set - else: - - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2*n): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - covariance_set += (1 / (2*n)) * np.outer(W_i, W_i) - - return covariance_set - - def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + for i in range(2 * n + 1): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + + covariance_set += (1 / (2 * n + 1)) * np.outer(W_i, W_i) + + return covariance_set + + def cross_covariance_set( + self, + mean: np.ndarray, + set: np.ndarray, + mean_2: np.ndarray, + set_2: np.ndarray, + weight: np.ndarray, + ) -> np.ndarray: + """Calculate the cross covariance of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + mean_2 (np.ndarray): The mean of the second set. + set_2 (np.ndarray): The second set of values. + + Returns: + np.ndarray: The cross covariance of the set. """ - Calculate the covarince of a set of sigmapoints - + n_x = set.shape[0] + n_z = set_2.shape[0] + covariance_mat = np.zeros((n_x, n_z)) + + for i in range(2 * n_x + 1): + # parse the 15-dim error state + err_vec = set[:, i] # shape (15,) + W_i = err_vec - mean # shape (15,) + + # parse the 3-dim measurement + meas_vec = set_2[:, i] # shape (3,) + W_i_2 = meas_vec - mean_2 # shape (3,) + + # outer product -> shape (15,3) + covariance_mat += weight[i] * np.outer(W_i, W_i_2) + + return covariance_mat + + def weighted_covariance_set( + self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray + ) -> np.ndarray: + """Calculate the covarince of a set of sigmapoints + Args: mean (np.ndarray): The mean of the set. set (np.ndarray): The set of values. - + Returns: np.ndarray: The covariance of the set. """ - n = set.shape[0] covariance_set = np.zeros((n, n)) - for i in range(2*n): + for i in range(2 * n + 1): vector = StateVector_euler() vector.position = set[:, i][:3] vector.velocity = set[:, i][3:6] @@ -146,38 +151,38 @@ def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np. W_i = vector - mean covariance_set += weight[i] * np.outer(W_i, W_i) - - return covariance_set - + return covariance_set - def generate_sigma_points(self, error_state: StateVector_euler, Q_process_noise) -> tuple[list[StateVector_euler], np.ndarray]: - """ - Generates the sigma points for the UKF + def generate_sigma_points( + self, error_state: StateVector_euler, Q_process_noise + ) -> tuple[list[StateVector_euler], np.ndarray]: + """Generates the sigma points for the UKF This is done using the Cholesky decomposition method """ - # Define n n = len(error_state.covariance) kappa = 3 - n # Computing S matrix using cholensky decomposition + # print(error_state.covariance + Q_process_noise) S = np.linalg.cholesky(error_state.covariance + Q_process_noise) + # print(S) S_scaled = np.sqrt(n + kappa) * S - weighted_points = np.concatenate((S_scaled , -S_scaled), axis=1) + weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] sigma_points[0].fill_states(error_state.as_vector()) - for i in range(2*n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:,i]) + for i in range(2 * n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - W = np.zeros(2*n + 1) + W = np.zeros(2 * n + 1) W[0] = kappa / (n + kappa) - for i in range(2*n): + for i in range(2 * n): W[i + 1] = 1 / (2 * (n + kappa)) return sigma_points, W @@ -214,7 +219,14 @@ def nominal_state_update( # Define the quaternion derivatives current_state_dot.orientation = 0.5 * quaternion_super_product( current_state.orientation, - np.array([0, imu_gyro[0] - current_state.gyro_bias[0], imu_gyro[1] - current_state.gyro_bias[1], imu_gyro[2] - current_state.gyro_bias[2]]), + np.array( + [ + 0, + imu_gyro[0] - current_state.gyro_bias[0], + imu_gyro[1] - current_state.gyro_bias[1], + imu_gyro[2] - current_state.gyro_bias[2], + ] + ), ) # Define the bias @@ -265,20 +277,24 @@ def error_state_update( next_error_state = A_d @ current_error_state.as_vector() return next_error_state - - def unscented_transform(self, sigma_points: list[StateVector_euler], current_state: StateVector_quaternion, - imu_reading: np.ndarray,) -> StateVector_euler: - """ - Performs the Unscented Transform + + def unscented_transform( + self, + sigma_points: list[StateVector_euler], + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Performs the Unscented Transform This is the corresponding to a preditction step in the EKF """ - n = len(sigma_points[0].as_vector()) - self.y_i = np.zeros((n, 2*n)) + self.y_i = np.zeros((n, 2 * n + 1)) - for i in range(2*n): - self.y_i[:, i] = self.error_state_update(sigma_points[i], current_state, imu_reading) + for i in range(2 * n + 1): + self.y_i[:, i] = self.error_state_update( + sigma_points[i], current_state, imu_reading + ) error_state_estimate = StateVector_euler() @@ -286,9 +302,9 @@ def unscented_transform(self, sigma_points: list[StateVector_euler], current_sta error_state_estimate.fill_states(x) error_state_estimate.covariance = self.covariance_set(x, self.y_i) - + return error_state_estimate - + def H(self) -> np.ndarray: """Calculates the measurement matrix. @@ -296,39 +312,83 @@ def H(self) -> np.ndarray: np.ndarray: The measurement matrix. """ # Define the measurement matrix - H = np.zeros((3, 15)) + H = np.zeros((3, 16)) # For now assume only velocity is measured - H[0:3, 3:6] = np.eye(3) + H[:, 3:6] = np.eye(3) return H - - def measurement_update(self, sigma_points: list[StateVector_euler], current_error_state: StateVector_euler, dvl_data: np.ndarray, Weight: np.ndarray) -> StateVector_euler: - """ - Updates the state vector with the DVL data + + def injection( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + ) -> StateVector_quaternion: + """Injects the error state into the nominal state + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + + Returns: + StateVector_quaternion: The updated nominal state """ + inj_state = StateVector_quaternion() + inj_state.position = current_state_nom.position + current_state_error.position + inj_state.velocity = current_state_nom.velocity + current_state_error.velocity + inj_state.orientation = quaternion_super_product( + current_state_nom.orientation, + 0.5 + * np.array( + [ + 2, + current_state_error.orientation[0], + current_state_error.orientation[1], + current_state_error.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + current_state_nom.acceleration_bias + current_state_error.acceleration_bias + ) + inj_state.gyro_bias = ( + current_state_nom.gyro_bias + current_state_error.gyro_bias + ) + + return inj_state + + def measurement_update( + self, + sigma_points: list[StateVector_euler], + current_nom_state: StateVector_quaternion, + current_error_state: StateVector_euler, + dvl_data: np.ndarray, + Weight: np.ndarray, + ) -> StateVector_euler: + """Updates the state vector with the DVL data + """ H = self.H() R = self.R n = len(sigma_points[0].as_vector()) - Z_i = np.zeros((H.shape[0], 2 * n)) + Z_i = np.zeros((H.shape[0], 2 * n + 1)) + + for i in range(2 * n + 1): + injected_state = self.injection(current_nom_state, sigma_points[i]) + Z_i[:, i] = np.dot(H, injected_state.as_vector()) - for i in range(2*n): - Z_i[:, i] = np.dot(H, sigma_points[i].as_vector()) - z = self.weighted_mean_set(Z_i, Weight) S = self.weighted_covariance_set(z, Z_i, Weight) x = self.mean_set(self.y_i) - # Calculate the rest innovation = dvl_data - z P_innovation = S + R - P_xz = self.covariance_set(x, self.y_i, z, Z_i) + P_xz = self.cross_covariance_set(x, self.y_i, z, Z_i, Weight) # Kalman gain K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) @@ -339,102 +399,99 @@ def measurement_update(self, sigma_points: list[StateVector_euler], current_erro updated_error_state.fill_states(x + np.dot(K_k, innovation)) # Update the covariance - updated_error_state.covariance = current_error_state.covariance - np.dot(K_k, np.dot(P_innovation, K_k.T)) + updated_error_state.covariance = current_error_state.covariance - np.dot( + K_k, np.dot(P_innovation, K_k.T) + ) - return updated_error_state + return updated_error_state + + def imu_update_states( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + imu_data: np.ndarray, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Updates the state vector with the IMU data - def imu_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, imu_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Updates the state vector with the IMU data - Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state imu_data (np.ndarray): The IMU data - + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ - # Update the nominal state current_state_nom = self.nominal_state_update(current_state_nom, imu_data) # Generate the sigma points - sigma_points, _ = self.generate_sigma_points(current_state_error, self.Q_process_noise) + sigma_points, _ = self.generate_sigma_points( + current_state_error, self.Q_process_noise + ) # Update the error state - current_state_error = self.unscented_transform(sigma_points, current_state_nom, imu_data) + current_state_error = self.unscented_transform( + sigma_points, current_state_nom, imu_data + ) return current_state_nom, current_state_error - - def dvl_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Update the error state given the DVL data + + def dvl_update_states( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + dvl_data: np.ndarray, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Update the error state given the DVL data Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state dvl_data (np.ndarray): The DVL data to update the state with - + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ - # Generate the sigma points - sigma_points, weight = self.generate_sigma_points(current_state_error, self.Q_process_noise) + sigma_points, weight = self.generate_sigma_points( + current_state_error, self.Q_process_noise + ) # Update the error state - current_state_error = self.measurement_update(sigma_points, current_state_error, dvl_data, weight) + current_state_error = self.measurement_update( + sigma_points, current_state_nom, current_state_error, dvl_data, weight + ) return current_state_nom, current_state_error - - def inject_and_reset(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Injects the error state into the nominal state and resets the error state + + def inject_and_reset( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Injects the error state into the nominal state and resets the error state Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state - - Returns: + + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ + inj_state = self.injection(current_state_nom, current_state_error) - inj_state = StateVector_quaternion() - - inj_state.position = current_state_nom.position + current_state_error.position - inj_state.velocity = current_state_nom.velocity + current_state_error.velocity - inj_state.orientation = quaternion_super_product( - current_state_nom.orientation, - 0.5 - * np.array( - [ - 2, - current_state_error.orientation[0], - current_state_error.orientation[1], - current_state_error.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - current_state_nom.acceleration_bias + current_state_error.acceleration_bias - ) - inj_state.gyro_bias = current_state_nom.gyro_bias + current_state_error.gyro_bias - - - # Resetting the error state G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - skew_symmetric( - 0.5 * current_state_error.orientation - ) + G[6:9, 6:9] = np.eye(3) - skew_symmetric(0.5 * current_state_error.orientation) current_state_error.covariance = np.dot( np.dot(G, current_state_error.covariance), G.T ) + current_state_error.covariance += np.eye(15) * 1e-4 + + eigvals = np.linalg.eigvals(current_state_error.covariance) + print("Min eigenvalue:", np.min(eigvals)) current_state_error.fill_states(np.zeros(15)) - return inj_state, current_state_error - diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py index 629ce460e..b9a76e26f 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -1,6 +1,13 @@ -import numpy as np from dataclasses import dataclass, field -from sp_ukf_python_utils import quaternion_super_product, quaternion_error, euler_rotation_quaternion, ssa + +import numpy as np +from sp_ukf_python_utils import ( + euler_rotation_quaternion, + quaternion_error, + quaternion_super_product, + ssa, +) + @dataclass class StateVector_quaternion: @@ -48,7 +55,7 @@ def fill_states(self, state: np.ndarray) -> None: self.orientation = state[6:10] self.acceleration_bias = state[10:13] self.gyro_bias = state[13:] - else: + else: self.position = state[0:3] self.velocity = state[3:6] self.orientation = euler_rotation_quaternion(state[6:9]) @@ -103,7 +110,7 @@ def euler_forward( new_state.orientation /= np.linalg.norm(new_state.orientation) return new_state - + def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: """Subtracts two StateVector_quaternion objects. @@ -128,7 +135,7 @@ def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: gyro_bias_diff, ] ) - + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': """Adds a numpy array to this StateVector_quaternion. @@ -229,7 +236,6 @@ def __add__(self, other: 'np.ndarray') -> 'np.ndarray': Returns: np.ndarray: The result of the addition. """ - new_position = self.position + other[:3] new_velcoity = self.velocity + other[3:6] new_orientation = self.orientation + other[6:9] @@ -247,9 +253,8 @@ def __add__(self, other: 'np.ndarray') -> 'np.ndarray': ) def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': - """ - Subtracts two StateVector_euler objects. - + """Subtracts two StateVector_euler objects. + Args: other (StateVector_euler): The other StateVector_euler object. @@ -263,7 +268,13 @@ def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': gyro_bias_diff = self.gyro_bias - other_state[12:] return np.concatenate( - (position_diff, velocity_diff, orientation_diff, acceleration_bias_diff, gyro_bias_diff) + [ + position_diff, + velocity_diff, + orientation_diff, + acceleration_bias_diff, + gyro_bias_diff, + ] ) @@ -277,4 +288,4 @@ class MeasurementModel: ) # Measurement matrix measurement_covariance: np.ndarray = field( default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix \ No newline at end of file + ) # Measurement noise matrix diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py index 59e08aa55..d46962797 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -1,45 +1,78 @@ -import numpy as np import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D # for 3D plotting +import numpy as np # (Assuming the following have been imported from your modules) -from sp_ukf_python_class import StateVector_quaternion, StateVector_euler -from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from sp_ukf_python_class import StateVector_euler, StateVector_quaternion + from sp_ukf_python import ErrorStateUnscentedKalmanFilter + def quat_to_yaw(q: np.ndarray) -> float: - """ - Convert a quaternion (assumed [w, x, y, z]) with zero roll and pitch - into a yaw angle. + """Convert a quaternion (assumed [w, x, y, z]) into a yaw angle. + In NED, yaw is typically around the z-down axis. """ return 2 * np.arctan2(q[3], q[0]) + def run_ESUKF_simulation(): + # ------------------------------------------------------------------------- # Simulation parameters - dt = 0.01 # time step [s] - T = 60.0 # total simulation time [s] - num_steps = int(T/dt) - g_val = 9.81 # gravitational acceleration + # ------------------------------------------------------------------------- + dt = 0.01 # time step [s] + T = 60.0 # total simulation time [s] + num_steps = int(T / dt) + # In an NED frame, gravity is +9.81 in the z (down) direction. + g_val = 9.81 + + # ------------------------------------------------------------------------- # Define noise and covariance matrices - Q = np.diag([0.1]*15) # Process noise covariance (15x15) - R_meas = np.diag([0.08]*3) # DVL measurement noise (velocity noise) - P_ab = 0.005 # Accelerometer bias dynamics matrix - P_wb = 0.005 # Gyro bias dynamics matrix - lever_arm = np.array([0.0, 0.0, 0.0]) # Assume sensor is at the center of mass + # ------------------------------------------------------------------------- + Q = np.diag( + [ + 0.06, + 0.06, + 0.06, # position error + 0.04, + 0.04, + 0.04, # velocity error + 0.003, + 0.003, + 0.003, # orientation error + 0.02, + 0.02, + 0.02, # accelerometer bias error + 0.02, + 0.02, + 0.02, # gyro bias error + ] + ) + + R_meas = np.diag([0.52, 0.52, 0.52]) # Increased DVL measurement noise - # Create ESUKF instance + # Bias dynamics tuning remains the same here: + P_ab = 0.002 + P_wb = 0.002 + lever_arm = np.array([0.0, 0.0, 0.0]) # Sensor at the vehicle CG + + # Create the Error-State UKF instance (NED convention) esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) - # Initialize true state (StateVector_quaternion) with no biases. + # ------------------------------------------------------------------------- + # Initialize the true state in NED + # ------------------------------------------------------------------------- + # We treat x as North, y as East, z as Down. + # We'll do a circular path in the horizontal plane (z=0). true_state = StateVector_quaternion() - true_state.position = np.array([20.0, 0.0, 0.0]) - true_state.velocity = np.array([0.0, 1.0, 0.0]) + true_state.position = np.array([20.0, 0.0, 0.0]) # [N, E, D]=[20, 0, 0] + true_state.velocity = np.array([0.0, 1.0, 0.0]) # 1 m/s in the East direction true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation true_state.acceleration_bias = np.zeros(3) true_state.gyro_bias = np.zeros(3) - # Initialize estimated (nominal) state with a small offset. + # ------------------------------------------------------------------------- + # Initialize the estimated state + # ------------------------------------------------------------------------- est_state_nom = StateVector_quaternion() est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) @@ -47,12 +80,14 @@ def run_ESUKF_simulation(): est_state_nom.acceleration_bias = np.zeros(3) est_state_nom.gyro_bias = np.zeros(3) - # Initialize error state (StateVector_euler) as zero with some initial covariance. + # Initialize error state (Euler) with some covariance est_state_error = StateVector_euler() est_state_error.fill_states(np.zeros(15)) - est_state_error.covariance = 0.1 * np.eye(15) + est_state_error.covariance = 0.5 * np.eye(15) + # ------------------------------------------------------------------------- # Prepare histories for plotting + # ------------------------------------------------------------------------- time_hist = [] true_pos_hist = [] est_pos_hist = [] @@ -61,67 +96,110 @@ def run_ESUKF_simulation(): true_yaw_hist = [] est_yaw_hist = [] - # For the true trajectory, we simulate a circle in the horizontal plane. - R_circle = 20.0 # circle radius [m] - omega = 0.05 # angular speed [rad/s] + # ------------------------------------------------------------------------- + # Define the "circular" trajectory in the horizontal plane (z=0) + # in NED: x=North, y=East, z=Down + # We'll revolve in the XY-plane, at D=0, with radius=20 m, angular speed=0.05 rad/s + # ------------------------------------------------------------------------- + R_circle = 20.0 + omega = 0.05 + # ------------------------------------------------------------------------- + # Main simulation loop + # ------------------------------------------------------------------------- t = 0.0 for step in range(num_steps): - # --- True State Generation --- - # Circular trajectory: position = [R*cos(omega*t), R*sin(omega*t), 0] - pos_true = np.array([R_circle * np.cos(omega * t), - R_circle * np.sin(omega * t), - 0.0]) - # Velocity is the derivative of position. - vel_true = np.array([-R_circle * omega * np.sin(omega * t), - R_circle * omega * np.cos(omega * t), - 0.0]) - # Acceleration is the second derivative. - acc_true = np.array([-R_circle * omega**2 * np.cos(omega * t), - -R_circle * omega**2 * np.sin(omega * t), - 0.0]) - # Update the true state. + # --- True State Generation (NED) --- + # Position: circle in x-y plane at z=0 + pos_true = np.array( + [ + R_circle * np.cos(omega * t), # N + R_circle * np.sin(omega * t), # E + 0.0, # D + ] + ) + # Velocity: derivative of pos + vel_true = np.array( + [ + -R_circle * omega * np.sin(omega * t), # d/dt of cos => -sin + R_circle * omega * np.cos(omega * t), # d/dt of sin => cos + 0.0, + ] + ) + # Acceleration: second derivative + acc_true = np.array( + [ + -R_circle * omega**2 * np.cos(omega * t), + -R_circle * omega**2 * np.sin(omega * t), + 0.0, + ] + ) + + # Update the "true" state in NED true_state.position = pos_true true_state.velocity = vel_true - # Compute heading (yaw) tangent to the path. + + # Compute full quaternion from Euler angles (roll, pitch, yaw) + roll_true = 0.0 + pitch_true = 0.0 yaw_true = np.arctan2(vel_true[1], vel_true[0]) - # For simplicity, assume roll and pitch are zero. - true_state.orientation = np.array([np.cos(yaw_true/2), 0.0, 0.0, np.sin(yaw_true/2)]) - # Biases remain zero for the true state. - - # --- Simulated IMU Measurements --- - # The nominal state propagation uses: - # velocity_dot = R_q() @ (imu_acc - bias) + g - # Therefore, the ideal accelerometer measurement is: - # imu_acc = R_true.T @ (acc_true - g_vector) - R_true = true_state.R_q() # rotation matrix from quaternion - imu_acc_ideal = np.dot(R_true.T, (acc_true - np.array([0.0, 0.0, g_val]))) - # Add noise (e.g., 0.1 m/s^2 std dev). - imu_acc_noise = np.random.normal(0.0, 0.1, 3) + cy = np.cos(yaw_true * 0.5) + sy = np.sin(yaw_true * 0.5) + cp = np.cos(pitch_true * 0.5) + sp = np.sin(pitch_true * 0.5) + cr = np.cos(roll_true * 0.5) + sr = np.sin(roll_true * 0.5) + true_state.orientation = np.array( + [ + cr * cp * cy + sr * sp * sy, # w + sr * cp * cy - cr * sp * sy, # x + cr * sp * cy + sr * cp * sy, # y + cr * cp * sy - sr * sp * cy, # z + ] + ) + + # --- Simulated IMU Measurements (NED) --- + # Gravity is +9.81 in the down (z) direction in NED + R_true = true_state.R_q() # rotation from body to inertial + # The "specific force" in body frame is (acc_inertial - gravity_inertial) rotated to body + imu_acc_ideal = R_true.T @ ( + acc_true - np.array([0.0, 0.0, g_val]) + ) + np.random.normal(0.01, 0.01, 3) # [rad/s] + + # Add small noise + imu_acc_noise = np.random.normal(0.0, 0.05, 3) # [m/s^2] imu_acc_meas = imu_acc_ideal + imu_acc_noise - # For the gyro: the true angular velocity in body frame. - # For a circular path with constant yaw rate, the ideal gyro reading is: - imu_gyro_ideal = np.array([0.0, 0.0, omega]) - # Add noise (e.g., 0.01 rad/s std dev). - imu_gyro_noise = np.random.normal(0.0, 0.01, 3) + # Gyro: angular velocity about body axes. Yaw rate is ~omega for a flat circle + imu_gyro_ideal = np.array([0.0, 0.0, omega]) + np.random.normal( + 0.01, 0.01, 3 + ) # [rad/s] + imu_gyro_noise = np.random.normal(0.0, 0.05, 3) # [rad/s] imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise - # Combine to form the complete IMU measurement vector. + # Combine imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) # --- Simulated DVL Measurement --- - # DVL measures velocity (here assumed in the inertial frame). + # Velocity in inertial frame (NED) with zero noise for this test dvl_noise = np.random.normal(0.0, 0.05, 3) dvl_meas = vel_true + dvl_noise - # --- Filter Updates --- - # 1. Propagate the nominal state with IMU data. - est_state_nom, est_state_error = esukf.imu_update_states(est_state_nom, est_state_error, imu_meas) - # 2. Incorporate DVL measurement. - est_state_nom, est_state_error = esukf.dvl_update_states(est_state_nom, est_state_error, dvl_meas) - # 3. Inject the error state into the nominal state and reset the error state. - est_state_nom, est_state_error = esukf.inject_and_reset(est_state_nom, est_state_error) + # --------------------------------------------------------------------- + # Filter Updates + # --------------------------------------------------------------------- + # 1. IMU update (prediction) + est_state_nom, est_state_error = esukf.imu_update_states( + est_state_nom, est_state_error, imu_meas + ) + # 2. DVL update (measurement) + est_state_nom, est_state_error = esukf.dvl_update_states( + est_state_nom, est_state_error, dvl_meas + ) + # 3. Inject error state + est_state_nom, est_state_error = esukf.inject_and_reset( + est_state_nom, est_state_error + ) # --- Store Histories --- time_hist.append(t) @@ -134,7 +212,9 @@ def run_ESUKF_simulation(): t += dt - # Convert histories to NumPy arrays. + # ------------------------------------------------------------------------- + # Convert histories to arrays + # ------------------------------------------------------------------------- true_pos_hist = np.array(true_pos_hist) est_pos_hist = np.array(est_pos_hist) true_vel_hist = np.array(true_vel_hist) @@ -143,76 +223,91 @@ def run_ESUKF_simulation(): est_yaw_hist = np.array(est_yaw_hist) time_hist = np.array(time_hist) - # --- Plotting Results --- - - # Plot positions (each axis separately) + # ------------------------------------------------------------------------- + # Plotting + # ------------------------------------------------------------------------- + # Positions plt.figure(figsize=(10, 8)) plt.subplot(3, 1, 1) - plt.plot(time_hist, true_pos_hist[:, 0], label='True X') - plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated X') - plt.ylabel('X Position (m)') + plt.plot(time_hist, true_pos_hist[:, 0], label='True N') + plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated N') + plt.ylabel('N (m)') plt.legend() plt.subplot(3, 1, 2) - plt.plot(time_hist, true_pos_hist[:, 1], label='True Y') - plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated Y') - plt.ylabel('Y Position (m)') + plt.plot(time_hist, true_pos_hist[:, 1], label='True E') + plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated E') + plt.ylabel('E (m)') plt.legend() plt.subplot(3, 1, 3) - plt.plot(time_hist, true_pos_hist[:, 2], label='True Z') - plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated Z') + plt.plot(time_hist, true_pos_hist[:, 2], label='True D') + plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated D') plt.xlabel('Time (s)') - plt.ylabel('Z Position (m)') + plt.ylabel('D (m)') plt.legend() plt.tight_layout() plt.show() - # Plot velocities + # Velocities plt.figure(figsize=(10, 8)) plt.subplot(3, 1, 1) - plt.plot(time_hist, true_vel_hist[:, 0], label='True Vx') - plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vx') - plt.ylabel('Vx (m/s)') + plt.plot(time_hist, true_vel_hist[:, 0], label='True Vn') + plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vn') + plt.ylabel('Vn (m/s)') plt.legend() plt.subplot(3, 1, 2) - plt.plot(time_hist, true_vel_hist[:, 1], label='True Vy') - plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Vy') - plt.ylabel('Vy (m/s)') + plt.plot(time_hist, true_vel_hist[:, 1], label='True Ve') + plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Ve') + plt.ylabel('Ve (m/s)') plt.legend() plt.subplot(3, 1, 3) - plt.plot(time_hist, true_vel_hist[:, 2], label='True Vz') - plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vz') + plt.plot(time_hist, true_vel_hist[:, 2], label='True Vd') + plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vd') plt.xlabel('Time (s)') - plt.ylabel('Vz (m/s)') + plt.ylabel('Vd (m/s)') plt.legend() plt.tight_layout() plt.show() - # Plot heading (yaw) + # Heading (Yaw) plt.figure(figsize=(10, 4)) plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') plt.xlabel('Time (s)') plt.ylabel('Yaw (deg)') plt.legend() - plt.title('Heading Comparison') + plt.title('Heading Comparison (NED)') plt.tight_layout() plt.show() - # Plot 3D Trajectory + # 3D Trajectory fig = plt.figure(figsize=(8, 6)) ax = fig.add_subplot(111, projection='3d') - ax.plot(true_pos_hist[:, 0], true_pos_hist[:, 1], true_pos_hist[:, 2], label='True Trajectory', linewidth=2) - ax.plot(est_pos_hist[:, 0], est_pos_hist[:, 1], est_pos_hist[:, 2], '--', label='Estimated Trajectory', linewidth=2) - ax.set_xlabel('X (m)') - ax.set_ylabel('Y (m)') - ax.set_zlabel('Z (m)') + ax.plot( + true_pos_hist[:, 0], + true_pos_hist[:, 1], + true_pos_hist[:, 2], + label='True Trajectory', + linewidth=2, + ) + ax.plot( + est_pos_hist[:, 0], + est_pos_hist[:, 1], + est_pos_hist[:, 2], + '--', + label='Estimated Trajectory', + linewidth=2, + ) + ax.set_xlabel('North (m)') + ax.set_ylabel('East (m)') + ax.set_zlabel('Down (m)') ax.legend() - plt.title('3D Trajectory') + plt.title('3D Trajectory (NED Frame)') plt.show() + if __name__ == '__main__': run_ESUKF_simulation() From 52bdaebd23f76be7c0ce0d22cbf013e48c141c9f Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 9 Mar 2025 01:32:46 +0100 Subject: [PATCH 05/19] feat: working ukf filter is added, some issue in the ESUKF --- .../sp_ukf_python/sp_ukf_python.py | 40 +- .../sp_ukf_python/sp_ukf_python_class.py | 3 +- .../sp_ukf_python/sp_ukf_python_utils.py | 11 +- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 2 +- navigation/ukf_okid/ukf_python/__ini__.py | 0 navigation/ukf_okid/ukf_python/ukf_okid.py | 376 ++++++++++++ .../ukf_python/ukf_okid_class copy.py | 568 ++++++++++++++++++ .../ukf_okid/ukf_python/ukf_okid_class.py | 464 ++++++++++++++ navigation/ukf_okid/ukf_python/ukf_utils.py | 36 ++ 9 files changed, 1476 insertions(+), 24 deletions(-) create mode 100644 navigation/ukf_okid/ukf_python/__ini__.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid_class copy.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid_class.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_utils.py diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py index 8d2ff6265..819cb737d 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -24,6 +24,7 @@ def __init__( self.g = np.array([0, 0, g]) self.dt = dt self.y_i = np.zeros((15, 2 * 15)) + self.W = np.zeros(2 * 15 + 1) def mean_set(self, set: np.ndarray) -> np.ndarray: """Calculates the mean of a set of values. @@ -34,11 +35,12 @@ def mean_set(self, set: np.ndarray) -> np.ndarray: Returns: np.ndarray: The mean of the set. """ - # Define the number of columns + # Define the number of sigma points based on columns n = set.shape[0] + mean_value = np.zeros(n) - # Calculate the mean value - mean_value = (1 / (2 * n)) * np.sum(set, axis=1) + for i in range(2 * n + 1): + mean_value += (1/(2 * n + 1)) * set[:, i] return mean_value @@ -58,8 +60,6 @@ def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: for i in range(2 * n + 1): mean_value += weight[i] * set[:, i] - mean_value = (1 / (2 * n + 1)) * mean_value - return mean_value def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: @@ -160,31 +160,26 @@ def generate_sigma_points( """Generates the sigma points for the UKF This is done using the Cholesky decomposition method """ - # Define n n = len(error_state.covariance) kappa = 3 - n - # Computing S matrix using cholensky decomposition - # print(error_state.covariance + Q_process_noise) S = np.linalg.cholesky(error_state.covariance + Q_process_noise) - # print(S) S_scaled = np.sqrt(n + kappa) * S weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] + W = np.zeros(2 * n + 1) sigma_points[0].fill_states(error_state.as_vector()) - for i in range(2 * n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - - W = np.zeros(2 * n + 1) W[0] = kappa / (n + kappa) for i in range(2 * n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) W[i + 1] = 1 / (2 * (n + kappa)) + self.W = W return sigma_points, W def nominal_state_update( @@ -298,10 +293,10 @@ def unscented_transform( error_state_estimate = StateVector_euler() - x = self.mean_set(self.y_i) + x = self.weighted_mean_set(self.y_i, self.W) error_state_estimate.fill_states(x) - error_state_estimate.covariance = self.covariance_set(x, self.y_i) + error_state_estimate.covariance = self.weighted_covariance_set(x, self.y_i, self.W) return error_state_estimate @@ -311,10 +306,10 @@ def H(self) -> np.ndarray: Returns: np.ndarray: The measurement matrix. """ - # Define the measurement matrix + # Define the measurement matrix (error state is 15-dim) H = np.zeros((3, 16)) - # For now assume only velocity is measured + # For now assume only velocity is measured (located at indices 3:6) H[:, 3:6] = np.eye(3) return H @@ -442,6 +437,7 @@ def dvl_update_states( current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray, + imu_data: np.ndarray, ) -> tuple[StateVector_quaternion, StateVector_euler]: """Update the error state given the DVL data @@ -458,6 +454,11 @@ def dvl_update_states( current_state_error, self.Q_process_noise ) + # Update the error state + current_state_error = self.unscented_transform( + sigma_points, current_state_nom, imu_data + ) + # Update the error state current_state_error = self.measurement_update( sigma_points, current_state_nom, current_state_error, dvl_data, weight @@ -487,10 +488,7 @@ def inject_and_reset( current_state_error.covariance = np.dot( np.dot(G, current_state_error.covariance), G.T ) - current_state_error.covariance += np.eye(15) * 1e-4 - - eigvals = np.linalg.eigvals(current_state_error.covariance) - print("Min eigenvalue:", np.min(eigvals)) + current_state_error.covariance += np.eye(15) current_state_error.fill_states(np.zeros(15)) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py index b9a76e26f..f8ede884d 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -6,6 +6,7 @@ quaternion_error, quaternion_super_product, ssa, + quat_norm, ) @@ -100,7 +101,7 @@ def euler_forward( # Define the state derivatives new_state.position = current_state.position + self.position * dt new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = current_state.orientation + self.orientation * dt + new_state.orientation = quat_norm(current_state.orientation + self.orientation * dt) new_state.acceleration_bias = ( current_state.acceleration_bias + self.acceleration_bias * dt ) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py index 071180fc7..56285f031 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py @@ -104,4 +104,13 @@ def ssa(angle: np.ndarray) -> np.ndarray: for i in range(len(angle)): ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi - return ssa_vector \ No newline at end of file + return ssa_vector + +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + + quat = quat / np.linalg.norm(quat) + + return quat diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py index d46962797..1d8c723b1 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -194,7 +194,7 @@ def run_ESUKF_simulation(): ) # 2. DVL update (measurement) est_state_nom, est_state_error = esukf.dvl_update_states( - est_state_nom, est_state_error, dvl_meas + est_state_nom, est_state_error, dvl_meas, imu_meas ) # 3. Inject error state est_state_nom, est_state_error = esukf.inject_and_reset( diff --git a/navigation/ukf_okid/ukf_python/__ini__.py b/navigation/ukf_okid/ukf_python/__ini__.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..c588d579e --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -0,0 +1,376 @@ +from ukf_okid_class import * +import numpy as np +import time +import matplotlib.pyplot as plt + + +class UKF: + def __init__(self, process_model: process_model, x_0, P_0, Q, R): + self.x = x_0 + self.P = P_0 + self.Q = Q + self.R = R + self.process_model = process_model + self.sigma_points_list = None + self.y_i = None + self.weight = None + + def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: + """ + Functions that generate the sigma points for the UKF + """ + n = len(current_state.covariance) + kappa = 3 - n + + S = np.linalg.cholesky(current_state.covariance + self.Q) + S_scaled = np.sqrt(n + kappa) * S + + weighted_points = np.concatenate([S_scaled, -S_scaled], axis=1) + + self.sigma_points_list = [StateQuat() for _ in range(2 * n + 1)] + W = np.zeros(2 * n + 1) + + self.sigma_points_list [0].fill_states(current_state.as_vector()) + W[0] = kappa / (n + kappa) + for i in range(2 * n): + self.sigma_points_list [i + 1].fill_states(current_state.insert_weights(weighted_points[:, i])) + W[i + 1] = 1 / (2 * (n + kappa)) + + self.weight = W + + return self.sigma_points_list , self.weight + + + 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 + 1)] + + for i in range(2 * n + 1): + self.process_model.model_prediction(self.sigma_points_list[i]) + self.y_i[i] = self.process_model.euler_forward() + + state_estimate = StateQuat() + x = mean_set(self.y_i, self.weight) + + state_estimate.fill_states(x) + state_estimate.covariance = covariance_set(self.y_i, x, self.weight) + return state_estimate + + def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: + """ + 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 + 1)] + + for i in range(2 * n + 1): + z_i[i] = measurement.H(self.sigma_points_list[i]) + + meas_update = MeasModel() + + meas_update.measurement = mean_measurement(z_i, self.weight) + + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) + + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) + + return meas_update, cross_correlation + + def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: + """ + Calculates the posteriori estimate using measurment and the prior estimate + """ + + nu_k = MeasModel() + + nu_k.measurement = measurement.measurement - ex_measuremnt.measurement + nu_k.covariance = ex_measuremnt.covariance + measurement.covariance + + K_k = np.dot(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))) + + self.process_model.state_vector_prev = posteriori_estimate + + return posteriori_estimate + +def add_quaternion_noise(q, noise_std): + + noise = np.random.normal(0, noise_std, 3) + + theta = np.linalg.norm(noise) + + if theta > 0: + + axis = noise / theta + + q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + + else: + + q_noise = np.array([1.0, 0.0, 0.0, 0.0]) + + q_new = quaternion_super_product(q, q_noise) + + return q_new / np.linalg.norm(q_new) + + +if __name__ == '__main__': + + # Create initial state vector and covariance matrix. + x0 = np.zeros(13) + x0[0:3] = [0.3, 0.3, 0.3] + x0[3] = 1 + x0[7:10] = [0.2, 0.2, 0.2] + dt = 0.01 + R = (0.1 / dt) * np.eye(3) + + Q = 0.1 * np.eye(12) + P0 = np.eye(12) * 0.1 + + model = process_model() + model.dt = 0.01 + 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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + model_ukf = model + + # Simulation parameters + simulation_time = 40 # seconds + num_steps = int(simulation_time / dt) + + # Initialize a dummy StateQuat. + test_state = StateQuat() + test_state.fill_states(x0) + test_state.covariance = P0 + + # Initialize a estimated state + estimated_state = StateQuat() + estimated_state.fill_states(x0) + estimated_state.covariance = P0 + + # Initialize a estimated state + noisy_state = StateQuat() + noisy_state.fill_states(x0) + noisy_state.covariance = P0 + + measurment_model = MeasModel() + measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + measurment_model.covariance = R + + # Initialize arrays to store the results + positions = np.zeros((num_steps, 3)) + orientations = np.zeros((num_steps, 3)) + velocities = np.zeros((num_steps, 3)) + angular_velocities = np.zeros((num_steps, 3)) + + # Initialize arrays to store the estimates + positions_est = np.zeros((num_steps, 3)) + orientations_est = np.zeros((num_steps, 3)) + velocities_est = np.zeros((num_steps, 3)) + angular_velocities_est = np.zeros((num_steps, 3)) + + # Initialize the okid params + okid_params = np.zeros((num_steps, 21)) + + model.state_vector_prev = test_state + model.state_vector = test_state + + model_ukf.state_vector_prev = test_state + model_ukf.state_vector = test_state + + # initialize the ukf + ukf = UKF(model_ukf, x0, P0, Q, R) + + # Test + ukf.unscented_transform(test_state) + + elapsed_times = [] + + u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # Run the simulation + for step in range(num_steps): + # Insert control input + model.Control_input = u(step * dt) + model_ukf.Control_input = u(step * dt) + + # Perform the unscented transform + model.model_prediction(test_state) + new_state = model.euler_forward() + + # Adding noise in the state vector + noisy_state.position = new_state.position + np.random.normal(0, 0.1, 3) + noisy_state.orientation = add_quaternion_noise(new_state.orientation, 0.1) + noisy_state.velocity = new_state.velocity + np.random.normal(0, 0.1, 3) + noisy_state.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.1, 3) + + start_time = time.time() + estimated_state = ukf.unscented_transform(noisy_state) + elapsed_time = time.time() - start_time + elapsed_times.append(elapsed_time) + + if step % 20 == 0: + measurment_model.measurement = new_state.velocity + np.random.normal(0, 0.2, 3) + meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + positions[step, :] = new_state.position + orientations[step, :] = quat_to_euler(new_state.orientation) + velocities[step, :] = new_state.velocity + angular_velocities[step, :] = new_state.angular_velocity + + positions_est[step, :] = estimated_state.position + orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + velocities_est[step, :] = estimated_state.velocity + angular_velocities_est[step, :] = estimated_state.angular_velocity + + # Update the state for the next iteration + model.state_vector_prev = new_state + + print('Average elapsed time: ', np.mean(elapsed_times)) + print('Max elapsed time: ', np.max(elapsed_times)) + print('Min elapsed time: ', np.min(elapsed_times)) + print('median elapsed time: ', np.median(elapsed_times)) + # Plot the results + time = np.linspace(0, simulation_time, num_steps) + + # Plot positions + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, positions[:, 0], label='True') + plt.plot(time, positions_est[:, 0], label='Estimated') + plt.title('Position X') + plt.xlabel('Time [s]') + plt.ylabel('Position X [m]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, positions[:, 1], label='True') + plt.plot(time, positions_est[:, 1], label='Estimated') + plt.title('Position Y') + plt.xlabel('Time [s]') + plt.ylabel('Position Y [m]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, positions[:, 2], label='True') + plt.plot(time, positions_est[:, 2], label='Estimated') + plt.title('Position Z') + plt.xlabel('Time [s]') + plt.ylabel('Position Z [m]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot orientations (Euler angles) + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, orientations[:, 0], label='True') + plt.plot(time, orientations_est[:, 0], label='Estimated') + plt.title('Orientation Roll') + plt.xlabel('Time [s]') + plt.ylabel('Roll [rad]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, orientations[:, 1], label='True') + plt.plot(time, orientations_est[:, 1], label='Estimated') + plt.title('Orientation Pitch') + plt.xlabel('Time [s]') + plt.ylabel('Pitch [rad]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, orientations[:, 2], label='True') + plt.plot(time, orientations_est[:, 2], label='Estimated') + plt.title('Orientation Yaw') + plt.xlabel('Time [s]') + plt.ylabel('Yaw [rad]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, velocities[:, 0], label='True') + plt.plot(time, velocities_est[:, 0], label='Estimated') + plt.title('Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Velocity X [m/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, velocities[:, 1], label='True') + plt.plot(time, velocities_est[:, 1], label='Estimated') + plt.title('Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Y [m/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, velocities[:, 2], label='True') + plt.plot(time, velocities_est[:, 2], label='Estimated') + plt.title('Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Z [m/s]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot angular velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, angular_velocities[:, 0], label='True') + plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + plt.title('Angular Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity X [rad/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, angular_velocities[:, 1], label='True') + plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + plt.title('Angular Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Y [rad/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, angular_velocities[:, 2], label='True') + plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + plt.title('Angular Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Z [rad/s]') + plt.legend() + + plt.tight_layout() + plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py b/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py new file mode 100644 index 000000000..86b7beb49 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py @@ -0,0 +1,568 @@ +from dataclasses import dataclass, field +import numpy as np + + +from dataclasses import dataclass, field +import numpy as np + +@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: np.ndarray = field(default_factory=lambda: np.zeros(21)) + 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]) + + 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] + + if len(state) > 13: + self.okid_params = state[13:] + + 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] + + if len(state) > 13: + self.okid_params = state[13:] + + def subtract(self, other: 'StateQuat') -> 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + new_array[12:] = self.okid_params - other.okid_params + + 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, 34)) + H[:, 7:10] = np.eye(3) + z_i = MeasModel() + z_i.measurement = np.dot(H, state.as_vector()) + 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_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))) + 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 + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + D_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + + 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, linear_damping: np.ndarray, nonlinear_damping: np.ndarray) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(linear_damping) + D_nl = -np.diag(nonlinear_damping) * 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.inertia = np.diag(self.state_vector.okid_params[:3]) + self.mass_interia_matrix[3:6, 3:6] = self.inertia + self.D_matrix = self.D(self.state_vector.okid_params[3:9], self.state_vector.okid_params[9:15]) + self.added_mass = self.state_vector.okid_params[15:21] + + 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_matrix, self.state_vector.nu())) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + self.state_vector_dot.okid_params = np.zeros(21) + + 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 + self.state_vector.okid_params = self.state_vector_prev.okid_params + 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], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: + """ + Computes the weighted mean of the quaternion orientations from a list of StateQuat objects + using an iterative approach, without requiring the caller to manually extract the quaternion. + + Parameters: + state_list (list[StateQuat]): List of StateQuat objects. + weights (np.ndarray): Weights for each state. + tol (float): Convergence tolerance. + max_iter (int): Maximum number of iterations. + + Returns: + np.ndarray: The averaged quaternion as a 4-element numpy array. + """ + # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] + + # Initialize the mean quaternion with the first quaternion + mean_q = sigma_quats[0].copy() + + for _ in range(max_iter): + weighted_error_vectors = [] + for i, q in enumerate(sigma_quats): + # Compute the error quaternion: e = q * inv(mean_q) + # For unit quaternions, the inverse is the conjugate. + 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) + + # Clip to avoid numerical issues + 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: + # Compute the full rotation vector (angle * axis) + error_vec = (angle / np.sin(angle / 2)) * e[1:4] + weighted_error_vectors.append(weights[i] * error_vec) + + error_avg = np.sum(weighted_error_vectors, axis=0) + if np.linalg.norm(error_avg) < tol: + break + + error_norm = np.linalg.norm(error_avg) + delta_q = (np.array([np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + if error_norm > 0 else 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], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement + +def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + covariance = np.zeros((n, n)) + mean_quat = StateQuat() + mean_quat.fill_states(mean) + + if weights is None: + for i in range(2 * n + 1): + covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + covariance = (1 / (2 * n + 1)) * covariance + + else: + for i in range(2 * n + 1): + covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + return covariance + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> 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 + + if weights is None: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += np.outer(temp_model.measurement, temp_model.measurement) + + covariance = (1 / (n)) * covariance + + else: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) + + return covariance + +def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: + """ + Calculates the cross covariance between the measurement and state prediction + """ + + n = len(mean_y) - 1 + m = len(mean_z) + cross_covariance = np.zeros((n,m)) + mean_quat = StateQuat() + mean_quat.fill_states(mean_y) + + for i in range(n): + cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + + cross_covariance = (1 / len(set_y)) * cross_covariance + + return cross_covariance 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..8444fd82e --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -0,0 +1,464 @@ +from dataclasses import dataclass, field +import numpy as np + + +from dataclasses import dataclass, field +import numpy as np + +@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)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + + 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]) + + 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] + + 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] + + def subtract(self, other: 'StateQuat') -> 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + 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 + + 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 + + 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 + + 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] + + 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 + +@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.as_vector()) + 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 + +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], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: + """ + Computes the weighted mean of the quaternion orientations from a list of StateQuat objects + using an iterative approach, without requiring the caller to manually extract the quaternion. + + Parameters: + state_list (list[StateQuat]): List of StateQuat objects. + weights (np.ndarray): Weights for each state. + tol (float): Convergence tolerance. + max_iter (int): Maximum number of iterations. + + Returns: + np.ndarray: The averaged quaternion as a 4-element numpy array. + """ + # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] + + # Initialize the mean quaternion with the first quaternion + mean_q = sigma_quats[0].copy() + + for _ in range(max_iter): + weighted_error_vectors = [] + for i, q in enumerate(sigma_quats): + # Compute the error quaternion: e = q * inv(mean_q) + # For unit quaternions, the inverse is the conjugate. + 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) + + # Clip to avoid numerical issues + 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: + # Compute the full rotation vector (angle * axis) + error_vec = (angle / np.sin(angle / 2)) * e[1:4] + weighted_error_vectors.append(weights[i] * error_vec) + + error_avg = np.sum(weighted_error_vectors, axis=0) + if np.linalg.norm(error_avg) < tol: + break + + error_norm = np.linalg.norm(error_avg) + delta_q = (np.array([np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + if error_norm > 0 else 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], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement + +def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + covariance = np.zeros((n, n)) + mean_quat = StateQuat() + mean_quat.fill_states(mean) + + if weights is None: + for i in range(2 * n + 1): + covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + covariance = (1 / (2 * n + 1)) * covariance + + else: + for i in range(2 * n + 1): + covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + return covariance + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> 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 + + if weights is None: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += np.outer(temp_model.measurement, temp_model.measurement) + + covariance = (1 / (n)) * covariance + + else: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) + + return covariance + +def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: + """ + Calculates the cross covariance between the measurement and state prediction + """ + + n = len(mean_y) - 1 + m = len(mean_z) + cross_covariance = np.zeros((n,m)) + mean_quat = StateQuat() + mean_quat.fill_states(mean_y) + + for i in range(n): + cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + + cross_covariance = (1 / len(set_y)) * cross_covariance + + return cross_covariance 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..f52f2eb62 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -0,0 +1,36 @@ +import numpy as np +from dataclasses import dataclass +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_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) From 2a3daba296c16810d8f73c8278154d6f9ada03e2 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 14 Mar 2025 23:04:40 +0100 Subject: [PATCH 06/19] added in some changes to eskf and the ukf algorithm --- .../eskf_python/eskf_python_class.py | 215 +++++++ .../eskf_python/eskf_python_filter.py | 600 +++++------------- .../eskf_python/eskf_python_node.py | 2 +- .../eskf_python/eskf_python_utils.py | 95 +++ .../eskf_python/ukf_okid_class.py} | 110 +--- navigation/sp_ukf_python/CMakeLists.txt | 33 - navigation/sp_ukf_python/README.md | 0 .../sp_ukf_python/config/sp_ukf_python.yaml | 3 - navigation/sp_ukf_python/launch/ukf.launch.py | 22 - navigation/sp_ukf_python/package.xml | 23 - .../sp_ukf_python/sp_ukf_python/__init__.py | 0 .../sp_ukf_python/sp_ukf_python.py | 495 --------------- .../sp_ukf_python/sp_ukf_python_class.py | 292 --------- .../sp_ukf_python/sp_ukf_python_node.py | 137 ---- .../sp_ukf_python/sp_ukf_python_utils.py | 116 ---- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 313 --------- navigation/ukf_okid/ukf_python/ukf_okid.py | 87 ++- 17 files changed, 551 insertions(+), 1992 deletions(-) create mode 100644 navigation/eskf_python/eskf_python/eskf_python_class.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_utils.py rename navigation/{ukf_okid/ukf_python/ukf_okid_class copy.py => eskf_python/eskf_python/ukf_okid_class.py} (78%) delete mode 100644 navigation/sp_ukf_python/CMakeLists.txt delete mode 100644 navigation/sp_ukf_python/README.md delete mode 100644 navigation/sp_ukf_python/config/sp_ukf_python.yaml delete mode 100644 navigation/sp_ukf_python/launch/ukf.launch.py delete mode 100644 navigation/sp_ukf_python/package.xml delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/__init__.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/test_ukf.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py new file mode 100644 index 000000000..949ab996d --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -0,0 +1,215 @@ +from dataclasses import dataclass, field +from typing import Tuple, List +from scipy.linalg import expm +import numpy as np +from eskf_python_utils import skew_matrix, quaternion_product + + +@dataclass +class StateQuat: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.array([1, 0, 0, 0]) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + g: np.ndarray = field( + default_factory=lambda: np.array([0, 0, 0]) + ) # Gravity vector + + def as_vector(self) -> np.ndarray: + """Returns the state vector as a numpy array. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + self.g, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:10] + self.acceleration_bias = state[10:13] + self.gyro_bias = state[13:16] + self.g = state[16:19] + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + 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 inject(self, EulerState: 'StateEuler') -> 'StateQuat': + # inj_state = StateQuat() + + # # Injecting the error state + # inj_state.position = self.position + EulerState.position + # inj_state.velocity = self.velocity + EulerState.velocity + # inj_state.orientation = quaternion_product( + # self.orientation, + # 0.5 + # * np.array( + # [ + # 2, + # EulerState.orientation[0], + # EulerState.orientation[1], + # EulerState.orientation[2], + # ] + # ), + # ) + # inj_state.acceleration_bias = self.acceleration_bias + EulerState.acceleration_bias + # inj_state.gyro_bias = self.gyro_bias + EulerState.gyro_bias + + # return inj_state + + + +@dataclass +class StateEuler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + g: np.ndarray = field( + default_factory=lambda: np.array([0, 0, 9.81]) + ) # Gravity vector + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((18, 18)) + ) # Covariance matrix + + def as_vector(self) -> np.ndarray: + """Returns the state vector as a numpy array. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + self.g, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + self.g = state[15:18] + + def copy_state(self, wanted_state: 'StateEuler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + H = np.zeros((3, 15)) + + H[0:3, 3:6] = np.eye(3) + + return H + +@dataclass +class Measurement: + acceleration: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + angular_velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + aiding: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + + aiding_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((3, 3)) + ) \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index edf39dcc0..a9c772e47 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -1,503 +1,249 @@ -from dataclasses import dataclass, field -from typing import tuple +# from dataclasses import dataclass +from typing import Tuple import numpy as np from scipy.linalg import expm +from eskf_python_class import StateEuler, StateQuat, Measurement +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion +from ukf_okid_class import euler_to_quat +from scipy.linalg import block_diag + +class ESKF: + def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): + self.Q = Q + self.Hx = Hx # Jacobian of the measurement model + self.dt = dt + self.nom_state = nom_state + self.error_state = StateEuler() + self.error_state.covariance = P0 + self.p_accBias = p_accBias + self.p_gyroBias = p_gyroBias + + def Fx(self, imu_data: Measurement) -> np.ndarray: + """Calculates the state transition matrix. -@dataclass -class StateVector_quaternion: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(4) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. + Args: + imu_data (np.ndarray): The IMU data. Returns: - np.ndarray: The rotation matrix. + np.ndarray: The state transition matrix. """ - 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 + F_x = np.zeros((18, 18)) + I = np.eye(3) + + F_x[0:3, 0:3] = I + F_x[0:3, 3:6] = self.dt * I + F_x[3:6, 3:6] = I + F_x[3:6, 6:9] = -self.nom_state.R_q() @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) * self.dt + F_x[6:9, 6:9] = R_from_angle_axis((imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt).T + F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt + F_x[3:6, 15:18] = I * self.dt + F_x[6:9, 12:15] = -I * self.dt + F_x[9:12, 9:12] = I + F_x[12:15, 12:15] = I + F_x[15:18, 15:18] = I + + return F_x + + def Fi(self) -> np.ndarray: + """Calculates the input matrix. + + Returns: + np.ndarray: The input matrix. + """ - def euler_forward( - self, current_state: 'StateVector_quaternion', dt: float - ) -> 'StateVector_quaternion': - # Define the new state - new_state = StateVector_quaternion() + F_i = np.zeros((18, 12)) + I = np.eye(3) - # Define the state derivatives - new_state.position = current_state.position + self.position * dt - new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = current_state.orientation + self.orientation * dt - new_state.acceleration_bias = ( - current_state.acceleration_bias + self.acceleration_bias * dt - ) - new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt - - # Normalize the orientation quaternion - new_state.orientation /= np.linalg.norm(new_state.orientation) - - return new_state - - -@dataclass -class StateVector_euler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((15, 15)) - ) # Covariance matrix - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. + F_i[3:6, 0:3] = I + F_i[6:9, 3:6] = I + F_i[9:12, 6:9] = I + F_i[12:15, 9:12] = I - Args: - state (np.ndarray): The state vector. + return F_i + + def Q_delta_theta(self) -> np.ndarray: + """ + Calculates the Q_delta_theta matrix. + See Joan Solà. Quaternion kinematics for the error-state Kalman filter. + chapter: 6.1.1 eq. 281 """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - def copy_state(self, wanted_state: 'StateVector_euler') -> None: - """Copies the state from a StateVector object into the current StateVector object. + qw, qx, qy, qz = self.nom_state.orientation - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) # Measurement vector - measurement_matrix: np.ndarray = field( - default_factory=lambda: np.zeros((6, 15)) - ) # Measurement matrix - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix - - -class ErrorStateKalmanFilter: - def __init__( - self, - P_ab: np.ndarray, - P_wb: np.ndarray, - Q: np.ndarray, - lever_arm: np.array, - R: np.ndarray, - g: float, - dt: float, - ) -> None: - self.P_ab = P_ab - self.P_wb = P_wb - self.Q_process_noise = Q - self.lever_arm = lever_arm - self.R = R - self.g = np.array([0, 0, g]) - self.dt = dt + Q_delta_theta = 0.5 * np.array([ + [-qx, -qy, -qz], + [qw, -qz, qy], + [qz, qw, -qx], + [-qy, qx, qw], + ]) - def skew_symmetric(self, vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. + return Q_delta_theta - Args: - vector (np.ndarray): The vector. + def H(self) -> np.ndarray: + """Calculates the measurement matrix. Returns: - np.ndarray: The skew symmetric matrix. + np.ndarray: The measurement matrix. """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - def quaternion_super_product(self, q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. + X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + H = self.Hx @ X_deltax + + return H + + def h(self) -> np.ndarray: + """ + Calculates the measurement model. Returns: - np.ndarray: The quaternion super product. + np.ndarray: The measurement model. """ - nu_0, eta_0_x, eta_0_y, eta_0_z = q1 - nu_1, eta_1_x, eta_1_y, eta_1_z = q2 + return self.nom_state.velocity - eta_0 = np.array([[eta_1_x, eta_1_y, eta_1_z]]).T - eta_1 = np.array([[eta_0_x, eta_0_y, eta_0_z]]).T - eta_new = ( - nu_1 * eta_0 + nu_0 * eta_1 + np.dot(self.skew_symmetric(eta_0), eta_1) - ) - nu_new = nu_0 * nu_1 - np.dot(eta_0.T, eta_1) - q_new = np.array([nu_new, eta_new[0], eta_new[1], eta_new[2]]) - q_new /= np.linalg.norm(q_new) + def nominal_state_discrete(self, imu_data: Measurement) -> None: + """ + Calculates the next nominal state using the discrete-time process model defined in: + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter: 5.4.1 The nominal state kinematics - return q_new + Args: + imu_data (np.ndarray): The IMU data. + """ - def van_loan_discretization( - self, A_c: np.ndarray, G_c: np.ndarray - ) -> tuple[np.ndarray, np.ndarray]: - """Calculates the Van Loan discretization of a continuous-time system. + # Rectify measurements. + acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias + gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias + + R = self.nom_state.R_q() + + self.nom_state.position = self.nom_state.position + self.nom_state.velocity * self.dt + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 + self.nom_state.velocity = self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt + self.nom_state.orientation = quaternion_product(self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt)) + self.nom_state.acceleration_bias = np.exp(-self.p_accBias * self.dt) * self.nom_state.acceleration_bias + self.nom_state.gyro_bias = np.exp(-self.p_gyroBias * self.dt) * self.nom_state.gyro_bias + self.nom_state.g = self.nom_state.g + + def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: + """ + Calculates the Van Loan discretization of a continuous-time system. Args: A_c (np.ndarray): The A matrix. G_c (np.ndarray): The G matrix. Returns: - tuple: The A_d and GQG_d matrices. + Tuple: The A_d and GQG_d matrices. """ - GQG_T = np.dot(np.dot(G_c, self.Q_process_noise), G_c.T) * self.dt + + GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) matrix_exp = ( - np.block([[A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), A_c.T]]) + np.block([[- A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)]]) * self.dt ) van_loan_matrix = expm(matrix_exp) - V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] - V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] + V1 = van_loan_matrix[A_c.shape[0]:, A_c.shape[0]:] + V2 = van_loan_matrix[:A_c.shape[0], A_c.shape[0]:] A_d = V1.T GQG_d = A_d @ V2 return A_d, GQG_d - def nominal_state_update( - self, current_state: StateVector_quaternion, imu_reading: np.ndarray - ) -> StateVector_quaternion: - """Updates the nominal state of the system. + def error_state_prediction(self, imu_data: Measurement) -> None: - Args: - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. + # Rectify measurements. + acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias + gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - Returns: - np.ndarray: The updated nominal state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - # Define the derivative of the state - current_state_dot = StateVector_quaternion() - - # Define the state derivates - current_state_dot.position = current_state.velocity - current_state_dot.velocity = ( - np.dot( - current_state.R_q(), - (imu_acceleration - current_state.acceleration_bias), - ) - + self.g - ) + R = self.nom_state.R_q() - # Define the quaternion derivatives - current_state_dot.orientation = 0.5 * self.quaternion_super_product( - current_state.orientation, - np.array([0, imu_gyro[0], imu_gyro[1], imu_gyro[2]]), - ) + A_c = np.zeros((18, 18)) - # Define the bias - current_state_dot.acceleration_bias = ( - -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias - ) - current_state_dot.gyro_bias = ( - -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias - ) - - return current_state_dot.euler_forward(current_state, self.dt) - - def error_state_update( - self, - current_error_state: StateVector_euler, - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Updates the error state of the system. - - Args: - current_error_state (np.ndarray): The current error state of the system. - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated error state. - """ - # Define the derivative of the state - next_error_state = StateVector_euler() - - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - A_c = np.zeros((15, 15)) A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -np.dot( - current_state.R_q(), - self.skew_symmetric(imu_acceleration - current_state.acceleration_bias), - ) - A_c[6:9, 6:9] = -self.skew_symmetric(imu_gyro - current_state.gyro_bias) - A_c[3:6, 9:12] = -current_state.R_q() + A_c[3:6, 6:9] = - R @ skew_matrix(acc_rect) + A_c[6:9, 6:9] = - skew_matrix(gyro_rect) + A_c[3:6, 9:12] = - R + A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) + A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) A_c[6:9, 12:15] = -np.eye(3) - A_c[9:12, 9:12] = -self.P_ab * np.eye(3) - A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + A_c[3:6, 15:18] = np.eye(3) + + G_c = np.zeros((18, 12)) - G_c = np.zeros((15, 12)) - G_c[3:6, 0:3] = -current_state.R_q() + G_c[3:6, 0:3] = -R G_c[6:9, 3:6] = -np.eye(3) G_c[9:12, 6:9] = np.eye(3) G_c[12:15, 9:12] = np.eye(3) - # Van loan discretization - A_d, GQG_d = self.van_loan_discretization(A_c, G_c, self.dt) + A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - # Inserting the new state and covariance - next_error_state.copy_state(current_error_state) - next_error_state.covariance = ( - np.dot(np.dot(A_d, current_error_state.covariance), A_d.T) + GQG_d - ) - - return next_error_state - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. + self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) - Returns: - np.ndarray: The measurement matrix. + def measurement_update(self, dvl_measurement:Measurement) -> None: """ - # Define the measurement matrix - H = np.zeros((3, 15)) - - # For now assume only velocity is measured - H[0:3, 3:6] = np.eye(3) - - return H - - def prediction_from_estimates( - self, - current_state: StateVector_quaternion, - current_error_state: StateVector_euler, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Predicts the measurement from the current state and error state. + Updates the error state using the DVL measurement. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter: 6.1 eq. 274-276 Args: - current_state (StateVector_quaternion): The current state of the system. - current_error_state (StateVector_euler): The current error state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - StateVector_euler: The predicted measurement. + dvl_measurement (np.ndarray): The DVL measurement. """ - # Define the z_pred matrix - z_pred = MeasurementModel() - - # Define the z_pred values separately - z_pred_1 = current_state.velocity - z_pred_2 = 0 # Currently assuming no lever arm compensation - - # Combine the z_pred values - z_pred.measurement = z_pred_1 + z_pred_2 - - # Define the H matrix - z_pred.measurement_matrix = self.H() - R = self.R - z_pred.measurement_covariance = ( - np.dot( - np.dot(z_pred.measurement_matrix, current_error_state.covariance), - z_pred.measurement_matrix.T, - ) - + R - ) - - return z_pred - def measurement_update( - self, - error_state_pred: StateVector_euler, - z_pred: MeasurementModel, - dvl_measure: np.array, - ) -> StateVector_euler: - """Updates the error state of the system. + H = self.H() + P = self.error_state.covariance + R= dvl_measurement.aiding_covariance + K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) + self.error_state.fill_states(K @ (dvl_measurement.aiding - self.h())) + self.error_state.covariance = (np.eye(18) - K @ H) @ P - Args: - current_error_state (np.ndarray): The current error state of the system. - measurement (np.ndarray): The measurement. - - Returns: - np.ndarray: The updated error state. + def injection(self) -> None: """ - # Define new error state value - new_error_state = StateVector_euler() - - # Define the measurement matrix - innovation = dvl_measure - z_pred.measurement - H = z_pred.measurement_matrix - R = self.R - P = error_state_pred.covariance - S = z_pred.measurement_covariance - - # Kalman gain calculation - W = np.dot(P, np.linalg.solve(S, H).T) - new_error_state.fill_states(np.dot(W, innovation)) - - I_WH = np.eye(15) - np.dot(W, H) - new_error_state.covariance = np.dot(np.dot(I_WH, P), I_WH.T) + np.dot( - np.dot(W, R), W.T - ) - - return new_error_state - - def imu_update_states( - self, - current_pred_nom: StateVector_quaternion, - current_pred_err: StateVector_euler, - imu_readings: np.array, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Calculates the predicted state using the IMU readings. - - Args: - current_pred_nom (StateVector_quaternion): The current nominal state. - current_pred_err (StateVector_euler): The current error state. - imu_readings (np.array): The IMU readings. - - Returns: - tuple: The predicted nominal state and the predicted error state. + Injects the error state into the nominal state to produce the estimated state. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter 6.2 eq. 282-283 + """ - pred_nom_state = self.nominal_state_update(current_pred_nom, imu_readings) - pred_err_state = self.error_state_update( - current_pred_err, current_pred_nom, imu_readings - ) - - return pred_nom_state, pred_err_state - - def dvl_update_states( - self, - current_pred_nom: StateVector_quaternion, - current_pred_err: StateVector_euler, - dvl_measure: np.array, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Calculates the predicted state using the DVL readings. - - Args: - current_pred_nom (StateVector_quaternion): The current nominal state. - current_pred_err (StateVector_euler): The current error state. - dvl_measure (np.array): The DVL readings. - - Returns: - tuple: The predicted nominal state and the predicted error state. + + self.nom_state.position = self.nom_state.position + self.error_state.position + self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity + self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) + self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.error_state.acceleration_bias + self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias + self.nom_state.g = self.nom_state.g + self.error_state.g + + def reset_error_state(self) -> None: + """ + Resets the error state after injection. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter 6.3 eq. 284-286 """ - z_pred = self.prediction_from_estimates( - current_pred_nom, current_pred_err, dvl_measure - ) - new_error_state = self.measurement_update(current_pred_err, z_pred, dvl_measure) - return current_pred_nom, new_error_state + G = np.eye(18) # Neglecting the delta_theta as this is most common in practice - def injection_and_reset( - self, next_state: StateVector_quaternion, next_error_state: StateVector_euler - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Injects the error state into the nominal state and resets the error state. + self.error_state.covariance = G @ self.error_state.covariance @ G.T + self.error_state.fill_states(np.zeros(18)) - Args: - next_state (StateVector_quaternion): The next nominal state. - next_error_state (StateVector_euler): The next error state. - - Returns: - tuple: The injected nominal state and the reset error state. + def imu_update(self, imu_data: Measurement) -> None: + """ + Updates the state using the IMU data. """ - # Define the new state - inj_state = StateVector_quaternion() - - # Injecting the error state - inj_state.position = next_state.position + next_error_state.position - inj_state.velocity = next_state.velocity + next_error_state.velocity - inj_state.orientation = self.quaternion_super_product( - next_state.orientation, - 0.5 - * np.array( - [ - 2, - next_error_state.orientation[0], - next_error_state.orientation[1], - next_error_state.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - next_state.acceleration_bias + next_error_state.acceleration_bias - ) - inj_state.gyro_bias = next_state.gyro_bias + next_error_state.gyro_bias - - # Resetting the error state - G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - self.skew_symmetric( - 0.5 * next_error_state.orientation - ) - - next_error_state.covariance = np.dot( - np.dot(G, next_error_state.covariance), G.T - ) - next_error_state.fill_states(np.zeros(15)) - return inj_state, next_error_state + self.nominal_state_discrete(imu_data) + self.error_state_prediction(imu_data) + + def dvl_update(self, dvl_measurement: Measurement) -> None: + """ + Updates the state using the DVL measurement. + """ + + self.measurement_update(dvl_measurement) + self.injection() + self.reset_error_state() \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index 5b860582e..ec206ab64 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -4,7 +4,7 @@ from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu, +from sensor_msgs.msg import Imu import numpy as np from geometry_msgs.msg import TwistWithCovarianceStamped diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py new file mode 100644 index 000000000..9ea439982 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -0,0 +1,95 @@ +import numpy as np + +def skew_matrix(vector: np.ndarray) -> np.ndarray: + """ + Returns the skew symmetric matrix of a 3x1 vector. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0] + ] + ) + +def quaternion_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 - np.dot(e_0, e_1) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) + + q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new = q_new / np.linalg.norm(q_new) + + return q_new + +def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: + """Converts an angle-axis representation to a quaternion. + + Args: + vector (np.ndarray): The angle-axis representation. + + Returns: + np.ndarray: The quaternion representation. + """ + angle = np.linalg.norm(vector) + if angle < 1e-8: + return np.array([1, 0, 0, 0]) + else: + axis = vector / angle + + + q = np.zeros(4) + q[0] = np.cos(angle / 2) + q[1:] = np.sin(angle / 2) * axis + + return q + + +def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: + """Calculates the rotation matrix from the angle-axis representation. + + Args: + vector (np.ndarray): The angle-axis representation. + + Returns: + np.ndarray: The rotation matrix. + """ + quaternion = angle_axis_to_quaternion(vector) + q0, q1, q2, q3 = quaternion + + 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 diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py b/navigation/eskf_python/eskf_python/ukf_okid_class.py similarity index 78% rename from navigation/ukf_okid/ukf_python/ukf_okid_class copy.py rename to navigation/eskf_python/eskf_python/ukf_okid_class.py index 86b7beb49..8444fd82e 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py +++ b/navigation/eskf_python/eskf_python/ukf_okid_class.py @@ -14,12 +14,11 @@ class StateQuat: 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: np.ndarray = field(default_factory=lambda: np.zeros(21)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) 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]) + return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -42,9 +41,6 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - if len(state) > 13: - self.okid_params = state[13:] - 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] @@ -52,9 +48,6 @@ def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] - if len(state) > 13: - self.okid_params = state[13:] - def subtract(self, other: 'StateQuat') -> np.ndarray: """Subtracts two StateQuat objects, returning the difference with Euler angles.""" new_array = np.zeros(len(self.as_vector()) - 1) @@ -63,8 +56,6 @@ def subtract(self, other: 'StateQuat') -> np.ndarray: new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity - new_array[12:] = self.okid_params - other.okid_params - return new_array def __add__(self, other: 'StateQuat') -> 'StateQuat': @@ -75,8 +66,6 @@ def __add__(self, other: 'StateQuat') -> 'StateQuat': 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': @@ -87,8 +76,6 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': 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': @@ -99,8 +86,6 @@ def __rmul__(self, scalar: float) -> 'StateQuat': 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: @@ -110,7 +95,6 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: 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() @@ -119,7 +103,6 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.position += other.position self.velocity += other.velocity self.angular_velocity += other.angular_velocity - self.okid_params += other.okid_params @dataclass class MeasModel: @@ -131,7 +114,7 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" - H = np.zeros((3, 34)) + H = np.zeros((3, 13)) H[:, 7:10] = np.eye(3) z_i = MeasModel() z_i.measurement = np.dot(H, state.as_vector()) @@ -233,93 +216,6 @@ def euler_forward(self) -> StateQuat: 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_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))) - 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 - prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - D_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - - 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, linear_damping: np.ndarray, nonlinear_damping: np.ndarray) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(linear_damping) - D_nl = -np.diag(nonlinear_damping) * 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.inertia = np.diag(self.state_vector.okid_params[:3]) - self.mass_interia_matrix[3:6, 3:6] = self.inertia - self.D_matrix = self.D(self.state_vector.okid_params[3:9], self.state_vector.okid_params[9:15]) - self.added_mass = self.state_vector.okid_params[15:21] - - 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_matrix, self.state_vector.nu())) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - self.state_vector_dot.okid_params = np.zeros(21) - - 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 - self.state_vector.okid_params = self.state_vector_prev.okid_params - return self.state_vector - - - def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: """ Converts Euler angles to a quaternion diff --git a/navigation/sp_ukf_python/CMakeLists.txt b/navigation/sp_ukf_python/CMakeLists.txt deleted file mode 100644 index a40f065cd..000000000 --- a/navigation/sp_ukf_python/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(sp_ukf_python) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - sp_ukf_python/sp_ukf_python_node.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) -endif() - -ament_package() diff --git a/navigation/sp_ukf_python/README.md b/navigation/sp_ukf_python/README.md deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/sp_ukf_python/config/sp_ukf_python.yaml b/navigation/sp_ukf_python/config/sp_ukf_python.yaml deleted file mode 100644 index d3d18145d..000000000 --- a/navigation/sp_ukf_python/config/sp_ukf_python.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - sp_ukf_python_node: diff --git a/navigation/sp_ukf_python/launch/ukf.launch.py b/navigation/sp_ukf_python/launch/ukf.launch.py deleted file mode 100644 index fdd3f07e6..000000000 --- a/navigation/sp_ukf_python/launch/ukf.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - sp_ukf_python_node = Node( - package='sp_ukf_python', - executable='sp_ukf_python_node.py', - name='sp_ukf_python_node', - parameters=[ - os.path.join( - get_package_share_directory('sp_ukf_python'), - 'config', - 'sp_ukf_python.yaml', - ), - ], - output='screen', - ) - return LaunchDescription([sp_ukf_python_node]) diff --git a/navigation/sp_ukf_python/package.xml b/navigation/sp_ukf_python/package.xml deleted file mode 100644 index 6aa4edbc0..000000000 --- a/navigation/sp_ukf_python/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - sp_ukf_python - 1.0.0 - This package provides the implementation of a sigma point based Unscented Error-state Kalman Filter - talhanc - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - - ament_cmake - - diff --git a/navigation/sp_ukf_python/sp_ukf_python/__init__.py b/navigation/sp_ukf_python/sp_ukf_python/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py deleted file mode 100644 index 819cb737d..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ /dev/null @@ -1,495 +0,0 @@ - -import numpy as np -from scipy.linalg import expm -from sp_ukf_python_class import StateVector_euler, StateVector_quaternion -from sp_ukf_python_utils import quaternion_super_product, skew_symmetric - - -class ErrorStateUnscentedKalmanFilter: - def __init__( - self, - P_ab: float, - P_wb: float, - Q: np.ndarray, - lever_arm: np.array, - R: np.ndarray, - g: float, - dt: float, - ) -> None: - self.P_ab = P_ab - self.P_wb = P_wb - self.Q_process_noise = Q - self.lever_arm = lever_arm - self.R = R - self.g = np.array([0, 0, g]) - self.dt = dt - self.y_i = np.zeros((15, 2 * 15)) - self.W = np.zeros(2 * 15 + 1) - - def mean_set(self, set: np.ndarray) -> np.ndarray: - """Calculates the mean of a set of values. - - Args: - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The mean of the set. - """ - # Define the number of sigma points based on columns - n = set.shape[0] - mean_value = np.zeros(n) - - for i in range(2 * n + 1): - mean_value += (1/(2 * n + 1)) * set[:, i] - - return mean_value - - def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: - """Calculates the mean of a set of values. - - Args: - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The mean of the set. - """ - # Define the number of columns - n = set.shape[0] - mean_value = np.zeros(n) - - for i in range(2 * n + 1): - mean_value += weight[i] * set[:, i] - - return mean_value - - def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: - """Calculate the covarince of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The covariance of the set. - """ - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2 * n + 1): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - - covariance_set += (1 / (2 * n + 1)) * np.outer(W_i, W_i) - - return covariance_set - - def cross_covariance_set( - self, - mean: np.ndarray, - set: np.ndarray, - mean_2: np.ndarray, - set_2: np.ndarray, - weight: np.ndarray, - ) -> np.ndarray: - """Calculate the cross covariance of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - mean_2 (np.ndarray): The mean of the second set. - set_2 (np.ndarray): The second set of values. - - Returns: - np.ndarray: The cross covariance of the set. - """ - n_x = set.shape[0] - n_z = set_2.shape[0] - covariance_mat = np.zeros((n_x, n_z)) - - for i in range(2 * n_x + 1): - # parse the 15-dim error state - err_vec = set[:, i] # shape (15,) - W_i = err_vec - mean # shape (15,) - - # parse the 3-dim measurement - meas_vec = set_2[:, i] # shape (3,) - W_i_2 = meas_vec - mean_2 # shape (3,) - - # outer product -> shape (15,3) - covariance_mat += weight[i] * np.outer(W_i, W_i_2) - - return covariance_mat - - def weighted_covariance_set( - self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray - ) -> np.ndarray: - """Calculate the covarince of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The covariance of the set. - """ - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2 * n + 1): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - covariance_set += weight[i] * np.outer(W_i, W_i) - - return covariance_set - - def generate_sigma_points( - self, error_state: StateVector_euler, Q_process_noise - ) -> tuple[list[StateVector_euler], np.ndarray]: - """Generates the sigma points for the UKF - This is done using the Cholesky decomposition method - """ - n = len(error_state.covariance) - kappa = 3 - n - - S = np.linalg.cholesky(error_state.covariance + Q_process_noise) - - S_scaled = np.sqrt(n + kappa) * S - - weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) - - sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] - W = np.zeros(2 * n + 1) - - sigma_points[0].fill_states(error_state.as_vector()) - W[0] = kappa / (n + kappa) - - for i in range(2 * n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - W[i + 1] = 1 / (2 * (n + kappa)) - - self.W = W - return sigma_points, W - - def nominal_state_update( - self, current_state: StateVector_quaternion, imu_reading: np.ndarray - ) -> StateVector_quaternion: - """Updates the nominal state of the system. - - Args: - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated nominal state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - # Define the derivative of the state - current_state_dot = StateVector_quaternion() - - # Define the state derivates - current_state_dot.position = current_state.velocity - current_state_dot.velocity = ( - np.dot( - current_state.R_q(), - (imu_acceleration - current_state.acceleration_bias), - ) - + self.g - ) - - # Define the quaternion derivatives - current_state_dot.orientation = 0.5 * quaternion_super_product( - current_state.orientation, - np.array( - [ - 0, - imu_gyro[0] - current_state.gyro_bias[0], - imu_gyro[1] - current_state.gyro_bias[1], - imu_gyro[2] - current_state.gyro_bias[2], - ] - ), - ) - - # Define the bias - current_state_dot.acceleration_bias = ( - -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias - ) - current_state_dot.gyro_bias = ( - -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias - ) - - return current_state_dot.euler_forward(current_state, self.dt) - - def error_state_update( - self, - current_error_state: StateVector_euler, - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> np.ndarray: - """Updates the error state of the system. - - Args: - current_error_state (np.ndarray): The current error state of the system. - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated error state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - A_c = np.zeros((15, 15)) - A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -np.dot( - current_state.R_q(), - skew_symmetric(imu_acceleration - current_state.acceleration_bias), - ) - A_c[6:9, 6:9] = -skew_symmetric(imu_gyro - current_state.gyro_bias) - A_c[3:6, 9:12] = -current_state.R_q() - A_c[6:9, 12:15] = -np.eye(3) - A_c[9:12, 9:12] = -self.P_ab * np.eye(3) - A_c[12:15, 12:15] = -self.P_wb * np.eye(3) - - # Exact matrix exponential - A_d = expm(A_c * self.dt) - - next_error_state = A_d @ current_error_state.as_vector() - - return next_error_state - - def unscented_transform( - self, - sigma_points: list[StateVector_euler], - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Performs the Unscented Transform - This is the corresponding to a preditction step in the EKF - """ - n = len(sigma_points[0].as_vector()) - - self.y_i = np.zeros((n, 2 * n + 1)) - - for i in range(2 * n + 1): - self.y_i[:, i] = self.error_state_update( - sigma_points[i], current_state, imu_reading - ) - - error_state_estimate = StateVector_euler() - - x = self.weighted_mean_set(self.y_i, self.W) - - error_state_estimate.fill_states(x) - error_state_estimate.covariance = self.weighted_covariance_set(x, self.y_i, self.W) - - return error_state_estimate - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - # Define the measurement matrix (error state is 15-dim) - H = np.zeros((3, 16)) - - # For now assume only velocity is measured (located at indices 3:6) - H[:, 3:6] = np.eye(3) - - return H - - def injection( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - ) -> StateVector_quaternion: - """Injects the error state into the nominal state - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - - Returns: - StateVector_quaternion: The updated nominal state - """ - inj_state = StateVector_quaternion() - - inj_state.position = current_state_nom.position + current_state_error.position - inj_state.velocity = current_state_nom.velocity + current_state_error.velocity - inj_state.orientation = quaternion_super_product( - current_state_nom.orientation, - 0.5 - * np.array( - [ - 2, - current_state_error.orientation[0], - current_state_error.orientation[1], - current_state_error.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - current_state_nom.acceleration_bias + current_state_error.acceleration_bias - ) - inj_state.gyro_bias = ( - current_state_nom.gyro_bias + current_state_error.gyro_bias - ) - - return inj_state - - def measurement_update( - self, - sigma_points: list[StateVector_euler], - current_nom_state: StateVector_quaternion, - current_error_state: StateVector_euler, - dvl_data: np.ndarray, - Weight: np.ndarray, - ) -> StateVector_euler: - """Updates the state vector with the DVL data - """ - H = self.H() - R = self.R - - n = len(sigma_points[0].as_vector()) - - Z_i = np.zeros((H.shape[0], 2 * n + 1)) - - for i in range(2 * n + 1): - injected_state = self.injection(current_nom_state, sigma_points[i]) - Z_i[:, i] = np.dot(H, injected_state.as_vector()) - - z = self.weighted_mean_set(Z_i, Weight) - S = self.weighted_covariance_set(z, Z_i, Weight) - - x = self.mean_set(self.y_i) - - innovation = dvl_data - z - - P_innovation = S + R - - P_xz = self.cross_covariance_set(x, self.y_i, z, Z_i, Weight) - - # Kalman gain - K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) - - updated_error_state = StateVector_euler() - - # Update the state - updated_error_state.fill_states(x + np.dot(K_k, innovation)) - - # Update the covariance - updated_error_state.covariance = current_error_state.covariance - np.dot( - K_k, np.dot(P_innovation, K_k.T) - ) - - return updated_error_state - - def imu_update_states( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - imu_data: np.ndarray, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Updates the state vector with the IMU data - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - imu_data (np.ndarray): The IMU data - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - - """ - # Update the nominal state - current_state_nom = self.nominal_state_update(current_state_nom, imu_data) - - # Generate the sigma points - sigma_points, _ = self.generate_sigma_points( - current_state_error, self.Q_process_noise - ) - - # Update the error state - current_state_error = self.unscented_transform( - sigma_points, current_state_nom, imu_data - ) - - return current_state_nom, current_state_error - - def dvl_update_states( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - dvl_data: np.ndarray, - imu_data: np.ndarray, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Update the error state given the DVL data - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - dvl_data (np.ndarray): The DVL data to update the state with - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - """ - # Generate the sigma points - sigma_points, weight = self.generate_sigma_points( - current_state_error, self.Q_process_noise - ) - - # Update the error state - current_state_error = self.unscented_transform( - sigma_points, current_state_nom, imu_data - ) - - # Update the error state - current_state_error = self.measurement_update( - sigma_points, current_state_nom, current_state_error, dvl_data, weight - ) - - return current_state_nom, current_state_error - - def inject_and_reset( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Injects the error state into the nominal state and resets the error state - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - """ - inj_state = self.injection(current_state_nom, current_state_error) - - G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - skew_symmetric(0.5 * current_state_error.orientation) - - current_state_error.covariance = np.dot( - np.dot(G, current_state_error.covariance), G.T - ) - current_state_error.covariance += np.eye(15) - - current_state_error.fill_states(np.zeros(15)) - - return inj_state, current_state_error diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py deleted file mode 100644 index f8ede884d..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ /dev/null @@ -1,292 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from sp_ukf_python_utils import ( - euler_rotation_quaternion, - quaternion_error, - quaternion_super_product, - ssa, - quat_norm, -) - - -@dataclass -class StateVector_quaternion: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(4) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - - def as_vector(self) -> np.ndarray: - """Calculates the state vector. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - if len(state) == 15: - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:10] - self.acceleration_bias = state[10:13] - self.gyro_bias = state[13:] - else: - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = euler_rotation_quaternion(state[6:9]) - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:] - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. - - Returns: - np.ndarray: The rotation matrix. - """ - 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 euler_forward( - self, current_state: 'StateVector_quaternion', dt: float - ) -> 'StateVector_quaternion': - # Define the new state - new_state = StateVector_quaternion() - - # Define the state derivatives - new_state.position = current_state.position + self.position * dt - new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = quat_norm(current_state.orientation + self.orientation * dt) - new_state.acceleration_bias = ( - current_state.acceleration_bias + self.acceleration_bias * dt - ) - new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt - - # Normalize the orientation quaternion - new_state.orientation /= np.linalg.norm(new_state.orientation) - - return new_state - - def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: - """Subtracts two StateVector_quaternion objects. - - Args: - other (StateVector_quaternion): The other StateVector_quaternion object. - - Returns: - np.ndarray: The difference between the two StateVector_quaternion objects. - """ - position_diff = self.position - other.position - velocity_diff = self.velocity - other.velocity - orientation_diff = quaternion_error(self.orientation, other.orientation) - acceleration_bias_diff = self.acceleration_bias - other.acceleration_bias - gyro_bias_diff = self.gyro_bias - other.gyro_bias - - return np.concatenate( - [ - position_diff, - velocity_diff, - orientation_diff, - acceleration_bias_diff, - gyro_bias_diff, - ] - ) - - def __add__(self, other: 'np.ndarray') -> 'np.ndarray': - """Adds a numpy array to this StateVector_quaternion. - - Args: - other (np.ndarray): The numpy array to add. - - Returns: - np.ndarray: The result of the addition. - """ - # Construct the quaternion from the array - add_to_position = other[:3] - add_to_orientation = euler_rotation_quaternion(other[6:10]) - - new_position = self.position + add_to_position - new_velcoity = self.velocity + other[3:6] - new_orientation = quaternion_super_product(self.orientation, add_to_orientation) - new_acceleration_bias = self.acceleration_bias + other[10:13] - new_gyro_bias = self.gyro_bias + other[13:] - - return np.concatenate( - [ - new_position, - new_velcoity, - new_orientation, - new_acceleration_bias, - new_gyro_bias, - ] - ) - - -@dataclass -class StateVector_euler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((15, 15)) - ) # Covariance matrix - - def as_vector(self) -> np.ndarray: - """Calculates the state estimate vector. - - Returns: - np.ndarray: The state estimate vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - - def copy_state(self, wanted_state: 'StateVector_euler') -> None: - """Copies the state from a StateVector object into the current StateVector object. - - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - def __add__(self, other: 'np.ndarray') -> 'np.ndarray': - """Adds a numpy array to this StateVector_quaternion. - - Args: - other (np.ndarray): The numpy array to add. - - Returns: - np.ndarray: The result of the addition. - """ - new_position = self.position + other[:3] - new_velcoity = self.velocity + other[3:6] - new_orientation = self.orientation + other[6:9] - new_acceleration_bias = self.acceleration_bias + other[9:12] - new_gyro_bias = self.gyro_bias + other[12:] - - return np.concatenate( - [ - new_position, - new_velcoity, - new_orientation, - new_acceleration_bias, - new_gyro_bias, - ] - ) - - def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': - """Subtracts two StateVector_euler objects. - - Args: - other (StateVector_euler): The other StateVector_euler object. - - Returns: - StateVector_euler: The difference between the two StateVector_euler objects. - """ - position_diff = self.position - other_state[:3] - velocity_diff = self.velocity - other_state[3:6] - orientation_diff = ssa(self.orientation - other_state[6:9]) - acceleration_bias_diff = self.acceleration_bias - other_state[9:12] - gyro_bias_diff = self.gyro_bias - other_state[12:] - - return np.concatenate( - [ - position_diff, - velocity_diff, - orientation_diff, - acceleration_bias_diff, - gyro_bias_diff, - ] - ) - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) # Measurement vector - measurement_matrix: np.ndarray = field( - default_factory=lambda: np.zeros((6, 15)) - ) # Measurement matrix - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py deleted file mode 100644 index 103528ef2..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from nav_msgs.msg import Odometry -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu, -import numpy as np -from geometry_msgs.msg import TwistWithCovarianceStamped - -# NEED TO CHANGE THIS TO THE CORRECT PATH -from eskf_python.eskf_python_filter import ( - ErrorStateKalmanFilter, - MeasurementModel, - StateVector_euler, - StateVector_quaternion, -) - -qos_profile = QoSProfile( - depth=1, - history=qos_profile_sensor_data.history, - reliability=qos_profile_sensor_data.reliability, -) - - -class ESKalmanFilterNode(Node): - def __init__(self): - super().__init__("sp_ukf_python_node") - - # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - self.imu_subscriber_ = self.create_subscription( - Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile - ) - - self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile - ) - - # This publisher will publish the estimtaed state of the vehicle - self.state_publisher_ = self.create_publisher( - Odometry, '/orca/odom', qos_profile=qos_profile - ) - - self.eskf_modual = ErrorStateKalmanFilter() - self.current_state_nom = StateVector_quaternion() - self.current_state_error = StateVector_euler() - self.measurement_pred = MeasurementModel() - self.odom_msg = Odometry() - - self.get_logger().info("Unscented Kalman Filter started") - - def imu_callback(self, msg: Imu): - - # Get the IMU data - - imu_acceleartion = msg.linear_acceleration - imu_angular_velocity = msg.angular_velocity - - # Combine the IMU data - imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) - - # Update the filter with the IMU data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.imu_update_states( - self.current_state_nom, self.current_state_error, imu_data - ) - ) - - # Inserting the nominal state into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x - self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y - self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z - - # Publish - self.state_publisher_.publish(self.odom_msg) - - - - def filter_callback(self, msg: TwistWithCovarianceStamped): - """Callback function for the filter measurement update, - this will be called when the filter needs to be updated with the DVL data. - """ - self.get_logger().info("Filter callback, got DVL data") - - # Get the DVL data (linear velocity) - dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) - - # Update the filter with the DVL data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.dvl_update_states( - self.current_state_nom, self.current_state_error, dvl_data - ) - ) - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.injection_and_reset( - self.current_state_nom, self.current_state_error - ) - ) - - # Inserting data into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - - # Publishing the data - self.state_publisher_.publish(self.odom_msg) - - - -def main(args=None): - rclpy.init(args=args) - node = ESKalmanFilterNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py deleted file mode 100644 index 56285f031..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py +++ /dev/null @@ -1,116 +0,0 @@ -import numpy as np - -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 = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new /= np.linalg.norm(q_new) - - 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 euler_rotation_quaternion(self, euler_angles: np.ndarray) -> np.ndarray: - """ - Converts An vector assumed to be rotation vector to quaternion - - Args: - euler_angles (np.ndarray): Rotation vector - - Returns: - np.ndarray: Quaternion representation of the rotation vector - """ - - angle = np.linalg.norm(euler_angles) - - if angle == 0: - axis = np.array([0, 0, 0]) - else: - axis = euler_angles / angle - - quaternion = np.zeros(4) - quaternion[0] = np.cos(angle / 2) - quaternion[1:] = np.sin(angle / 2) * axis - - return quaternion - -def quaternion_rotation_euler(self, quaternion: np.ndarray) -> np.ndarray: - """ - Converts a quaternion to an euler rotation vector - Used to generate the covarince matrix - - Args: - quaternion (np.ndarray): The quaternion to convert - - Returns: - np.ndarray: The euler rotation vector - """ - nu, eta_x, eta_y, eta_z = quaternion - - phi = np.arctan2(2 * (nu * eta_x + eta_y * eta_z), 1 - 2 * (eta_x ** 2 + eta_y ** 2)) - theta = -np.arcsin(2 * (eta_z * eta_x - nu * eta_y)) - psi = np.arctan2(2 * (nu * eta_z + eta_x * eta_y), 1 - 2 * (eta_y ** 2 + eta_z ** 2)) - - return np.array([phi, theta, psi]) - -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 ssa(angle: np.ndarray) -> np.ndarray: - """ - smallest signed angle between two angles - """ - ssa_vector = np.zeros(len(angle)) - - for i in range(len(angle)): - ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi - - return ssa_vector - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """ - Function that normalizes a quaternion - """ - - quat = quat / np.linalg.norm(quat) - - return quat diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py deleted file mode 100644 index 1d8c723b1..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ /dev/null @@ -1,313 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np - -# (Assuming the following have been imported from your modules) -from sp_ukf_python_class import StateVector_euler, StateVector_quaternion - -from sp_ukf_python import ErrorStateUnscentedKalmanFilter - - -def quat_to_yaw(q: np.ndarray) -> float: - """Convert a quaternion (assumed [w, x, y, z]) into a yaw angle. - In NED, yaw is typically around the z-down axis. - """ - return 2 * np.arctan2(q[3], q[0]) - - -def run_ESUKF_simulation(): - # ------------------------------------------------------------------------- - # Simulation parameters - # ------------------------------------------------------------------------- - dt = 0.01 # time step [s] - T = 60.0 # total simulation time [s] - num_steps = int(T / dt) - - # In an NED frame, gravity is +9.81 in the z (down) direction. - g_val = 9.81 - - # ------------------------------------------------------------------------- - # Define noise and covariance matrices - # ------------------------------------------------------------------------- - Q = np.diag( - [ - 0.06, - 0.06, - 0.06, # position error - 0.04, - 0.04, - 0.04, # velocity error - 0.003, - 0.003, - 0.003, # orientation error - 0.02, - 0.02, - 0.02, # accelerometer bias error - 0.02, - 0.02, - 0.02, # gyro bias error - ] - ) - - R_meas = np.diag([0.52, 0.52, 0.52]) # Increased DVL measurement noise - - # Bias dynamics tuning remains the same here: - P_ab = 0.002 - P_wb = 0.002 - lever_arm = np.array([0.0, 0.0, 0.0]) # Sensor at the vehicle CG - - # Create the Error-State UKF instance (NED convention) - esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) - - # ------------------------------------------------------------------------- - # Initialize the true state in NED - # ------------------------------------------------------------------------- - # We treat x as North, y as East, z as Down. - # We'll do a circular path in the horizontal plane (z=0). - true_state = StateVector_quaternion() - true_state.position = np.array([20.0, 0.0, 0.0]) # [N, E, D]=[20, 0, 0] - true_state.velocity = np.array([0.0, 1.0, 0.0]) # 1 m/s in the East direction - true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation - true_state.acceleration_bias = np.zeros(3) - true_state.gyro_bias = np.zeros(3) - - # ------------------------------------------------------------------------- - # Initialize the estimated state - # ------------------------------------------------------------------------- - est_state_nom = StateVector_quaternion() - est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) - est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) - est_state_nom.orientation = true_state.orientation.copy() - est_state_nom.acceleration_bias = np.zeros(3) - est_state_nom.gyro_bias = np.zeros(3) - - # Initialize error state (Euler) with some covariance - est_state_error = StateVector_euler() - est_state_error.fill_states(np.zeros(15)) - est_state_error.covariance = 0.5 * np.eye(15) - - # ------------------------------------------------------------------------- - # Prepare histories for plotting - # ------------------------------------------------------------------------- - time_hist = [] - true_pos_hist = [] - est_pos_hist = [] - true_vel_hist = [] - est_vel_hist = [] - true_yaw_hist = [] - est_yaw_hist = [] - - # ------------------------------------------------------------------------- - # Define the "circular" trajectory in the horizontal plane (z=0) - # in NED: x=North, y=East, z=Down - # We'll revolve in the XY-plane, at D=0, with radius=20 m, angular speed=0.05 rad/s - # ------------------------------------------------------------------------- - R_circle = 20.0 - omega = 0.05 - - # ------------------------------------------------------------------------- - # Main simulation loop - # ------------------------------------------------------------------------- - t = 0.0 - for step in range(num_steps): - # --- True State Generation (NED) --- - # Position: circle in x-y plane at z=0 - pos_true = np.array( - [ - R_circle * np.cos(omega * t), # N - R_circle * np.sin(omega * t), # E - 0.0, # D - ] - ) - # Velocity: derivative of pos - vel_true = np.array( - [ - -R_circle * omega * np.sin(omega * t), # d/dt of cos => -sin - R_circle * omega * np.cos(omega * t), # d/dt of sin => cos - 0.0, - ] - ) - # Acceleration: second derivative - acc_true = np.array( - [ - -R_circle * omega**2 * np.cos(omega * t), - -R_circle * omega**2 * np.sin(omega * t), - 0.0, - ] - ) - - # Update the "true" state in NED - true_state.position = pos_true - true_state.velocity = vel_true - - # Compute full quaternion from Euler angles (roll, pitch, yaw) - roll_true = 0.0 - pitch_true = 0.0 - yaw_true = np.arctan2(vel_true[1], vel_true[0]) - cy = np.cos(yaw_true * 0.5) - sy = np.sin(yaw_true * 0.5) - cp = np.cos(pitch_true * 0.5) - sp = np.sin(pitch_true * 0.5) - cr = np.cos(roll_true * 0.5) - sr = np.sin(roll_true * 0.5) - true_state.orientation = np.array( - [ - cr * cp * cy + sr * sp * sy, # w - sr * cp * cy - cr * sp * sy, # x - cr * sp * cy + sr * cp * sy, # y - cr * cp * sy - sr * sp * cy, # z - ] - ) - - # --- Simulated IMU Measurements (NED) --- - # Gravity is +9.81 in the down (z) direction in NED - R_true = true_state.R_q() # rotation from body to inertial - # The "specific force" in body frame is (acc_inertial - gravity_inertial) rotated to body - imu_acc_ideal = R_true.T @ ( - acc_true - np.array([0.0, 0.0, g_val]) - ) + np.random.normal(0.01, 0.01, 3) # [rad/s] - - # Add small noise - imu_acc_noise = np.random.normal(0.0, 0.05, 3) # [m/s^2] - imu_acc_meas = imu_acc_ideal + imu_acc_noise - - # Gyro: angular velocity about body axes. Yaw rate is ~omega for a flat circle - imu_gyro_ideal = np.array([0.0, 0.0, omega]) + np.random.normal( - 0.01, 0.01, 3 - ) # [rad/s] - imu_gyro_noise = np.random.normal(0.0, 0.05, 3) # [rad/s] - imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise - - # Combine - imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) - - # --- Simulated DVL Measurement --- - # Velocity in inertial frame (NED) with zero noise for this test - dvl_noise = np.random.normal(0.0, 0.05, 3) - dvl_meas = vel_true + dvl_noise - - # --------------------------------------------------------------------- - # Filter Updates - # --------------------------------------------------------------------- - # 1. IMU update (prediction) - est_state_nom, est_state_error = esukf.imu_update_states( - est_state_nom, est_state_error, imu_meas - ) - # 2. DVL update (measurement) - est_state_nom, est_state_error = esukf.dvl_update_states( - est_state_nom, est_state_error, dvl_meas, imu_meas - ) - # 3. Inject error state - est_state_nom, est_state_error = esukf.inject_and_reset( - est_state_nom, est_state_error - ) - - # --- Store Histories --- - time_hist.append(t) - true_pos_hist.append(pos_true) - est_pos_hist.append(est_state_nom.position.copy()) - true_vel_hist.append(vel_true) - est_vel_hist.append(est_state_nom.velocity.copy()) - true_yaw_hist.append(yaw_true) - est_yaw_hist.append(quat_to_yaw(est_state_nom.orientation)) - - t += dt - - # ------------------------------------------------------------------------- - # Convert histories to arrays - # ------------------------------------------------------------------------- - true_pos_hist = np.array(true_pos_hist) - est_pos_hist = np.array(est_pos_hist) - true_vel_hist = np.array(true_vel_hist) - est_vel_hist = np.array(est_vel_hist) - true_yaw_hist = np.array(true_yaw_hist) - est_yaw_hist = np.array(est_yaw_hist) - time_hist = np.array(time_hist) - - # ------------------------------------------------------------------------- - # Plotting - # ------------------------------------------------------------------------- - # Positions - plt.figure(figsize=(10, 8)) - plt.subplot(3, 1, 1) - plt.plot(time_hist, true_pos_hist[:, 0], label='True N') - plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated N') - plt.ylabel('N (m)') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time_hist, true_pos_hist[:, 1], label='True E') - plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated E') - plt.ylabel('E (m)') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time_hist, true_pos_hist[:, 2], label='True D') - plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated D') - plt.xlabel('Time (s)') - plt.ylabel('D (m)') - plt.legend() - plt.tight_layout() - plt.show() - - # Velocities - plt.figure(figsize=(10, 8)) - plt.subplot(3, 1, 1) - plt.plot(time_hist, true_vel_hist[:, 0], label='True Vn') - plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vn') - plt.ylabel('Vn (m/s)') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time_hist, true_vel_hist[:, 1], label='True Ve') - plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Ve') - plt.ylabel('Ve (m/s)') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time_hist, true_vel_hist[:, 2], label='True Vd') - plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vd') - plt.xlabel('Time (s)') - plt.ylabel('Vd (m/s)') - plt.legend() - plt.tight_layout() - plt.show() - - # Heading (Yaw) - plt.figure(figsize=(10, 4)) - plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') - plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') - plt.xlabel('Time (s)') - plt.ylabel('Yaw (deg)') - plt.legend() - plt.title('Heading Comparison (NED)') - plt.tight_layout() - plt.show() - - # 3D Trajectory - fig = plt.figure(figsize=(8, 6)) - ax = fig.add_subplot(111, projection='3d') - ax.plot( - true_pos_hist[:, 0], - true_pos_hist[:, 1], - true_pos_hist[:, 2], - label='True Trajectory', - linewidth=2, - ) - ax.plot( - est_pos_hist[:, 0], - est_pos_hist[:, 1], - est_pos_hist[:, 2], - '--', - label='Estimated Trajectory', - linewidth=2, - ) - ax.set_xlabel('North (m)') - ax.set_ylabel('East (m)') - ax.set_zlabel('Down (m)') - ax.legend() - plt.title('3D Trajectory (NED Frame)') - plt.show() - - -if __name__ == '__main__': - run_ESUKF_simulation() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index c588d579e..dd52c6589 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -14,6 +14,31 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.sigma_points_list = None self.y_i = None self.weight = None + # self.T = self.generate_T_matrix(len(P_0)) + + def generate_T_matrix(n): + """ + Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + + Parameters: + n (int): The state dimension. + + Returns: + T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + """ + T = np.zeros((n, 2 * n)) + + for i in range(1, 2 * n + 1): # indexing matches equation (1, ..., 2n) + for j in range(1, (n // 2) + 1): + T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) + T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) + + if n % 2 == 1: # if n is odd, add the last term as described in the paper + T[n - 1, i - 1] = (-1) ** i + + T = T / np.sqrt(2) # Normalize matrix for orthonormality (unit scaling) + + return T def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: """ @@ -134,10 +159,10 @@ def add_quaternion_noise(q, noise_std): x0[3] = 1 x0[7:10] = [0.2, 0.2, 0.2] dt = 0.01 - R = (0.1 / dt) * np.eye(3) + R = (0.01) * np.eye(3) - Q = 0.1 * np.eye(12) - P0 = np.eye(12) * 0.1 + Q = 0.00015 * np.eye(12) + P0 = np.eye(12) * 0.0001 model = process_model() model.dt = 0.01 @@ -156,16 +181,35 @@ def add_quaternion_noise(q, noise_std): model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - model_ukf = model + model_ukf = process_model() + model_ukf.dt = 0.01 + model_ukf.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] + ]) + model_ukf.m = 30.0 + model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) # Simulation parameters - simulation_time = 40 # seconds + simulation_time = 20 # seconds num_steps = int(simulation_time / dt) # Initialize a dummy StateQuat. - test_state = StateQuat() - test_state.fill_states(x0) - test_state.covariance = P0 + new_state = StateQuat() + new_state.fill_states(x0) + new_state.covariance = P0 + + test_state_x = StateQuat() + test_state_x.fill_states(x0) + test_state_x.covariance = P0 # Initialize a estimated state estimated_state = StateQuat() @@ -196,18 +240,15 @@ def add_quaternion_noise(q, noise_std): # Initialize the okid params okid_params = np.zeros((num_steps, 21)) - model.state_vector_prev = test_state - model.state_vector = test_state + model.state_vector_prev = new_state + model.state_vector = new_state - model_ukf.state_vector_prev = test_state - model_ukf.state_vector = test_state + model_ukf.state_vector_prev = test_state_x + model_ukf.state_vector = test_state_x # initialize the ukf ukf = UKF(model_ukf, x0, P0, Q, R) - # Test - ukf.unscented_transform(test_state) - elapsed_times = [] u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) @@ -219,22 +260,22 @@ def add_quaternion_noise(q, noise_std): model_ukf.Control_input = u(step * dt) # Perform the unscented transform - model.model_prediction(test_state) + model.model_prediction(new_state) new_state = model.euler_forward() # Adding noise in the state vector - noisy_state.position = new_state.position + np.random.normal(0, 0.1, 3) - noisy_state.orientation = add_quaternion_noise(new_state.orientation, 0.1) - noisy_state.velocity = new_state.velocity + np.random.normal(0, 0.1, 3) - noisy_state.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.1, 3) + estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) start_time = time.time() - estimated_state = ukf.unscented_transform(noisy_state) + estimated_state = ukf.unscented_transform(estimated_state) elapsed_time = time.time() - start_time elapsed_times.append(elapsed_time) - if step % 20 == 0: - measurment_model.measurement = new_state.velocity + np.random.normal(0, 0.2, 3) + if step % 10 == 0: + measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) From 007c2403925f35cf49571cc733f41c06e55f20f4 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 14 Mar 2025 23:17:19 +0100 Subject: [PATCH 07/19] added test script --- .../eskf_python/eskf_python/eskf_test.py | 235 ++++++++++++++++++ 1 file changed, 235 insertions(+) create mode 100644 navigation/eskf_python/eskf_python/eskf_test.py diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py new file mode 100644 index 000000000..4623b7b86 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -0,0 +1,235 @@ + +from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement +import numpy as np +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion +from ukf_okid_class import process_model, quat_to_euler, euler_to_quat +from ukf_okid_class import StateQuat as StateQuatmodel +from scipy.linalg import block_diag +import matplotlib.pyplot as plt +from eskf_python_filter import ESKF + + +def fancy_print_state_quat(state: StateQuat) -> None: + print("Nominal State (Quaternion):") + print(f" Position : {np.array2string(state.position, precision=3, separator=', ')}") + print(f" Velocity : {np.array2string(state.velocity, precision=3, separator=', ')}") + print(f" Orientation (Quat): {np.array2string(state.orientation, precision=3, separator=', ')}") + print(f" Acceleration Bias : {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") + print(f" Gyro Bias : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") + print(f" Gravity : {np.array2string(state.g, precision=3, separator=', ')}\n") + + +def fancy_print_state_euler(state: StateEuler) -> None: + print("Error State (Euler):") + print(f" Position Error : {np.array2string(state.position, precision=3, separator=', ')}") + print(f" Velocity Error : {np.array2string(state.velocity, precision=3, separator=', ')}") + print(f" Orientation Error : {np.array2string(state.orientation, precision=3, separator=', ')}") + print(f" Acceleration Bias Error: {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") + print(f" Gyro Bias Error : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") + print(f" Gravity Error : {np.array2string(state.g, precision=3, separator=', ')}\n") + +def fancy_print_matrix(matrix: np.ndarray) -> None: + print(f"Matrix shape: {matrix.shape}") + print("========== Matrix ==========") + for row in matrix: + print(" ".join(f"{value:8.3f}" for value in row)) + print("======== End Matrix ========") + +if __name__ == "__main__": + + # Simulation parameters + simulation_time = 20.0 # seconds + dt = 0.01 + num_steps = int(simulation_time / dt) + time = np.linspace(0, simulation_time, num_steps) + + # ----------------------- Setup Initial States, Filter & Model ----------------------- + # True initial state + true_state_init = StateQuat() + true_state_init.position = np.array([0.1, 0.0, 0.0]) + true_state_init.velocity = np.array([0.1, 0.0, 0.0]) + P0 = np.diag([ + 1.0, 1.0, 1.0, # Position + 0.2, 0.2, 0.2, # Velocity + 0.01, 0.01, 0.01, # Orientation + 0.00001, 0.00001, 0.00001, # Acceleration bias + 0.00001, 0.00001, 0.00001, # Gyro bias + 0.00001, 0.00001, 0.00001 # Gravity + ]) + + # Estimated initial state (for filter) + est_state_init = StateQuat() + est_state_init.position = np.array([0.1, 0.0, 0.0]) + est_state_init.velocity = np.array([0.1, 0.0, 0.0]) + + # Noise parameters + Q = np.diag([ + (0.02**2) / dt, (0.02**2) / dt, (0.02**2) / dt, # Accelerometer noise + (0.001**2) / dt, (0.001**2) / dt, (0.001**2) / dt, # Gyroscope noise + 0.0001, 0.0001, 0.0001, # Acceleration bias random walk + 0.00001, 0.00001, 0.00001 # Gyro bias random walk + ]) + + Hx = np.zeros((3, 19)) + Hx[0:3, 6:9] = np.eye(3) + + # Create filter object + eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + + imu_data = Measurement() + dvl_data = Measurement() + dvl_data.aiding_covariance = np.eye(3) * 0.2 + + # Setup the process model + model = process_model() + model.dt = dt + 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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) + + # Initialize a dummy state for simulation dynamics. + new_state = StateQuatmodel() + new_state.position = np.array([0.1, 0.0, 0.0]) + new_state.velocity = np.array([0.1, 0.0, 0.0]) + new_state_prev = StateQuatmodel() + new_state_prev.position = np.array([0.1, 0.0, 0.0]) + new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) + + model.state_vector = new_state + model.state_vector_prev = new_state_prev + + # ----------------------- Data Storage Arrays ----------------------- + true_positions = np.zeros((num_steps, 3)) + true_orientations = np.zeros((num_steps, 3)) + true_velocities = np.zeros((num_steps, 3)) + + est_positions = np.zeros((num_steps, 3)) + est_orientations = np.zeros((num_steps, 3)) + est_velocities = np.zeros((num_steps, 3)) + + # We'll record the filter’s covariance diagonal for each state component. + pos_cov = np.zeros((num_steps, 3)) # covariance for position (indices 0:3) + vel_cov = np.zeros((num_steps, 3)) # covariance for velocity (indices 3:6) + ori_cov = np.zeros((num_steps, 3)) # covariance for orientation (indices 6:9) + + prev_velocity = np.zeros(3) + u = lambda t: np.array([ + 0.5 * np.sin(0.1 * t), + 0.5 * np.sin(0.1 * t + 0.3), + 0.5 * np.sin(0.1 * t + 0.6), + 0.05 * np.cos(0.1 * t), + 0.05 * np.cos(0.1 * t + 0.3), + 0.05 * np.cos(0.1 * t + 0.6) + ]) + + # ----------------------- Simulation Loop ----------------------- + for step in range(num_steps): + t = step * dt + + model.Control_input = u(t) + model.model_prediction(new_state) + new_state = model.euler_forward() + + # Simulate IMU measurements (with noise) + imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) + imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) + + eskf.imu_update(imu_data) + + # DVL update every 20 time-steps + if step % 20 == 0: + dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) + eskf.dvl_update(dvl_data) + + # Store True data (from the simulated dynamics) + true_positions[step, :] = np.copy(new_state.position) + true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) + true_velocities[step, :] = np.copy(new_state.velocity) + + # Store estimated state (from the filter) + est_positions[step, :] = np.copy(eskf.nom_state.position) + est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) + est_velocities[step, :] = np.copy(eskf.nom_state.velocity) + + # Record covariance diagonal (assumed ordering: pos (0:3), vel (3:6), orientation (6:9)) + P_diag = np.diag(eskf.error_state.covariance) + pos_cov[step, :] = P_diag[0:3] + vel_cov[step, :] = P_diag[3:6] + ori_cov[step, :] = P_diag[6:9] + + prev_velocity = new_state.velocity + model.state_vector_prev = new_state + + # ----------------------- New Plotting Scheme ----------------------- + # Create 3 separate figures, each corresponding to one degree of freedom: + # For position and velocity: X, Y, Z. + # For orientation: Roll, Pitch, Yaw. + axis_labels_pos = ["X", "Y", "Z"] + axis_labels_vel = ["X", "Y", "Z"] + axis_labels_ori = ["Roll", "Pitch", "Yaw"] + + # Plot Position + fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) + fig_pos.suptitle("True Data vs Filter Estimates for Position") + for i in range(3): + ax_pos = axs_pos[i] + ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') + ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + sigma_pos = np.sqrt(pos_cov[:, i]) + ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, + color=f"C{i}", alpha=0.2) + ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") + ax_pos.set_xlabel("Time [s]") + ax_pos.set_ylabel("Position") + ax_pos.grid(True) + ax_pos.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() + + # Plot Velocity + fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) + fig_vel.suptitle("True Data vs Filter Estimates for Velocity") + for i in range(3): + ax_vel = axs_vel[i] + ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') + ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + sigma_vel = np.sqrt(vel_cov[:, i]) + ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, + color=f"C{i}", alpha=0.2) + ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") + ax_vel.set_xlabel("Time [s]") + ax_vel.set_ylabel("Velocity") + ax_vel.grid(True) + ax_vel.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() + + # Plot Orientation + fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) + fig_ori.suptitle("True Data vs Filter Estimates for Orientation") + for i in range(3): + ax_ori = axs_ori[i] + ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') + ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + sigma_ori = np.sqrt(ori_cov[:, i]) + ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, + color=f"C{i}", alpha=0.2) + ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") + ax_ori.set_xlabel("Time [s]") + ax_ori.set_ylabel("Orientation") + ax_ori.grid(True) + ax_ori.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() From 1fb496b0b80852a750b6ffe69367493184033ab4 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sat, 15 Mar 2025 00:02:45 +0100 Subject: [PATCH 08/19] fixed some errors, code runs from eskf_test now --- .../eskf_python/eskf_python_class.py | 28 -- .../eskf_python/eskf_python_filter.py | 22 +- .../eskf_python/eskf_python_utils.py | 53 ++ .../eskf_python/eskf_python/eskf_test.py | 83 +--- .../eskf_python/eskf_test_utils.py | 184 +++++++ .../eskf_python/eskf_python/ukf_okid_class.py | 464 ------------------ 6 files changed, 272 insertions(+), 562 deletions(-) create mode 100644 navigation/eskf_python/eskf_python/eskf_test_utils.py delete mode 100644 navigation/eskf_python/eskf_python/ukf_okid_class.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index 949ab996d..c2ffc8e02 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,8 +1,5 @@ from dataclasses import dataclass, field -from typing import Tuple, List -from scipy.linalg import expm import numpy as np -from eskf_python_utils import skew_matrix, quaternion_product @dataclass @@ -85,31 +82,6 @@ def R_q(self) -> np.ndarray: return R - # def inject(self, EulerState: 'StateEuler') -> 'StateQuat': - # inj_state = StateQuat() - - # # Injecting the error state - # inj_state.position = self.position + EulerState.position - # inj_state.velocity = self.velocity + EulerState.velocity - # inj_state.orientation = quaternion_product( - # self.orientation, - # 0.5 - # * np.array( - # [ - # 2, - # EulerState.orientation[0], - # EulerState.orientation[1], - # EulerState.orientation[2], - # ] - # ), - # ) - # inj_state.acceleration_bias = self.acceleration_bias + EulerState.acceleration_bias - # inj_state.gyro_bias = self.gyro_bias + EulerState.gyro_bias - - # return inj_state - - - @dataclass class StateEuler: position: np.ndarray = field( diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index a9c772e47..b1efeb0a9 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -4,9 +4,9 @@ import numpy as np from scipy.linalg import expm from eskf_python_class import StateEuler, StateQuat, Measurement -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion -from ukf_okid_class import euler_to_quat +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat from scipy.linalg import block_diag +from scipy.spatial.transform import Rotation as R_scipy class ESKF: def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): @@ -103,8 +103,6 @@ def h(self) -> np.ndarray: """ return self.nom_state.velocity - - def nominal_state_discrete(self, imu_data: Measurement) -> None: """ Calculates the next nominal state using the discrete-time process model defined in: @@ -196,13 +194,18 @@ def measurement_update(self, dvl_measurement:Measurement) -> None: Args: dvl_measurement (np.ndarray): The DVL measurement. """ - + H = self.H() P = self.error_state.covariance - R= dvl_measurement.aiding_covariance - K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) - self.error_state.fill_states(K @ (dvl_measurement.aiding - self.h())) - self.error_state.covariance = (np.eye(18) - K @ H) @ P + R = dvl_measurement.aiding_covariance + + S = H @ P @ H.T + R + K = P @ H.T @ np.linalg.inv(S) + innovation = dvl_measurement.aiding - self.h() + self.error_state.fill_states(K @ innovation) + + I_KH = np.eye(18) - K @ H + self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability def injection(self) -> None: """ @@ -211,7 +214,6 @@ def injection(self) -> None: Chapter 6.2 eq. 282-283 """ - self.nom_state.position = self.nom_state.position + self.error_state.position self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py index 9ea439982..aaef5f8d6 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -12,6 +12,14 @@ def skew_matrix(vector: np.ndarray) -> np.ndarray: ] ) +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + quat = quat / np.linalg.norm(quat) + + return quat + def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: """Calculates the quaternion super product of two quaternions. @@ -37,6 +45,17 @@ def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: 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_product(quat_1, quat_2_inv) + + return error_quat + def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: """Converts an angle-axis representation to a quaternion. @@ -93,3 +112,37 @@ def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: ) return R + +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]) + diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 4623b7b86..bf4cc8f8b 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -1,40 +1,11 @@ from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement +from eskf_python_utils import quat_to_euler +from eskf_test_utils import process_model, StateQuatModel import numpy as np -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion -from ukf_okid_class import process_model, quat_to_euler, euler_to_quat -from ukf_okid_class import StateQuat as StateQuatmodel -from scipy.linalg import block_diag import matplotlib.pyplot as plt from eskf_python_filter import ESKF - -def fancy_print_state_quat(state: StateQuat) -> None: - print("Nominal State (Quaternion):") - print(f" Position : {np.array2string(state.position, precision=3, separator=', ')}") - print(f" Velocity : {np.array2string(state.velocity, precision=3, separator=', ')}") - print(f" Orientation (Quat): {np.array2string(state.orientation, precision=3, separator=', ')}") - print(f" Acceleration Bias : {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") - print(f" Gyro Bias : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") - print(f" Gravity : {np.array2string(state.g, precision=3, separator=', ')}\n") - - -def fancy_print_state_euler(state: StateEuler) -> None: - print("Error State (Euler):") - print(f" Position Error : {np.array2string(state.position, precision=3, separator=', ')}") - print(f" Velocity Error : {np.array2string(state.velocity, precision=3, separator=', ')}") - print(f" Orientation Error : {np.array2string(state.orientation, precision=3, separator=', ')}") - print(f" Acceleration Bias Error: {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") - print(f" Gyro Bias Error : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") - print(f" Gravity Error : {np.array2string(state.g, precision=3, separator=', ')}\n") - -def fancy_print_matrix(matrix: np.ndarray) -> None: - print(f"Matrix shape: {matrix.shape}") - print("========== Matrix ==========") - for row in matrix: - print(" ".join(f"{value:8.3f}" for value in row)) - print("======== End Matrix ========") - if __name__ == "__main__": # Simulation parameters @@ -51,23 +22,18 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: P0 = np.diag([ 1.0, 1.0, 1.0, # Position 0.2, 0.2, 0.2, # Velocity - 0.01, 0.01, 0.01, # Orientation + 0.2, 0.2, 0.2, # Orientation 0.00001, 0.00001, 0.00001, # Acceleration bias 0.00001, 0.00001, 0.00001, # Gyro bias 0.00001, 0.00001, 0.00001 # Gravity ]) - # Estimated initial state (for filter) - est_state_init = StateQuat() - est_state_init.position = np.array([0.1, 0.0, 0.0]) - est_state_init.velocity = np.array([0.1, 0.0, 0.0]) - # Noise parameters Q = np.diag([ - (0.02**2) / dt, (0.02**2) / dt, (0.02**2) / dt, # Accelerometer noise - (0.001**2) / dt, (0.001**2) / dt, (0.001**2) / dt, # Gyroscope noise - 0.0001, 0.0001, 0.0001, # Acceleration bias random walk - 0.00001, 0.00001, 0.00001 # Gyro bias random walk + (0.05**2) / dt, (0.05**2) / dt, (0.05**2) / dt, # Accelerometer noise + (0.004**2) / dt, (0.004**2) / dt, (0.004**2) / dt, # Gyroscope noise + 0.0002, 0.0002, 0.0002, # Acceleration bias random walk + 0.0001, 0.0001, 0.0001 # Gyro bias random walk ]) Hx = np.zeros((3, 19)) @@ -76,11 +42,14 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: # Create filter object eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + # Create measurement objects imu_data = Measurement() dvl_data = Measurement() - dvl_data.aiding_covariance = np.eye(3) * 0.2 - # Setup the process model + # R matrix for DVL aiding + dvl_data.aiding_covariance = np.diag([(0.5)**2, (0.5)**2, (0.5)**2]) + + # Setup the process model for simulation of AUV model = process_model() model.dt = dt model.mass_interia_matrix = np.array([ @@ -97,17 +66,19 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) # Initialize a dummy state for simulation dynamics. - new_state = StateQuatmodel() + # Two where made since there seems to be an issue with declaring two identical objects. + new_state = StateQuatModel() new_state.position = np.array([0.1, 0.0, 0.0]) new_state.velocity = np.array([0.1, 0.0, 0.0]) - new_state_prev = StateQuatmodel() + + new_state_prev = StateQuatModel() new_state_prev.position = np.array([0.1, 0.0, 0.0]) new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) model.state_vector = new_state model.state_vector_prev = new_state_prev - # ----------------------- Data Storage Arrays ----------------------- + # Initialize arrays to store true and estimated states true_positions = np.zeros((num_steps, 3)) true_orientations = np.zeros((num_steps, 3)) true_velocities = np.zeros((num_steps, 3)) @@ -116,10 +87,10 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: est_orientations = np.zeros((num_steps, 3)) est_velocities = np.zeros((num_steps, 3)) - # We'll record the filter’s covariance diagonal for each state component. - pos_cov = np.zeros((num_steps, 3)) # covariance for position (indices 0:3) - vel_cov = np.zeros((num_steps, 3)) # covariance for velocity (indices 3:6) - ori_cov = np.zeros((num_steps, 3)) # covariance for orientation (indices 6:9) + # covariance arrays + pos_cov = np.zeros((num_steps, 3)) + vel_cov = np.zeros((num_steps, 3)) + ori_cov = np.zeros((num_steps, 3)) prev_velocity = np.zeros(3) u = lambda t: np.array([ @@ -131,7 +102,7 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: 0.05 * np.cos(0.1 * t + 0.6) ]) - # ----------------------- Simulation Loop ----------------------- + # Sim for step in range(num_steps): t = step * dt @@ -139,28 +110,23 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: model.model_prediction(new_state) new_state = model.euler_forward() - # Simulate IMU measurements (with noise) imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) eskf.imu_update(imu_data) - # DVL update every 20 time-steps if step % 20 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) eskf.dvl_update(dvl_data) - # Store True data (from the simulated dynamics) true_positions[step, :] = np.copy(new_state.position) true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) true_velocities[step, :] = np.copy(new_state.velocity) - # Store estimated state (from the filter) est_positions[step, :] = np.copy(eskf.nom_state.position) est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) est_velocities[step, :] = np.copy(eskf.nom_state.velocity) - # Record covariance diagonal (assumed ordering: pos (0:3), vel (3:6), orientation (6:9)) P_diag = np.diag(eskf.error_state.covariance) pos_cov[step, :] = P_diag[0:3] vel_cov[step, :] = P_diag[3:6] @@ -169,10 +135,7 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: prev_velocity = new_state.velocity model.state_vector_prev = new_state - # ----------------------- New Plotting Scheme ----------------------- - # Create 3 separate figures, each corresponding to one degree of freedom: - # For position and velocity: X, Y, Z. - # For orientation: Roll, Pitch, Yaw. + # Plotting axis_labels_pos = ["X", "Y", "Z"] axis_labels_vel = ["X", "Y", "Z"] axis_labels_ori = ["Roll", "Pitch", "Yaw"] diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py new file mode 100644 index 000000000..34abe8eda --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_test_utils.py @@ -0,0 +1,184 @@ +import numpy as np +from dataclasses import dataclass, field +from typing import Tuple +from eskf_python_utils import quaternion_product, euler_to_quat, quat_to_euler, quaternion_error, quat_norm, skew_matrix + +# This was the original code from the ukf_okid.py file + +@dataclass +class StateQuatModel: + """ + 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)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + + 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]) + + 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] + + 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_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 subtract(self, other: 'StateQuatModel') -> np.ndarray: + """Subtracts two StateQuatModel 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + return new_array + + def __add__(self, other: 'StateQuatModel') -> 'StateQuatModel': + """Adds two StateQuatModel objects.""" + new_state = StateQuatModel() + new_state.position = self.position + other.position + new_state.orientation = quaternion_product(self.orientation, other.orientation) + new_state.velocity = self.velocity + other.velocity + new_state.angular_velocity = self.angular_velocity + other.angular_velocity + + return new_state + + def __sub__(self, other: 'StateQuatModel') -> 'StateQuatModel': + """Subtracts two StateQuatModel objects.""" + new_state = StateQuatModel() + 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 + + return new_state.as_vector() + + def __rmul__(self, scalar: float) -> 'StateQuatModel': + """Multiplies the StateQuatModel object by a scalar.""" + new_state = StateQuatModel() + 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 + + return new_state + + def insert_weights(self, weights: np.ndarray) -> np.ndarray: + """Inserts the weights into the covariance matrix.""" + new_state = StateQuatModel() + 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] + + return new_state.as_vector() + + def add_without_quaternions(self, other: 'StateQuatModel') -> 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 + + +@dataclass +class process_model: + """ + A class defined for a general process model. + """ + state_vector: StateQuatModel = field(default_factory=StateQuatModel) + state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) + state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) + 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_matrix(ang_vel) + lever_arm_skew = skew_matrix(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_matrix(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: StateQuatModel) -> 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) -> StateQuatModel: + """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 \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/ukf_okid_class.py b/navigation/eskf_python/eskf_python/ukf_okid_class.py deleted file mode 100644 index 8444fd82e..000000000 --- a/navigation/eskf_python/eskf_python/ukf_okid_class.py +++ /dev/null @@ -1,464 +0,0 @@ -from dataclasses import dataclass, field -import numpy as np - - -from dataclasses import dataclass, field -import numpy as np - -@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)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) - - 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]) - - 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] - - 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] - - def subtract(self, other: 'StateQuat') -> 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) - new_array[6:9] = self.velocity - other.velocity - new_array[9:12] = self.angular_velocity - other.angular_velocity - - 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 - - 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 - - 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 - - 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] - - 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 - -@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.as_vector()) - 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 - -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], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: - """ - Computes the weighted mean of the quaternion orientations from a list of StateQuat objects - using an iterative approach, without requiring the caller to manually extract the quaternion. - - Parameters: - state_list (list[StateQuat]): List of StateQuat objects. - weights (np.ndarray): Weights for each state. - tol (float): Convergence tolerance. - max_iter (int): Maximum number of iterations. - - Returns: - np.ndarray: The averaged quaternion as a 4-element numpy array. - """ - # Internally extract the quaternion from each state - sigma_quats = [state.orientation for state in state_list] - - # Initialize the mean quaternion with the first quaternion - mean_q = sigma_quats[0].copy() - - for _ in range(max_iter): - weighted_error_vectors = [] - for i, q in enumerate(sigma_quats): - # Compute the error quaternion: e = q * inv(mean_q) - # For unit quaternions, the inverse is the conjugate. - 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) - - # Clip to avoid numerical issues - 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: - # Compute the full rotation vector (angle * axis) - error_vec = (angle / np.sin(angle / 2)) * e[1:4] - weighted_error_vectors.append(weights[i] * error_vec) - - error_avg = np.sum(weighted_error_vectors, axis=0) - if np.linalg.norm(error_avg) < tol: - break - - error_norm = np.linalg.norm(error_avg) - delta_q = (np.array([np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm))]) - if error_norm > 0 else 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], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points - """ - n = len(set_points[0].as_vector()) - 1 - mean_value = StateQuat() - - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) - - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - - return mean_value.as_vector() - -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points - """ - n = len(set_points) - mean_value = MeasModel() - - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement - -def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the covariance of a set of points - """ - n = len(set_points[0].as_vector()) - 1 - covariance = np.zeros((n, n)) - mean_quat = StateQuat() - mean_quat.fill_states(mean) - - if weights is None: - for i in range(2 * n + 1): - covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) - - covariance = (1 / (2 * n + 1)) * covariance - - else: - for i in range(2 * n + 1): - covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) - - return covariance - -def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> 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 - - if weights is None: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += np.outer(temp_model.measurement, temp_model.measurement) - - covariance = (1 / (n)) * covariance - - else: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) - - return covariance - -def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: - """ - Calculates the cross covariance between the measurement and state prediction - """ - - n = len(mean_y) - 1 - m = len(mean_z) - cross_covariance = np.zeros((n,m)) - mean_quat = StateQuat() - mean_quat.fill_states(mean_y) - - for i in range(n): - cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) - - cross_covariance = (1 / len(set_y)) * cross_covariance - - return cross_covariance From e110152d818afe8592c7a715b18c92071aff0ca3 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 20 Mar 2025 16:29:42 +0100 Subject: [PATCH 09/19] added changes to ukf --- .../eskf_python/eskf_python_class.py | 23 +- .../eskf_python/eskf_python_filter.py | 28 +- .../eskf_python/eskf_python/eskf_test.py | 185 ++++++---- navigation/ukf_okid/ukf_python/rest.py | 37 ++ navigation/ukf_okid/ukf_python/ukf_okid.py | 324 +----------------- .../ukf_okid/ukf_python/ukf_okid_class.py | 143 ++++---- navigation/ukf_okid/ukf_python/ukf_test.py | 323 +++++++++++++++++ navigation/ukf_okid/ukf_python/ukf_utils.py | 2 +- 8 files changed, 614 insertions(+), 451 deletions(-) create mode 100644 navigation/ukf_okid/ukf_python/rest.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_test.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index c2ffc8e02..0ba96ba1f 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,6 +1,6 @@ from dataclasses import dataclass, field import numpy as np - +from eskf_python_utils import quaternion_error @dataclass class StateQuat: @@ -82,6 +82,27 @@ def R_q(self) -> np.ndarray: return R + def __sub__(self, other: 'StateQuat') -> 'StateQuat': + """Subtracts the values of two state vectors. + + Args: + other (StateQuat): The state vector to subtract. + + Returns: + np.ndarray: The difference between the two state vectors. + """ + result = StateQuat() + result.position = self.position - other.position + result.velocity = self.velocity - other.velocity + result.orientation = quaternion_error(self.orientation, other.orientation) + result.acceleration_bias = self.acceleration_bias - other.acceleration_bias + result.gyro_bias = self.gyro_bias - other.gyro_bias + result.g = self.g - other.g + + return result + + + @dataclass class StateEuler: position: np.ndarray = field( diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index b1efeb0a9..8c4b01963 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -185,7 +185,7 @@ def error_state_prediction(self, imu_data: Measurement) -> None: self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) - def measurement_update(self, dvl_measurement:Measurement) -> None: + def measurement_update(self, dvl_measurement:Measurement) -> float: """ Updates the error state using the DVL measurement. Joan Solà. Quaternion kinematics for the error-state Kalman filter. @@ -202,11 +202,16 @@ def measurement_update(self, dvl_measurement:Measurement) -> None: S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) innovation = dvl_measurement.aiding - self.h() + + NIS_value = self.NIS(S, innovation) + self.error_state.fill_states(K @ innovation) I_KH = np.eye(18) - K @ H self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability + return NIS_value + def injection(self) -> None: """ Injects the error state into the nominal state to produce the estimated state. @@ -241,11 +246,26 @@ def imu_update(self, imu_data: Measurement) -> None: self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) - def dvl_update(self, dvl_measurement: Measurement) -> None: + def dvl_update(self, dvl_measurement: Measurement) -> float: """ Updates the state using the DVL measurement. """ - self.measurement_update(dvl_measurement) + NIS = self.measurement_update(dvl_measurement) self.injection() - self.reset_error_state() \ No newline at end of file + self.reset_error_state() + + return NIS + + # functions for tuning the filter + def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: + """ + Calculates the Normalized Innovation Squared (NIS) value. + """ + return innovation.T @ np.linalg.inv(S) @ innovation + + def NEES(self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat) -> float: + """ + Calculates the Normalized Estimation Error Squared (NEES) value. + """ + return (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) @ (true_state - estimate_state).as_vector() \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index bf4cc8f8b..95f5e996b 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -5,8 +5,9 @@ import numpy as np import matplotlib.pyplot as plt from eskf_python_filter import ESKF +from scipy.stats import chi2 -if __name__ == "__main__": +def simulate_eskf(): # Simulation parameters simulation_time = 20.0 # seconds @@ -20,7 +21,7 @@ true_state_init.position = np.array([0.1, 0.0, 0.0]) true_state_init.velocity = np.array([0.1, 0.0, 0.0]) P0 = np.diag([ - 1.0, 1.0, 1.0, # Position + 0.5, 0.5, 0.5, # Position 0.2, 0.2, 0.2, # Velocity 0.2, 0.2, 0.2, # Orientation 0.00001, 0.00001, 0.00001, # Acceleration bias @@ -30,14 +31,14 @@ # Noise parameters Q = np.diag([ - (0.05**2) / dt, (0.05**2) / dt, (0.05**2) / dt, # Accelerometer noise - (0.004**2) / dt, (0.004**2) / dt, (0.004**2) / dt, # Gyroscope noise - 0.0002, 0.0002, 0.0002, # Acceleration bias random walk - 0.0001, 0.0001, 0.0001 # Gyro bias random walk + (0.034**2) / dt, (0.034**2) / dt, (0.034**2) / dt, # Accelerometer noise + (0.002**2) / dt, (0.002**2) / dt, (0.002**2) / dt, # Gyroscope noise + 0.00001, 0.00001, 0.00001, # Acceleration bias random walk + 0.00001, 0.00001, 0.00001 # Gyro bias random walk ]) Hx = np.zeros((3, 19)) - Hx[0:3, 6:9] = np.eye(3) + Hx[0:3, 3:6] = np.eye(3) # Create filter object eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) @@ -47,7 +48,7 @@ dvl_data = Measurement() # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag([(0.5)**2, (0.5)**2, (0.5)**2]) + dvl_data.aiding_covariance = np.diag([(0.01)**2, (0.01)**2, (0.01)**2]) # Setup the process model for simulation of AUV model = process_model() @@ -102,6 +103,9 @@ 0.05 * np.cos(0.1 * t + 0.6) ]) + NIS_list = [] + NIS_value = 0.0 + # Sim for step in range(num_steps): t = step * dt @@ -117,7 +121,8 @@ if step % 20 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) - eskf.dvl_update(dvl_data) + NIS_value = eskf.dvl_update(dvl_data) + NIS_list.append(NIS_value) true_positions[step, :] = np.copy(new_state.position) true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) @@ -135,64 +140,104 @@ prev_velocity = new_state.velocity model.state_vector_prev = new_state - # Plotting - axis_labels_pos = ["X", "Y", "Z"] - axis_labels_vel = ["X", "Y", "Z"] - axis_labels_ori = ["Roll", "Pitch", "Yaw"] - - # Plot Position - fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) - fig_pos.suptitle("True Data vs Filter Estimates for Position") - for i in range(3): - ax_pos = axs_pos[i] - ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') - ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') - sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, - color=f"C{i}", alpha=0.2) - ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") - ax_pos.set_xlabel("Time [s]") - ax_pos.set_ylabel("Position") - ax_pos.grid(True) - ax_pos.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() - - # Plot Velocity - fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) - fig_vel.suptitle("True Data vs Filter Estimates for Velocity") - for i in range(3): - ax_vel = axs_vel[i] - ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') - ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') - sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, - color=f"C{i}", alpha=0.2) - ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") - ax_vel.set_xlabel("Time [s]") - ax_vel.set_ylabel("Velocity") - ax_vel.grid(True) - ax_vel.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() - - # Plot Orientation - fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) - fig_ori.suptitle("True Data vs Filter Estimates for Orientation") - for i in range(3): - ax_ori = axs_ori[i] - ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') - ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') - sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, - color=f"C{i}", alpha=0.2) - ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") - ax_ori.set_xlabel("Time [s]") - ax_ori.set_ylabel("Orientation") - ax_ori.grid(True) - ax_ori.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() + return time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, NIS_list + +time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, _ = simulate_eskf() + +# Plotting +axis_labels_pos = ["X", "Y", "Z"] +axis_labels_vel = ["X", "Y", "Z"] +axis_labels_ori = ["Roll", "Pitch", "Yaw"] + +# Plot Position +fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) +fig_pos.suptitle("True Data vs Filter Estimates for Position") +for i in range(3): + ax_pos = axs_pos[i] + ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') + ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + sigma_pos = np.sqrt(pos_cov[:, i]) + ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, + color=f"C{i}", alpha=0.2) + ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") + ax_pos.set_xlabel("Time [s]") + ax_pos.set_ylabel("Position") + ax_pos.grid(True) + ax_pos.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + +# Plot Velocity +fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) +fig_vel.suptitle("True Data vs Filter Estimates for Velocity") +for i in range(3): + ax_vel = axs_vel[i] + ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') + ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + sigma_vel = np.sqrt(vel_cov[:, i]) + ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, + color=f"C{i}", alpha=0.2) + ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") + ax_vel.set_xlabel("Time [s]") + ax_vel.set_ylabel("Velocity") + ax_vel.grid(True) + ax_vel.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + +# Plot Orientation +fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) +fig_ori.suptitle("True Data vs Filter Estimates for Orientation") +for i in range(3): + ax_ori = axs_ori[i] + ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') + ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + sigma_ori = np.sqrt(ori_cov[:, i]) + ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, + color=f"C{i}", alpha=0.2) + ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") + ax_ori.set_xlabel("Time [s]") + ax_ori.set_ylabel("Orientation") + ax_ori.grid(True) + ax_ori.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + + +### _______ NIS AND NEES _______ + +num_simulations = 10 +NIS_runs = [] + +for sim in range(num_simulations): + time, true_positions, true_orientations, true_velocities, \ + est_positions, est_orientations, est_velocities, \ + pos_cov, vel_cov, ori_cov, NIS_list = simulate_eskf() + + NIS_runs.append(np.array(NIS_list)) + +NIS_runs = np.vstack(NIS_runs) +ANIS = np.mean(NIS_runs, axis=0) + +measurement_dimension = 3 + +chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations +chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations + +time_steps = np.arange(len(ANIS)) * 0.01 * 20 + +fig, ax = plt.subplots(figsize=(10, 6)) +ax.plot(time_steps, ANIS, label="ANIS", color="C0") +ax.axhline(chi2_lower, color="C1", linestyle="--", label="95% CI Lower") +ax.axhline(chi2_upper, color="C2", linestyle="--", label="95% CI Upper") +ax.set_title("Average Normalized Innovation Squared (ANIS)") +ax.set_xlabel("Time [s]") +ax.set_ylabel("ANIS") +ax.grid(True) +ax.legend() + +plt.tight_layout() +plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py new file mode 100644 index 000000000..df7392bd4 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -0,0 +1,37 @@ +def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index dd52c6589..1ede3f22b 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -14,9 +14,9 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.sigma_points_list = None self.y_i = None self.weight = None - # self.T = self.generate_T_matrix(len(P_0)) + self.T = self.generate_T_matrix(len(P_0)) - def generate_T_matrix(n): + def generate_T_matrix(n: float) -> np.ndarray: """ Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. @@ -28,42 +28,36 @@ def generate_T_matrix(n): """ T = np.zeros((n, 2 * n)) - for i in range(1, 2 * n + 1): # indexing matches equation (1, ..., 2n) + for i in range(1, 2 * n + 1): for j in range(1, (n // 2) + 1): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) - if n % 2 == 1: # if n is odd, add the last term as described in the paper + if n % 2 == 1: # if n is odd T[n - 1, i - 1] = (-1) ** i - T = T / np.sqrt(2) # Normalize matrix for orthonormality (unit scaling) + T = T / np.sqrt(2) return T - def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: + def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: """ Functions that generate the sigma points for the UKF """ n = len(current_state.covariance) - kappa = 3 - n - S = np.linalg.cholesky(current_state.covariance + self.Q) - S_scaled = np.sqrt(n + kappa) * S - - weighted_points = np.concatenate([S_scaled, -S_scaled], axis=1) + I = np.hstack([np.eye(n), -np.eye(n)]) + my = np.sqrt(n) * I + delta = self.T @ my - self.sigma_points_list = [StateQuat() for _ in range(2 * n + 1)] - W = np.zeros(2 * n + 1) - - self.sigma_points_list [0].fill_states(current_state.as_vector()) - W[0] = kappa / (n + kappa) - for i in range(2 * n): - self.sigma_points_list [i + 1].fill_states(current_state.insert_weights(weighted_points[:, i])) - W[i + 1] = 1 / (2 * (n + kappa)) + S = np.linalg.cholesky(current_state.covariance + self.Q) - self.weight = W + self.sigma_points_list = [StateQuat() for _ in range(2 * n)] + + for state in self.sigma_points_list: + state.fill_states_different_dim(current_state.as_vector(), delta[:, self.sigma_points_list.index - return self.sigma_points_list , self.weight + return self.sigma_points_list def unscented_transform(self, current_state: StateQuat) -> StateQuat: @@ -128,290 +122,4 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd self.process_model.state_vector_prev = posteriori_estimate - return posteriori_estimate - -def add_quaternion_noise(q, noise_std): - - noise = np.random.normal(0, noise_std, 3) - - theta = np.linalg.norm(noise) - - if theta > 0: - - axis = noise / theta - - q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) - - else: - - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) - - q_new = quaternion_super_product(q, q_noise) - - return q_new / np.linalg.norm(q_new) - - -if __name__ == '__main__': - - # Create initial state vector and covariance matrix. - x0 = np.zeros(13) - x0[0:3] = [0.3, 0.3, 0.3] - x0[3] = 1 - x0[7:10] = [0.2, 0.2, 0.2] - dt = 0.01 - R = (0.01) * np.eye(3) - - Q = 0.00015 * np.eye(12) - P0 = np.eye(12) * 0.0001 - - model = process_model() - model.dt = 0.01 - 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] - ]) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - model_ukf = process_model() - model_ukf.dt = 0.01 - model_ukf.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] - ]) - model_ukf.m = 30.0 - model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # Simulation parameters - simulation_time = 20 # seconds - num_steps = int(simulation_time / dt) - - # Initialize a dummy StateQuat. - new_state = StateQuat() - new_state.fill_states(x0) - new_state.covariance = P0 - - test_state_x = StateQuat() - test_state_x.fill_states(x0) - test_state_x.covariance = P0 - - # Initialize a estimated state - estimated_state = StateQuat() - estimated_state.fill_states(x0) - estimated_state.covariance = P0 - - # Initialize a estimated state - noisy_state = StateQuat() - noisy_state.fill_states(x0) - noisy_state.covariance = P0 - - measurment_model = MeasModel() - measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R - - # Initialize arrays to store the results - positions = np.zeros((num_steps, 3)) - orientations = np.zeros((num_steps, 3)) - velocities = np.zeros((num_steps, 3)) - angular_velocities = np.zeros((num_steps, 3)) - - # Initialize arrays to store the estimates - positions_est = np.zeros((num_steps, 3)) - orientations_est = np.zeros((num_steps, 3)) - velocities_est = np.zeros((num_steps, 3)) - angular_velocities_est = np.zeros((num_steps, 3)) - - # Initialize the okid params - okid_params = np.zeros((num_steps, 21)) - - model.state_vector_prev = new_state - model.state_vector = new_state - - model_ukf.state_vector_prev = test_state_x - model_ukf.state_vector = test_state_x - - # initialize the ukf - ukf = UKF(model_ukf, x0, P0, Q, R) - - elapsed_times = [] - - u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) - - # Run the simulation - for step in range(num_steps): - # Insert control input - model.Control_input = u(step * dt) - model_ukf.Control_input = u(step * dt) - - # Perform the unscented transform - model.model_prediction(new_state) - new_state = model.euler_forward() - - # Adding noise in the state vector - estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) - - start_time = time.time() - estimated_state = ukf.unscented_transform(estimated_state) - elapsed_time = time.time() - start_time - elapsed_times.append(elapsed_time) - - if step % 10 == 0: - measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - - - positions[step, :] = new_state.position - orientations[step, :] = quat_to_euler(new_state.orientation) - velocities[step, :] = new_state.velocity - angular_velocities[step, :] = new_state.angular_velocity - - positions_est[step, :] = estimated_state.position - orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - velocities_est[step, :] = estimated_state.velocity - angular_velocities_est[step, :] = estimated_state.angular_velocity - - # Update the state for the next iteration - model.state_vector_prev = new_state - - print('Average elapsed time: ', np.mean(elapsed_times)) - print('Max elapsed time: ', np.max(elapsed_times)) - print('Min elapsed time: ', np.min(elapsed_times)) - print('median elapsed time: ', np.median(elapsed_times)) - # Plot the results - time = np.linspace(0, simulation_time, num_steps) - - # Plot positions - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, positions[:, 0], label='True') - plt.plot(time, positions_est[:, 0], label='Estimated') - plt.title('Position X') - plt.xlabel('Time [s]') - plt.ylabel('Position X [m]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, positions[:, 1], label='True') - plt.plot(time, positions_est[:, 1], label='Estimated') - plt.title('Position Y') - plt.xlabel('Time [s]') - plt.ylabel('Position Y [m]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, positions[:, 2], label='True') - plt.plot(time, positions_est[:, 2], label='Estimated') - plt.title('Position Z') - plt.xlabel('Time [s]') - plt.ylabel('Position Z [m]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot orientations (Euler angles) - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, orientations[:, 0], label='True') - plt.plot(time, orientations_est[:, 0], label='Estimated') - plt.title('Orientation Roll') - plt.xlabel('Time [s]') - plt.ylabel('Roll [rad]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, orientations[:, 1], label='True') - plt.plot(time, orientations_est[:, 1], label='Estimated') - plt.title('Orientation Pitch') - plt.xlabel('Time [s]') - plt.ylabel('Pitch [rad]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, orientations[:, 2], label='True') - plt.plot(time, orientations_est[:, 2], label='Estimated') - plt.title('Orientation Yaw') - plt.xlabel('Time [s]') - plt.ylabel('Yaw [rad]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, velocities[:, 0], label='True') - plt.plot(time, velocities_est[:, 0], label='Estimated') - plt.title('Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Velocity X [m/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, velocities[:, 1], label='True') - plt.plot(time, velocities_est[:, 1], label='Estimated') - plt.title('Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Y [m/s]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, velocities[:, 2], label='True') - plt.plot(time, velocities_est[:, 2], label='Estimated') - plt.title('Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Z [m/s]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot angular velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, angular_velocities[:, 0], label='True') - plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - plt.title('Angular Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity X [rad/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, angular_velocities[:, 1], label='True') - plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - plt.title('Angular Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Y [rad/s]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, angular_velocities[:, 2], label='True') - plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - plt.title('Angular Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Z [rad/s]') - plt.legend() - - plt.tight_layout() - plt.show() \ No newline at end of file + return posteriori_estimate \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 8444fd82e..50f1988b4 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -48,11 +48,11 @@ def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] - def subtract(self, other: 'StateQuat') -> np.ndarray: + 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[3:6] = error_ori new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity @@ -115,7 +115,7 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" H = np.zeros((3, 13)) - H[:, 7:10] = np.eye(3) + H[0:3, 7:10] = np.eye(3) z_i = MeasModel() z_i.measurement = np.dot(H, state.as_vector()) return z_i @@ -309,7 +309,7 @@ def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: return error_quat -def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: +def iterative_quaternion_mean_statequat(state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: """ Computes the weighted mean of the quaternion orientations from a list of StateQuat objects using an iterative approach, without requiring the caller to manually extract the quaternion. @@ -323,142 +323,151 @@ def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np Returns: np.ndarray: The averaged quaternion as a 4-element numpy array. """ - # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] - - # Initialize the mean quaternion with the first quaternion + 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): - # Compute the error quaternion: e = q * inv(mean_q) - # For unit quaternions, the inverse is the conjugate. 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) - # Clip to avoid numerical issues 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: - # Compute the full rotation vector (angle * axis) error_vec = (angle / np.sin(angle / 2)) * e[1:4] - weighted_error_vectors.append(weights[i] * error_vec) + weighted_error_vectors.append(error_vec) - error_avg = np.sum(weighted_error_vectors, axis=0) + 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) - delta_q = (np.array([np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm))]) - if error_norm > 0 else np.array([1.0, 0.0, 0.0, 0.0])) + 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], weights: np.ndarray = None) -> np.ndarray: +def mean_set(set_points: list[StateQuat]) -> np.ndarray: """ - Function that calculates the mean of a set of points + Functio 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[0].as_vector()) - 1 + n = len(set_points) mean_value = StateQuat() - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + for state in set_points: + mean_value.add_without_quaternions(state) - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) + mean_value = (1 / (n)) * mean_value + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points) - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - return mean_value.as_vector() -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: +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() - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) + 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, weights: np.ndarray = None) -> np.ndarray: +def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: """ Function that calculates the covariance of a set of points """ - n = len(set_points[0].as_vector()) - 1 - covariance = np.zeros((n, n)) + n = len(set_points) + covariance = np.zeros(set_points[0].covariance.shape) + mean_quat = StateQuat() - mean_quat.fill_states(mean) + mean_quat.fill_states(mean.as_vector()) - if weights is None: - for i in range(2 * n + 1): - covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + mean_q = mean.orientation - covariance = (1 / (2 * n + 1)) * covariance + 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] - else: - for i in range(2 * n + 1): - covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + 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, weights: np.ndarray = None) -> np.ndarray: +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 - if weights is None: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += np.outer(temp_model.measurement, temp_model.measurement) - - covariance = (1 / (n)) * covariance + for state in set_points: + temp_state = state - mean_meas + covariance += np.outer(temp_state.measurement, temp_state.measurement) - else: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += weights[i] * np.outer(temp_model.measurement, temp_model.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, weights: np.ndarray) -> np.ndarray: +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) - n = len(mean_y) - 1 - m = len(mean_z) - cross_covariance = np.zeros((n,m)) + 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): - cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + 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 / len(set_y)) * cross_covariance + cross_covariance = (1 / n) * cross_covariance return cross_covariance 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..5a7e9eaba --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -0,0 +1,323 @@ +from ukf_okid import UKF +from ukf_okid_class import StateQuat, process_model, MeasModel +import numpy as np +import time +import matplotlib.pyplot as plt +from ukf_utils import print_StateQuat_list, print_StateQuat +from ukf_okid_class import quaternion_super_product, quat_to_euler, mean_set, covariance_set + + + +def add_quaternion_noise(q, noise_std): + + noise = np.random.normal(0, noise_std, 3) + + theta = np.linalg.norm(noise) + + if theta > 0: + + axis = noise / theta + + q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + + else: + + q_noise = np.array([1.0, 0.0, 0.0, 0.0]) + + q_new = quaternion_super_product(q, q_noise) + + return q_new / np.linalg.norm(q_new) + + +if __name__ == '__main__': + + # Define a mean StateQuat + mean_state = StateQuat() + mean_state.position = np.array([1.0, 2.0, 3.0]) + mean_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # Quaternion + mean_state.velocity = np.array([0.5, 0.5, 0.5]) + mean_state.angular_velocity = np.array([0.1, 0.1, 0.1]) + + test_state = StateQuat() + test_state.position = np.array([1.0, 1.0, 1.0]) + test_state.orientation = np.array([0.0, 1.0, 0.0, 0.0]) # Quaternion + test_state.velocity = np.array([0.2, 0.2, 0.2]) + test_state.angular_velocity = np.array([0.2, 0.2, 0.2]) + + # Create a set with only one element + state_set = list() + state_set.append(test_state) + print(len(state_set)) + + # Compute the mean + mean = mean_set(state_set) + + # Compute the covariance + mean_state.covariance = covariance_set(state_set, mean_state) + + # Print the results + print("Mean State:") + print_StateQuat(mean_state) + + # # Create initial state vector and covariance matrix. + # x0 = np.zeros(13) + # x0[0:3] = [0.3, 0.3, 0.3] + # x0[3] = 1 + # x0[7:10] = [0.2, 0.2, 0.2] + # dt = 0.01 + # R = (0.01) * np.eye(3) + + # Q = 0.00015 * np.eye(12) + # P0 = np.eye(12) * 0.0001 + + # model = process_model() + # model.dt = 0.01 + # 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] + # ]) + # model.m = 30.0 + # model.r_b_bg = np.array([0.01, 0.0, 0.02]) + # model.inertia = np.diag([0.68, 3.32, 3.34]) + # model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + # model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + # model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # model_ukf = process_model() + # model_ukf.dt = 0.01 + # model_ukf.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] + # ]) + # model_ukf.m = 30.0 + # model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + # model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + # model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + # model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + # model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # # Simulation parameters + # simulation_time = 20 # seconds + # num_steps = int(simulation_time / dt) + + # # Initialize a dummy StateQuat. + # new_state = StateQuat() + # new_state.fill_states(x0) + # new_state.covariance = P0 + + # test_state_x = StateQuat() + # test_state_x.fill_states(x0) + # test_state_x.covariance = P0 + + # # Initialize a estimated state + # estimated_state = StateQuat() + # estimated_state.fill_states(x0) + # estimated_state.covariance = P0 + + # # Initialize a estimated state + # noisy_state = StateQuat() + # noisy_state.fill_states(x0) + # noisy_state.covariance = P0 + + # measurment_model = MeasModel() + # measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + # measurment_model.covariance = R + + # # Initialize arrays to store the results + # positions = np.zeros((num_steps, 3)) + # orientations = np.zeros((num_steps, 3)) + # velocities = np.zeros((num_steps, 3)) + # angular_velocities = np.zeros((num_steps, 3)) + + # # Initialize arrays to store the estimates + # positions_est = np.zeros((num_steps, 3)) + # orientations_est = np.zeros((num_steps, 3)) + # velocities_est = np.zeros((num_steps, 3)) + # angular_velocities_est = np.zeros((num_steps, 3)) + + # # Initialize the okid params + # okid_params = np.zeros((num_steps, 21)) + + # model.state_vector_prev = new_state + # model.state_vector = new_state + + # model_ukf.state_vector_prev = test_state_x + # model_ukf.state_vector = test_state_x + + # # initialize the ukf + # ukf = UKF(model_ukf, x0, P0, Q, R) + + # elapsed_times = [] + + # u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # # Run the simulation + # for step in range(num_steps): + # # Insert control input + # model.Control_input = u(step * dt) + # model_ukf.Control_input = u(step * dt) + + # # Perform the unscented transform + # model.model_prediction(new_state) + # new_state = model.euler_forward() + + # # Adding noise in the state vector + # estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + # estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + # estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + # estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + + # start_time = time.time() + # estimated_state = ukf.unscented_transform(estimated_state) + # elapsed_time = time.time() - start_time + # elapsed_times.append(elapsed_time) + + # if step % 10 == 0: + # measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) + # meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + # estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + # positions[step, :] = new_state.position + # orientations[step, :] = quat_to_euler(new_state.orientation) + # velocities[step, :] = new_state.velocity + # angular_velocities[step, :] = new_state.angular_velocity + + # positions_est[step, :] = estimated_state.position + # orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + # velocities_est[step, :] = estimated_state.velocity + # angular_velocities_est[step, :] = estimated_state.angular_velocity + + # # Update the state for the next iteration + # model.state_vector_prev = new_state + + # print('Average elapsed time: ', np.mean(elapsed_times)) + # print('Max elapsed time: ', np.max(elapsed_times)) + # print('Min elapsed time: ', np.min(elapsed_times)) + # print('median elapsed time: ', np.median(elapsed_times)) + # # Plot the results + # time = np.linspace(0, simulation_time, num_steps) + + # # Plot positions + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, positions[:, 0], label='True') + # plt.plot(time, positions_est[:, 0], label='Estimated') + # plt.title('Position X') + # plt.xlabel('Time [s]') + # plt.ylabel('Position X [m]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, positions[:, 1], label='True') + # plt.plot(time, positions_est[:, 1], label='Estimated') + # plt.title('Position Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Position Y [m]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, positions[:, 2], label='True') + # plt.plot(time, positions_est[:, 2], label='Estimated') + # plt.title('Position Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Position Z [m]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot orientations (Euler angles) + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, orientations[:, 0], label='True') + # plt.plot(time, orientations_est[:, 0], label='Estimated') + # plt.title('Orientation Roll') + # plt.xlabel('Time [s]') + # plt.ylabel('Roll [rad]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, orientations[:, 1], label='True') + # plt.plot(time, orientations_est[:, 1], label='Estimated') + # plt.title('Orientation Pitch') + # plt.xlabel('Time [s]') + # plt.ylabel('Pitch [rad]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, orientations[:, 2], label='True') + # plt.plot(time, orientations_est[:, 2], label='Estimated') + # plt.title('Orientation Yaw') + # plt.xlabel('Time [s]') + # plt.ylabel('Yaw [rad]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot velocities + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, velocities[:, 0], label='True') + # plt.plot(time, velocities_est[:, 0], label='Estimated') + # plt.title('Velocity X') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity X [m/s]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, velocities[:, 1], label='True') + # plt.plot(time, velocities_est[:, 1], label='Estimated') + # plt.title('Velocity Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity Y [m/s]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, velocities[:, 2], label='True') + # plt.plot(time, velocities_est[:, 2], label='Estimated') + # plt.title('Velocity Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity Z [m/s]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot angular velocities + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, angular_velocities[:, 0], label='True') + # plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + # plt.title('Angular Velocity X') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity X [rad/s]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, angular_velocities[:, 1], label='True') + # plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + # plt.title('Angular Velocity Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity Y [rad/s]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, angular_velocities[:, 2], label='True') + # plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + # plt.title('Angular Velocity Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity Z [rad/s]') + # plt.legend() + + # plt.tight_layout() + # plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index f52f2eb62..ad5871567 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -20,7 +20,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): print(f" Orientation: {state.orientation}") print(f" Velocity: {state.velocity}") print(f" Angular Velocity: {state.angular_velocity}") - print(f" okid_params: {state.okid_params}") + # print(f" okid_params: {state.okid_params}") if print_covariance: print_matrix(state.covariance, "Covariance") From 1e02ea23e98cec18a8f6f81b3e970d1162a21261 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 28 Mar 2025 21:41:33 +0100 Subject: [PATCH 10/19] feat: Added ESKF in cpp using .hpp and .cpp, current implementation uses msg imu/data_raw and /orca/pose as the dvl info --- navigation/eskf/CMakeLists.txt | 53 ++++ navigation/eskf/config/eskf_params.yaml | 5 + navigation/eskf/include/eskf/eskf.hpp | 92 +++++++ navigation/eskf/include/eskf/eskf_ros.hpp | 69 +++++ navigation/eskf/include/eskf/eskf_utils.hpp | 11 + navigation/eskf/include/eskf/typedefs.hpp | 100 +++++++ navigation/eskf/launch/eskf.launch.py | 22 ++ navigation/eskf/package.xml | 22 ++ navigation/eskf/src/eskf.cpp | 240 ++++++++++++++++ navigation/eskf/src/eskf_node.cpp | 9 + navigation/eskf/src/eskf_ros.cpp | 118 ++++++++ navigation/eskf/src/eskf_utils.cpp | 13 + .../eskf_python/eskf_python_filter.py | 193 +++++++------ .../eskf_python/eskf_python/eskf_test.py | 257 +++++++++++++----- navigation/ukf_okid/ukf_python/ukf_okid.py | 22 +- 15 files changed, 1067 insertions(+), 159 deletions(-) create mode 100644 navigation/eskf/CMakeLists.txt create mode 100644 navigation/eskf/config/eskf_params.yaml create mode 100644 navigation/eskf/include/eskf/eskf.hpp create mode 100644 navigation/eskf/include/eskf/eskf_ros.hpp create mode 100644 navigation/eskf/include/eskf/eskf_utils.hpp create mode 100644 navigation/eskf/include/eskf/typedefs.hpp create mode 100644 navigation/eskf/launch/eskf.launch.py create mode 100644 navigation/eskf/package.xml create mode 100644 navigation/eskf/src/eskf.cpp create mode 100644 navigation/eskf/src/eskf_node.cpp create mode 100644 navigation/eskf/src/eskf_ros.cpp create mode 100644 navigation/eskf/src/eskf_utils.cpp diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 000000000..2809f9ed9 --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +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) + +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 +) + +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..f3402f4a9 --- /dev/null +++ b/navigation/eskf/config/eskf_params.yaml @@ -0,0 +1,5 @@ +eskf_node: + ros__parameters: + imu_topic: imu/date_raw + dvl_twist: /orca/twist + odom_topic: odom diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..ee47277ea --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,92 @@ +#ifndef ESKF_HPP +#define ESKF_HPP + +#include +#include +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKF { + public: + ESKF(const eskf_params& params); + + std::pair imu_update( + const state_quat& nom_state, + const state_euler& error_state, + const imu_measurement& imu_meas, + const double dt); + + std::pair dvl_update( + const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas); + + private: + // @brief Predict the nominal state + // @param nom_state: Nominal state + // @param imu_meas: IMU measurement + // @return Predicted nominal state + state_quat nominal_state_discrete(const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt); + + // @brief Predict the error state + // @param error_state: Error state + // @param nom_state: Nominal state + // @param imu_meas: IMU measurement + // @return Predicted error state + state_euler error_state_prediction(const state_euler& error_state, + const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt); + + // @brief Update the error state + // @param error_state: Error state + // @param dvl_meas: DVL measurement + // @return Updated error state + state_euler measurement_update(const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas); + + // @brief Inject the error state into the nominal state and reset the error + // state + // @param nom_state: Nominal state + // @param error_state: Error state + // @return Injected and reset state + std::pair injection_and_reset( + const state_quat& nom_state, + const state_euler& error_state); + + // @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(const state_quat& nom_state); + + // @brief Calculate the measurement matrix jakobian + // @param nom_state: Nominal state + // @return Measurement matrix + Eigen::Matrix3x19d calculate_Hx(const state_quat& nom_state); + + // @brief Calculate the full measurement matrix + // @param nom_state: Nominal state + // @return Measurement matrix + Eigen::Matrix3x18d calculate_H(const state_quat& nom_state); + + // @brief Calculate the measurement + // @param nom_state: Nominal state + // @return Measurement + Eigen::Vector3d calculate_h(const state_quat& nom_state); + + Eigen::Matrix12d Q_; +}; + +#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..88f97459f --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,69 @@ +#ifndef ESKF_ROS_HPP +#define ESKF_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "eskf/eskf.hpp" +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKFNode : public rclcpp::Node { + public: + explicit ESKFNode(); + + private: + // @brief Callback function for the imu topic + // @param msg: Imu message containing the imu data + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + + // @brief Callback function for the dvl topic + // @param msg: TwistWithCovarianceStamped message containing the dvl data + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Publish the odometry message + void publish_odom(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Set the parameters for the eskf + void set_parameters(); + + rclcpp::Subscription::SharedPtr imu_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + std::chrono::milliseconds time_step; + + rclcpp::TimerBase::SharedPtr odom_pub_timer_; + + state_quat nom_state_; + + state_euler error_state_; + + imu_measurement imu_meas_; + + dvl_measurement dvl_meas_; + + eskf_params eskf_params_; + + std::unique_ptr eskf_; + + rclcpp::Time last_imu_time_; + + bool first_imu_msg_received_ = false; +}; + +#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..afd871772 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -0,0 +1,11 @@ +#ifndef ESKF_UTILS_HPP +#define ESKF_UTILS_HPP + +#include "eigen3/Eigen/Dense" +#include "eskf/typedefs.hpp" + +Eigen::Matrix3d skew(const Eigen::Vector3d& v); + +double sq(const double& value); + +#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..c93c92402 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,100 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF_TYPEDEFS_H +#define ESKF_TYPEDEFS_H + +#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 + +struct state_quat { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + + Eigen::Vector19d as_vector() const { + Eigen::Vector19d vec; + vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, + accel_bias; + return vec; + } + + state_quat operator-(const state_quat& other) const { + state_quat diff; + diff.pos = pos - other.pos; + diff.vel = vel - other.vel; + diff.quat = quat * other.quat.inverse(); + diff.gyro_bias = gyro_bias - other.gyro_bias; + diff.accel_bias = accel_bias - other.accel_bias; + return diff; + } + + Eigen::Matrix3d get_R() const { return quat.toRotationMatrix(); } +}; + +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(0, 0, 9.81); + + Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); + + Eigen::Vector18d as_vector() const { + Eigen::Vector18d vec; + vec << pos, vel, euler, gyro_bias, accel_bias, gravity; + return vec; + } + + void set_from_vector(const Eigen::Vector18d& vec) { + pos = vec.block<3, 1>(0, 0); + vel = vec.block<3, 1>(3, 0); + euler = vec.block<3, 1>(6, 0); + gyro_bias = vec.block<3, 1>(9, 0); + accel_bias = vec.block<3, 1>(12, 0); + gravity = vec.block<3, 1>(15, 0); + } +}; + +struct 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..8218bf8d0 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,240 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +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(const state_quat& nom_state) { + Eigen::Matrix4x3d Q_delta_theta = Eigen::Matrix4x3d::Zero(); + double qw = nom_state.quat.w(); + double qx = nom_state.quat.x(); + double qy = nom_state.quat.y(); + double qz = 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(const state_quat& nom_state) { + Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); + + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); + + // normal measurement of the velocity + Hx.block<3, 3>(0, 3) = R_bn; + + Eigen::Vector3d v_n = nom_state.vel; + + Eigen::Matrix dR_dq; + Eigen::Quaterniond q = nom_state.quat.normalized(); + double qw = q.w(); + double qx = q.x(); + double qy = q.y(); + double qz = q.z(); + + dR_dq.col(0) = + 2 * Eigen::Vector3d(qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z(), + -qy * v_n.x() + qx * v_n.y() + qw * v_n.z()); + + dR_dq.col(1) = + 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + + dR_dq.col(2) = + 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + + dR_dq.col(3) = + 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + + Hx.block<3, 4>(0, 6) = dR_dq; + + return Hx; +} + +Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { + 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(nom_state); + X_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); + + Eigen::Matrix3x18d H = calculate_Hx(nom_state) * X_delta; + return H; +} + +Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { + Eigen::Matrix3x1d h; + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); + + h = R_bn * nom_state.vel; + + return h; +} + +state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt) { + Eigen::Vector3d acc = + nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + + nom_state.gravity; + Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; + + state_quat next_nom_state; + next_nom_state.pos = + nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + next_nom_state.vel = nom_state.vel + dt * acc; + next_nom_state.quat = + (nom_state.quat * + Eigen::Quaterniond(0, 0.5 * gyro.x(), 0.5 * gyro.y(), 0.5 * gyro.z())); + next_nom_state.quat.normalize(); + next_nom_state.gyro_bias = nom_state.gyro_bias; + next_nom_state.accel_bias = nom_state.accel_bias; + next_nom_state.gravity = nom_state.gravity; + + return next_nom_state; +} + +state_euler ESKF::error_state_prediction(const state_euler& error_state, + const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = nom_state.get_R(); + Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); + Eigen::Vector3d gyro = imu_meas.gyro - 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(); + + auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); + + state_euler next_error_state; + next_error_state.covariance = + A_d * error_state.covariance * A_d.transpose() + GQG_d; + + return next_error_state; +} + +state_euler ESKF::measurement_update(const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas) { + state_euler new_error_state; + + Eigen::Matrix3x18d H = calculate_H(nom_state); + Eigen::Matrix18d P = 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(nom_state); + new_error_state.set_from_vector(K * innovation); + + Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; + new_error_state.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations + + return new_error_state; +} + +std::pair ESKF::injection_and_reset( + const state_quat& nom_state, + const state_euler& error_state) { + state_quat next_nom_state; + + next_nom_state.pos = nom_state.pos + error_state.pos; + next_nom_state.vel = nom_state.vel + error_state.vel; + next_nom_state.quat = + nom_state.quat * Eigen::Quaterniond(1, 0.5 * error_state.euler.x(), + 0.5 * error_state.euler.y(), + 0.5 * error_state.euler.z()); + next_nom_state.quat.normalize(); + next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; + next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; + next_nom_state.gravity = nom_state.gravity + error_state.gravity; + + state_euler new_error_state; + + Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); + + new_error_state.covariance = G * error_state.covariance * G.transpose(); + new_error_state.pos = Eigen::Vector3d::Zero(); + new_error_state.vel = Eigen::Vector3d::Zero(); + new_error_state.euler = Eigen::Vector3d::Zero(); + new_error_state.gyro_bias = Eigen::Vector3d::Zero(); + new_error_state.accel_bias = Eigen::Vector3d::Zero(); + new_error_state.gravity = Eigen::Vector3d::Zero(); + + return {next_nom_state, new_error_state}; +} + +std::pair ESKF::imu_update( + const state_quat& nom_state, + const state_euler& error_state, + const imu_measurement& imu_meas, + const double dt) { + state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); + state_euler next_error_state = + error_state_prediction(error_state, nom_state, imu_meas, dt); + + return {next_nom_state, next_error_state}; +} + +std::pair ESKF::dvl_update( + const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas) { + state_euler new_error_state = + measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = + injection_and_reset(nom_state, new_error_state); + + return {updated_nom_state, updated_error_state}; +} diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp new file mode 100644 index 000000000..e90cebde3 --- /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); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started 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..a3b9c5cf3 --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,118 @@ +#include "eskf/eskf_ros.hpp" +#include +#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(); +} + +void ESKFNode::set_subscribers_and_publisher() { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + this->declare_parameter("imu_topic", "imu/data_raw"); + 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", "/orca/twist"); + 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", "odom"); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); +} + +void ESKFNode::set_parameters() { + Eigen::Matrix12d Q; + Q.setZero(); + Q.diagonal() << sq(0.0103), sq(0.0118), sq(0.0043), // acceleration noise + sq(0.00193), sq(0.00306), sq(0.00118), // gyroscope noise + sq(0.05), sq(0.05), sq(0.05), // acceleration bias noise + sq(0.03), sq(0.03), sq(0.03); // gyroscope bias noise + + eskf_params_.Q = Q; + + eskf_ = std::make_unique(eskf_params_); + + Eigen::Matrix18d P; + P.setZero(); + P.diagonal() << 0.1, 0.1, 0.1, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.01, 0.01, 0.01, // accel bias + 0.01, 0.01, 0.01, // gyro bias + 0.001, 0.001, 0.001; // gravity + + error_state_.covariance = P; +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + rclcpp::Time current_time = msg->header.stamp; + + if (!first_imu_msg_received_) { + last_imu_time_ = current_time; + first_imu_msg_received_ = true; + return; + } + + double dt = (current_time - last_imu_time_).seconds(); + last_imu_time_ = current_time; + + imu_meas_.accel << msg->linear_acceleration.x, msg->linear_acceleration.y, + msg->linear_acceleration.z; + imu_meas_.gyro << msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z; + + std::tie(nom_state_, error_state_) = + eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = + eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); +} + +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(); // Add timestamp to the message + 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..4d33ec7ce --- /dev/null +++ b/navigation/eskf/src/eskf_utils.cpp @@ -0,0 +1,13 @@ + +#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; +} diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 8c4b01963..245d7ece4 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -2,16 +2,22 @@ from typing import Tuple import numpy as np -from scipy.linalg import expm -from eskf_python_class import StateEuler, StateQuat, Measurement -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat -from scipy.linalg import block_diag -from scipy.spatial.transform import Rotation as R_scipy +from eskf_python_class import Measurement, StateEuler, StateQuat +from eskf_python_utils import ( + R_from_angle_axis, + angle_axis_to_quaternion, + euler_to_quat, + quaternion_product, + skew_matrix, +) +from scipy.linalg import block_diag, expm + class ESKF: - def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): + def __init__( + self, Q: np.ndarray, P0, nom_state: StateQuat, p_accBias, p_gyroBias, dt + ): self.Q = Q - self.Hx = Hx # Jacobian of the measurement model self.dt = dt self.nom_state = nom_state self.error_state = StateEuler() @@ -28,15 +34,20 @@ def Fx(self, imu_data: Measurement) -> np.ndarray: Returns: np.ndarray: The state transition matrix. """ - F_x = np.zeros((18, 18)) I = np.eye(3) F_x[0:3, 0:3] = I F_x[0:3, 3:6] = self.dt * I F_x[3:6, 3:6] = I - F_x[3:6, 6:9] = -self.nom_state.R_q() @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) * self.dt - F_x[6:9, 6:9] = R_from_angle_axis((imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt).T + F_x[3:6, 6:9] = ( + -self.nom_state.R_q() + @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) + * self.dt + ) + F_x[6:9, 6:9] = R_from_angle_axis( + (imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt + ).T F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt F_x[3:6, 15:18] = I * self.dt F_x[6:9, 12:15] = -I * self.dt @@ -45,14 +56,13 @@ def Fx(self, imu_data: Measurement) -> np.ndarray: F_x[15:18, 15:18] = I return F_x - + def Fi(self) -> np.ndarray: """Calculates the input matrix. Returns: np.ndarray: The input matrix. """ - F_i = np.zeros((18, 12)) I = np.eye(3) @@ -62,73 +72,99 @@ def Fi(self) -> np.ndarray: F_i[12:15, 9:12] = I return F_i - + def Q_delta_theta(self) -> np.ndarray: - """ - Calculates the Q_delta_theta matrix. + """Calculates the Q_delta_theta matrix. See Joan Solà. Quaternion kinematics for the error-state Kalman filter. chapter: 6.1.1 eq. 281 """ - qw, qx, qy, qz = self.nom_state.orientation - Q_delta_theta = 0.5 * np.array([ - [-qx, -qy, -qz], - [qw, -qz, qy], - [qz, qw, -qx], - [-qy, qx, qw], - ]) + Q_delta_theta = 0.5 * np.array( + [ + [-qx, -qy, -qz], + [qw, -qz, qy], + [qz, qw, -qx], + [-qy, qx, qw], + ] + ) return Q_delta_theta + def Hx(self) -> np.ndarray: + """Calculates the Jacobian of the measurement model. + + Returns: + np.ndarray: The Jacobian of the measurement model. + """ + Hx = np.zeros((3, 19)) + + q0, q1, q2, q3 = self.nom_state.orientation + e = np.array([q1, q2, q3]) + v = self.nom_state.velocity + + temp_nu = -2 * skew_matrix(e) @ v + temp_eps = 2 * (q0 * np.eye(3) + skew_matrix(e)) @ skew_matrix(v) + + Hx[0:3, 3:6] = self.nom_state.R_q() + Hx[0:3, 6:10] = np.vstack([temp_nu, temp_eps]).T + + Hx = np.zeros((3, 19)) + Hx[0:3, 3:6] = np.eye(3) + + return Hx + def H(self) -> np.ndarray: """Calculates the measurement matrix. Returns: np.ndarray: The measurement matrix. """ - X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - H = self.Hx @ X_deltax + H = self.Hx() @ X_deltax return H - + def h(self) -> np.ndarray: - """ - Calculates the measurement model. + """Calculates the measurement model. Returns: np.ndarray: The measurement model. """ - return self.nom_state.velocity + return self.nom_state.velocity # self.nom_state.R_q() @ self.nom_state.velocity def nominal_state_discrete(self, imu_data: Measurement) -> None: - """ - Calculates the next nominal state using the discrete-time process model defined in: + """Calculates the next nominal state using the discrete-time process model defined in: Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter: 5.4.1 The nominal state kinematics - Args: + Args: imu_data (np.ndarray): The IMU data. """ - # Rectify measurements. acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias R = self.nom_state.R_q() - self.nom_state.position = self.nom_state.position + self.nom_state.velocity * self.dt + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 - self.nom_state.velocity = self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt - self.nom_state.orientation = quaternion_product(self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt)) - self.nom_state.acceleration_bias = np.exp(-self.p_accBias * self.dt) * self.nom_state.acceleration_bias - self.nom_state.gyro_bias = np.exp(-self.p_gyroBias * self.dt) * self.nom_state.gyro_bias + self.nom_state.position = ( + self.nom_state.position + + self.nom_state.velocity * self.dt + + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 + ) + self.nom_state.velocity = ( + self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt + ) + self.nom_state.orientation = quaternion_product( + self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt) + ) + self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.nom_state.gyro_bias = self.nom_state.gyro_bias self.nom_state.g = self.nom_state.g def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: - """ - Calculates the Van Loan discretization of a continuous-time system. + """Calculates the Van Loan discretization of a continuous-time system. Args: A_c (np.ndarray): The A matrix. @@ -137,18 +173,22 @@ def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: Returns: Tuple: The A_d and GQG_d matrices. """ - GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) matrix_exp = ( - np.block([[- A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)]]) + np.block( + [ + [-A_c, GQG_T], + [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)], + ] + ) * self.dt ) van_loan_matrix = expm(matrix_exp) - V1 = van_loan_matrix[A_c.shape[0]:, A_c.shape[0]:] - V2 = van_loan_matrix[:A_c.shape[0], A_c.shape[0]:] + V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] + V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] A_d = V1.T GQG_d = A_d @ V2 @@ -156,7 +196,6 @@ def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: return A_d, GQG_d def error_state_prediction(self, imu_data: Measurement) -> None: - # Rectify measurements. acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias @@ -166,9 +205,9 @@ def error_state_prediction(self, imu_data: Measurement) -> None: A_c = np.zeros((18, 18)) A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = - R @ skew_matrix(acc_rect) - A_c[6:9, 6:9] = - skew_matrix(gyro_rect) - A_c[3:6, 9:12] = - R + A_c[3:6, 6:9] = -R @ skew_matrix(acc_rect) + A_c[6:9, 6:9] = -skew_matrix(gyro_rect) + A_c[3:6, 9:12] = -R A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) A_c[6:9, 12:15] = -np.eye(3) @@ -183,18 +222,16 @@ def error_state_prediction(self, imu_data: Measurement) -> None: A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) + self.error_state.covariance = A_d @ self.error_state.covariance @ A_d.T + GQG_d - def measurement_update(self, dvl_measurement:Measurement) -> float: - """ - Updates the error state using the DVL measurement. + def measurement_update(self, dvl_measurement: Measurement) -> float: + """Updates the error state using the DVL measurement. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter: 6.1 eq. 274-276 Args: dvl_measurement (np.ndarray): The DVL measurement. """ - H = self.H() P = self.error_state.covariance R = dvl_measurement.aiding_covariance @@ -208,49 +245,47 @@ def measurement_update(self, dvl_measurement:Measurement) -> float: self.error_state.fill_states(K @ innovation) I_KH = np.eye(18) - K @ H - self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability - + self.error_state.covariance = ( + I_KH @ P @ I_KH.T + K @ R @ K.T + ) # Joseph form for more stability return NIS_value def injection(self) -> None: - """ - Injects the error state into the nominal state to produce the estimated state. + """Injects the error state into the nominal state to produce the estimated state. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter 6.2 eq. 282-283 - + """ self.nom_state.position = self.nom_state.position + self.error_state.position self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity - self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) - self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.error_state.acceleration_bias + self.nom_state.orientation = quaternion_product( + self.nom_state.orientation, euler_to_quat(self.error_state.orientation) + ) + self.nom_state.acceleration_bias = ( + self.nom_state.acceleration_bias + self.error_state.acceleration_bias + ) self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias self.nom_state.g = self.nom_state.g + self.error_state.g def reset_error_state(self) -> None: - """ - Resets the error state after injection. + """Resets the error state after injection. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter 6.3 eq. 284-286 """ - - G = np.eye(18) # Neglecting the delta_theta as this is most common in practice + G = np.eye(18) # Neglecting the delta_theta as this is most common in practice self.error_state.covariance = G @ self.error_state.covariance @ G.T self.error_state.fill_states(np.zeros(18)) def imu_update(self, imu_data: Measurement) -> None: + """Updates the state using the IMU data. """ - Updates the state using the IMU data. - """ - self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) - + def dvl_update(self, dvl_measurement: Measurement) -> float: + """Updates the state using the DVL measurement. """ - Updates the state using the DVL measurement. - """ - NIS = self.measurement_update(dvl_measurement) self.injection() self.reset_error_state() @@ -259,13 +294,17 @@ def dvl_update(self, dvl_measurement: Measurement) -> float: # functions for tuning the filter def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """ - Calculates the Normalized Innovation Squared (NIS) value. + """Calculates the Normalized Innovation Squared (NIS) value. """ return innovation.T @ np.linalg.inv(S) @ innovation - - def NEES(self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat) -> float: - """ - Calculates the Normalized Estimation Error Squared (NEES) value. + + def NEEDS( + self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat + ) -> float: + """Calculates the Normalized Estimation Error Squared (NEEDS) value. """ - return (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) @ (true_state - estimate_state).as_vector() \ No newline at end of file + return ( + (true_state - estimate_state).as_vector().T + @ np.linalg.inv(P) + @ (true_state - estimate_state).as_vector() + ) diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 95f5e996b..53ad83958 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -1,14 +1,13 @@ - -from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement -from eskf_python_utils import quat_to_euler -from eskf_test_utils import process_model, StateQuatModel -import numpy as np import matplotlib.pyplot as plt +import numpy as np +from eskf_python_class import Measurement, StateQuat from eskf_python_filter import ESKF +from eskf_python_utils import quat_to_euler +from eskf_test_utils import StateQuatModel, process_model from scipy.stats import chi2 -def simulate_eskf(): +def simulate_eskf(): # Simulation parameters simulation_time = 20.0 # seconds dt = 0.01 @@ -20,47 +19,71 @@ def simulate_eskf(): true_state_init = StateQuat() true_state_init.position = np.array([0.1, 0.0, 0.0]) true_state_init.velocity = np.array([0.1, 0.0, 0.0]) - P0 = np.diag([ - 0.5, 0.5, 0.5, # Position - 0.2, 0.2, 0.2, # Velocity - 0.2, 0.2, 0.2, # Orientation - 0.00001, 0.00001, 0.00001, # Acceleration bias - 0.00001, 0.00001, 0.00001, # Gyro bias - 0.00001, 0.00001, 0.00001 # Gravity - ]) - + P0 = np.diag( + [ + 0.3, + 0.3, + 0.3, # Position + 0.2, + 0.2, + 0.2, # Velocity + 0.2, + 0.2, + 0.2, # Orientation + 0.0001, + 0.0001, + 0.0001, # Acceleration bias + 0.00001, + 0.00001, + 0.00001, # Gyro bias + 0.00001, + 0.00001, + 0.00001, # Gravity + ] + ) # Noise parameters - Q = np.diag([ - (0.034**2) / dt, (0.034**2) / dt, (0.034**2) / dt, # Accelerometer noise - (0.002**2) / dt, (0.002**2) / dt, (0.002**2) / dt, # Gyroscope noise - 0.00001, 0.00001, 0.00001, # Acceleration bias random walk - 0.00001, 0.00001, 0.00001 # Gyro bias random walk - ]) - - Hx = np.zeros((3, 19)) - Hx[0:3, 3:6] = np.eye(3) + Q = np.diag( + [ + (0.13**2), + (0.13**2), + (0.13**2), # Adjusted Accelerometer noise + (0.13**2), + (0.13**2), + (0.13**2), # Adjusted Gyroscope noise + 0.0001, + 0.0001, + 0.0001, # Adjusted Acceleration bias random walk + 0.0001, + 0.0001, + 0.0001, # Adjusted Gyro bias random walk + ] + ) # Create filter object - eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + eskf = ESKF(Q, P0, true_state_init, 1e-13, 1e-13, dt) # Create measurement objects imu_data = Measurement() dvl_data = Measurement() # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag([(0.01)**2, (0.01)**2, (0.01)**2]) + dvl_data.aiding_covariance = np.diag( + [(0.01) ** 2, (0.01) ** 2, (0.01) ** 2] + ) # Adjusted DVL aiding covariance # Setup the process model for simulation of AUV model = process_model() model.dt = dt - 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] - ]) + 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], + ] + ) model.m = 30.0 model.r_b_bg = np.array([0.01, 0.0, 0.02]) model.inertia = np.diag([0.68, 3.32, 3.34]) @@ -89,19 +112,21 @@ def simulate_eskf(): est_velocities = np.zeros((num_steps, 3)) # covariance arrays - pos_cov = np.zeros((num_steps, 3)) - vel_cov = np.zeros((num_steps, 3)) - ori_cov = np.zeros((num_steps, 3)) + pos_cov = np.zeros((num_steps, 3)) + vel_cov = np.zeros((num_steps, 3)) + ori_cov = np.zeros((num_steps, 3)) prev_velocity = np.zeros(3) - u = lambda t: np.array([ - 0.5 * np.sin(0.1 * t), - 0.5 * np.sin(0.1 * t + 0.3), - 0.5 * np.sin(0.1 * t + 0.6), - 0.05 * np.cos(0.1 * t), - 0.05 * np.cos(0.1 * t + 0.3), - 0.05 * np.cos(0.1 * t + 0.6) - ]) + u = lambda t: np.array( + [ + 0.5 * np.sin(0.1 * t), + 0.5 * np.sin(0.1 * t + 0.3), + 0.5 * np.sin(0.1 * t + 0.6), + 0.05 * np.cos(0.1 * t), + 0.05 * np.cos(0.1 * t + 0.3), + 0.05 * np.cos(0.1 * t + 0.6), + ] + ) NIS_list = [] NIS_value = 0.0 @@ -114,12 +139,16 @@ def simulate_eskf(): model.model_prediction(new_state) new_state = model.euler_forward() - imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) - imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) + imu_data.acceleration = ( + (new_state.velocity - prev_velocity) / dt + ) + np.random.normal(0, 0.13, 3) + imu_data.angular_velocity = new_state.angular_velocity + np.random.normal( + 0, 0.13, 3 + ) eskf.imu_update(imu_data) - if step % 20 == 0: + if step % 200 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) NIS_value = eskf.dvl_update(dvl_data) NIS_list.append(NIS_value) @@ -140,9 +169,34 @@ def simulate_eskf(): prev_velocity = new_state.velocity model.state_vector_prev = new_state - return time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, NIS_list - -time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, _ = simulate_eskf() + return ( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + NIS_list, + ) + + +( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + _, +) = simulate_eskf() # Plotting axis_labels_pos = ["X", "Y", "Z"] @@ -154,11 +208,28 @@ def simulate_eskf(): fig_pos.suptitle("True Data vs Filter Estimates for Position") for i in range(3): ax_pos = axs_pos[i] - ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') - ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + ax_pos.plot( + time, + true_positions[:, i], + label=f"True Pos {axis_labels_pos[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_pos.plot( + time, + est_positions[:, i], + label=f"Est Pos {axis_labels_pos[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, - color=f"C{i}", alpha=0.2) + ax_pos.fill_between( + time, + est_positions[:, i] - sigma_pos, + est_positions[:, i] + sigma_pos, + color=f"C{i}", + alpha=0.2, + ) ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") ax_pos.set_xlabel("Time [s]") ax_pos.set_ylabel("Position") @@ -173,11 +244,28 @@ def simulate_eskf(): fig_vel.suptitle("True Data vs Filter Estimates for Velocity") for i in range(3): ax_vel = axs_vel[i] - ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') - ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + ax_vel.plot( + time, + true_velocities[:, i], + label=f"True Vel {axis_labels_vel[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_vel.plot( + time, + est_velocities[:, i], + label=f"Est Vel {axis_labels_vel[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, - color=f"C{i}", alpha=0.2) + ax_vel.fill_between( + time, + est_velocities[:, i] - sigma_vel, + est_velocities[:, i] + sigma_vel, + color=f"C{i}", + alpha=0.2, + ) ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") ax_vel.set_xlabel("Time [s]") ax_vel.set_ylabel("Velocity") @@ -192,11 +280,28 @@ def simulate_eskf(): fig_ori.suptitle("True Data vs Filter Estimates for Orientation") for i in range(3): ax_ori = axs_ori[i] - ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') - ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + ax_ori.plot( + time, + true_orientations[:, i], + label=f"True Ori {axis_labels_ori[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_ori.plot( + time, + est_orientations[:, i], + label=f"Est Ori {axis_labels_ori[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, - color=f"C{i}", alpha=0.2) + ax_ori.fill_between( + time, + est_orientations[:, i] - sigma_ori, + est_orientations[:, i] + sigma_ori, + color=f"C{i}", + alpha=0.2, + ) ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") ax_ori.set_xlabel("Time [s]") ax_ori.set_ylabel("Orientation") @@ -207,19 +312,29 @@ def simulate_eskf(): plt.show() -### _______ NIS AND NEES _______ +### _______ NIS AND NEEDS _______ -num_simulations = 10 +num_simulations = 10 NIS_runs = [] for sim in range(num_simulations): - time, true_positions, true_orientations, true_velocities, \ - est_positions, est_orientations, est_velocities, \ - pos_cov, vel_cov, ori_cov, NIS_list = simulate_eskf() + ( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + NIS_list, + ) = simulate_eskf() NIS_runs.append(np.array(NIS_list)) -NIS_runs = np.vstack(NIS_runs) +NIS_runs = np.vstack(NIS_runs) ANIS = np.mean(NIS_runs, axis=0) measurement_dimension = 3 @@ -227,7 +342,7 @@ def simulate_eskf(): chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations -time_steps = np.arange(len(ANIS)) * 0.01 * 20 +time_steps = np.arange(len(ANIS)) * 0.01 * 20 fig, ax = plt.subplots(figsize=(10, 6)) ax.plot(time_steps, ANIS, label="ANIS", color="C0") @@ -240,4 +355,4 @@ def simulate_eskf(): ax.legend() plt.tight_layout() -plt.show() \ No newline at end of file +plt.show() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 1ede3f22b..f7000adb4 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -36,10 +36,10 @@ def generate_T_matrix(n: float) -> np.ndarray: if n % 2 == 1: # if n is odd T[n - 1, i - 1] = (-1) ** i - T = T / np.sqrt(2) + T = T / np.sqrt(2) return T - + def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: """ Functions that generate the sigma points for the UKF @@ -53,12 +53,12 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: S = np.linalg.cholesky(current_state.covariance + self.Q) self.sigma_points_list = [StateQuat() for _ in range(2 * n)] - + for state in self.sigma_points_list: - state.fill_states_different_dim(current_state.as_vector(), delta[:, self.sigma_points_list.index + state.fill_states_different_dim(current_state.as_vector(), return self.sigma_points_list - + def unscented_transform(self, current_state: StateQuat) -> StateQuat: """ @@ -94,18 +94,18 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - z_i[i] = measurement.H(self.sigma_points_list[i]) meas_update = MeasModel() - + meas_update.measurement = mean_measurement(z_i, self.weight) - + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) - + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) - + return meas_update, cross_correlation def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: """ - Calculates the posteriori estimate using measurment and the prior estimate + Calculates the posteriori estimate using measurement and the prior estimate """ nu_k = MeasModel() @@ -122,4 +122,4 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd self.process_model.state_vector_prev = posteriori_estimate - return posteriori_estimate \ No newline at end of file + return posteriori_estimate From 1ea9e56ae4b733f191cf0952eec69f71a649c1ad Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Wed, 2 Apr 2025 17:03:52 +0200 Subject: [PATCH 11/19] fix: Added correction for the IMU measurements --- navigation/eskf/CMakeLists.txt | 10 +- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf.hpp | 2 +- navigation/eskf/include/eskf/eskf_utils.hpp | 4 + navigation/eskf/include/eskf/typedefs.hpp | 48 +- navigation/eskf/src/eskf.cpp | 95 ++- navigation/eskf/src/eskf_ros.cpp | 81 ++- navigation/eskf/src/eskf_utils.cpp | 18 + .../eskf_python/eskf_python_filter.py | 116 ++-- .../eskf_python/eskf_python/eskf_test.py | 3 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 33 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 6 +- navigation/ukf_okid/ukf_python/ukf_test.py | 550 +++++++++--------- 13 files changed, 476 insertions(+), 493 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 2809f9ed9..6c8167609 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(eskf) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -16,6 +16,8 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -38,6 +40,12 @@ ament_target_dependencies(eskf_node Eigen3 tf2 vortex_msgs + spdlog + fmt +) + +target_link_libraries(eskf_node + fmt::fmt ) install(TARGETS diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index f3402f4a9..f89b62f79 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,5 +1,6 @@ eskf_node: ros__parameters: - imu_topic: imu/date_raw + imu_topic: imu/data_raw dvl_twist: /orca/twist odom_topic: odom + diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.000003, 0.000003, 0.000003] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index ee47277ea..b30dc35b0 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -1,7 +1,7 @@ #ifndef ESKF_HPP #define ESKF_HPP -#include +#include #include #include "eskf/typedefs.hpp" #include "typedefs.hpp" diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index afd871772..100f7673d 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -8,4 +8,8 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v); double sq(const double& value); +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 index c93c92402..c0a1e41f9 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,26 +5,26 @@ #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; + 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 struct state_quat { @@ -61,7 +61,7 @@ struct state_euler { Eigen::Vector3d euler = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); @@ -84,6 +84,18 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_uncorrected = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_uncorrected = Eigen::Vector3d::Zero(); + + void correct() { + Eigen::Matrix3d R_nb; + R_nb << 0, 0, -1, + 0, -1, 0, + -1, 0, 0; + + accel = R_nb * accel_uncorrected; + gyro = R_nb * gyro_uncorrected; + } }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 8218bf8d0..55d8516dd 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -6,7 +6,8 @@ #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} +ESKF::ESKF(const eskf_params& params) : + Q_(params.Q) {} std::pair ESKF::van_loan_discretization( const Eigen::Matrix18d& A_c, @@ -42,44 +43,45 @@ Eigen::Matrix4x3d ESKF::calculate_Q_delta(const state_quat& nom_state) { Q_delta_theta *= 0.5; return Q_delta_theta; } - Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); - Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); - - // normal measurement of the velocity - Hx.block<3, 3>(0, 3) = R_bn; - + Eigen::Quaterniond q = nom_state.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + Eigen::Vector3d v_n = nom_state.vel; + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + Eigen::Matrix dR_dq; - Eigen::Quaterniond q = nom_state.quat.normalized(); double qw = q.w(); double qx = q.x(); double qy = q.y(); double qz = q.z(); - dR_dq.col(0) = - 2 * Eigen::Vector3d(qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z(), - -qy * v_n.x() + qx * v_n.y() + qw * v_n.z()); - - dR_dq.col(1) = - 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); - - dR_dq.col(2) = - 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); - - dR_dq.col(3) = - 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + dR_dq.col(0) = 2 * Eigen::Vector3d( + qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), + -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), + qy * v_n.x() - qx * v_n.y() + qw * v_n.z() + ); + + dR_dq.col(1) = 2 * Eigen::Vector3d( + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z() + ); + + dR_dq.col(2) = 2 * Eigen::Vector3d( + -qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z() + ); + + dR_dq.col(3) = 2 * Eigen::Vector3d( + -qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z() + ); Hx.block<3, 4>(0, 6) = dR_dq; @@ -98,8 +100,7 @@ Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { Eigen::Matrix3x1d h; - Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); + Eigen::Matrix3d R_bn = nom_state.quat.normalized().toRotationMatrix().transpose(); h = R_bn * nom_state.vel; @@ -109,18 +110,14 @@ Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = - nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + - nom_state.gravity; + Eigen::Vector3d acc = nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + nom_state.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; state_quat next_nom_state; - next_nom_state.pos = - nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + + next_nom_state.pos = nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; next_nom_state.vel = nom_state.vel + dt * acc; - next_nom_state.quat = - (nom_state.quat * - Eigen::Quaterniond(0, 0.5 * gyro.x(), 0.5 * gyro.y(), 0.5 * gyro.z())); + next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias; @@ -135,7 +132,7 @@ state_euler ESKF::error_state_prediction(const state_euler& error_state, const double dt) { Eigen::Matrix3d R = nom_state.get_R(); Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); - Eigen::Vector3d gyro = imu_meas.gyro - nom_state.gyro_bias; + Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias); Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -191,10 +188,7 @@ std::pair ESKF::injection_and_reset( next_nom_state.pos = nom_state.pos + error_state.pos; next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = - nom_state.quat * Eigen::Quaterniond(1, 0.5 * error_state.euler.x(), - 0.5 * error_state.euler.y(), - 0.5 * error_state.euler.z()); + next_nom_state.quat = nom_state.quat * vector3d_to_quaternion(error_state.euler); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; @@ -205,12 +199,6 @@ std::pair ESKF::injection_and_reset( Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); new_error_state.covariance = G * error_state.covariance * G.transpose(); - new_error_state.pos = Eigen::Vector3d::Zero(); - new_error_state.vel = Eigen::Vector3d::Zero(); - new_error_state.euler = Eigen::Vector3d::Zero(); - new_error_state.gyro_bias = Eigen::Vector3d::Zero(); - new_error_state.accel_bias = Eigen::Vector3d::Zero(); - new_error_state.gravity = Eigen::Vector3d::Zero(); return {next_nom_state, new_error_state}; } @@ -221,8 +209,7 @@ std::pair ESKF::imu_update( const imu_measurement& imu_meas, const double dt) { state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = - error_state_prediction(error_state, nom_state, imu_meas, dt); + state_euler next_error_state = error_state_prediction(error_state, next_nom_state, imu_meas, dt); return {next_nom_state, next_error_state}; } @@ -231,10 +218,8 @@ std::pair ESKF::dvl_update( const state_quat& nom_state, const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = - measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = - injection_and_reset(nom_state, new_error_state); + state_euler new_error_state = measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = injection_and_reset(nom_state, new_error_state); return {updated_nom_state, updated_error_state}; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a3b9c5cf3..524b48c29 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,63 +1,64 @@ #include "eskf/eskf_ros.hpp" -#include -#include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" +#include 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)); + 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); + auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); this->declare_parameter("imu_topic", "imu/data_raw"); 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)); + imu_sub_ = this->create_subscription(imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); this->declare_parameter("dvl_topic", "/orca/twist"); 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)); + dvl_sub_ = this->create_subscription(dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + this->declare_parameter("odom_topic", "odom"); std::string odom_topic = this->get_parameter("odom_topic").as_string(); - odom_pub_ = this->create_publisher( - odom_topic, qos_sensor_data); + odom_pub_ = this->create_publisher(odom_topic, qos_sensor_data); } void ESKFNode::set_parameters() { + + std::vector diag_Q_std; + this->declare_parameter>("diag_Q_std"); // gyroscope bias noise + + diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); + Eigen::Matrix12d Q; Q.setZero(); - Q.diagonal() << sq(0.0103), sq(0.0118), sq(0.0043), // acceleration noise - sq(0.00193), sq(0.00306), sq(0.00118), // gyroscope noise - sq(0.05), sq(0.05), sq(0.05), // acceleration bias noise - sq(0.03), sq(0.03), sq(0.03); // gyroscope bias noise - + spdlog::info("Q diagonal: {}",diag_Q_std[0]); + Q.diagonal() << + sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), // acceleration noise + sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), // gyroscope noise + sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), // acceleration bias noise + sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); // gyroscope bias noise eskf_params_.Q = Q; eskf_ = std::make_unique(eskf_params_); Eigen::Matrix18d P; P.setZero(); - P.diagonal() << 0.1, 0.1, 0.1, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.01, 0.01, 0.01, // accel bias - 0.01, 0.01, 0.01, // gyro bias - 0.001, 0.001, 0.001; // gravity + P.diagonal() << 1.0, 1.0, 1.0, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.001, 0.001, 0.001, // accel bias + 0.001, 0.001, 0.001, // gyro bias + 0.001, 0.001, 0.001; // gravity error_state_.covariance = P; } @@ -71,30 +72,24 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { return; } - double dt = (current_time - last_imu_time_).seconds(); + double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel << msg->linear_acceleration.x, msg->linear_acceleration.y, - msg->linear_acceleration.z; - imu_meas_.gyro << msg->angular_velocity.x, msg->angular_velocity.y, - msg->angular_velocity.z; + imu_meas_.accel_uncorrected << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; + imu_meas_.gyro_uncorrected << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + imu_meas_.correct(); - std::tie(nom_state_, error_state_) = - eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], - msg->twist.covariance[2], msg->twist.covariance[6], - msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], - msg->twist.covariance[14]; - - std::tie(nom_state_, error_state_) = - eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 4d33ec7ce..7a668adfc 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -11,3 +11,21 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { double sq(const double& value) { return value * value; } + +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; + return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + } +} + +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()); + return q; +} \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 245d7ece4..890226639 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -25,54 +25,6 @@ def __init__( self.p_accBias = p_accBias self.p_gyroBias = p_gyroBias - def Fx(self, imu_data: Measurement) -> np.ndarray: - """Calculates the state transition matrix. - - Args: - imu_data (np.ndarray): The IMU data. - - Returns: - np.ndarray: The state transition matrix. - """ - F_x = np.zeros((18, 18)) - I = np.eye(3) - - F_x[0:3, 0:3] = I - F_x[0:3, 3:6] = self.dt * I - F_x[3:6, 3:6] = I - F_x[3:6, 6:9] = ( - -self.nom_state.R_q() - @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) - * self.dt - ) - F_x[6:9, 6:9] = R_from_angle_axis( - (imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt - ).T - F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt - F_x[3:6, 15:18] = I * self.dt - F_x[6:9, 12:15] = -I * self.dt - F_x[9:12, 9:12] = I - F_x[12:15, 12:15] = I - F_x[15:18, 15:18] = I - - return F_x - - def Fi(self) -> np.ndarray: - """Calculates the input matrix. - - Returns: - np.ndarray: The input matrix. - """ - F_i = np.zeros((18, 12)) - I = np.eye(3) - - F_i[3:6, 0:3] = I - F_i[6:9, 3:6] = I - F_i[9:12, 6:9] = I - F_i[12:15, 9:12] = I - - return F_i - def Q_delta_theta(self) -> np.ndarray: """Calculates the Q_delta_theta matrix. See Joan Solà. Quaternion kinematics for the error-state Kalman filter. @@ -92,26 +44,58 @@ def Q_delta_theta(self) -> np.ndarray: return Q_delta_theta def Hx(self) -> np.ndarray: - """Calculates the Jacobian of the measurement model. - - Returns: - np.ndarray: The Jacobian of the measurement model. """ - Hx = np.zeros((3, 19)) - - q0, q1, q2, q3 = self.nom_state.orientation - e = np.array([q1, q2, q3]) - v = self.nom_state.velocity - - temp_nu = -2 * skew_matrix(e) @ v - temp_eps = 2 * (q0 * np.eye(3) + skew_matrix(e)) @ skew_matrix(v) - - Hx[0:3, 3:6] = self.nom_state.R_q() - Hx[0:3, 6:10] = np.vstack([temp_nu, temp_eps]).T + Computes the true-state measurement Jacobian for the measurement + h(x) = R(q) * velocity, + where: + - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, + - velocity is a 3-vector. + + The state is assumed to be ordered as: + [position (3), velocity (3), quaternion (4), ...] (total length 19). + + The Jacobian Hx is a 3x19 matrix with nonzero blocks: + - Columns 3:6 (velocity): R(q) + - Columns 6:10 (quaternion): + """ + q = self.nom_state.orientation # shape (4,) + v = self.nom_state.velocity # shape (3,) + q0, q1, q2, q3 = q + v1, v2, v3 = v + + R = self.nom_state.R_q().transpose() # shape (3, 3) + + dhdq0 = 2 * np.array([ + q0 * v1 - q3 * v2 + q2 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + -q2 * v1 + q1 * v2 + q0 * v3 + ]) + + dhdq1 = 2 * np.array([ + q1 * v1 + q2 * v2 + q3 * v3, + q2 * v1 - q1 * v2 - q0 * v3, + q3 * v1 + q0 * v2 - q1 * v3 + ]) + + dhdq2 = 2 * np.array([ + -q2 * v1 + q1 * v2 + q0 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + -q0 * v1 + q3 * v2 - q2 * v3 + ]) + + dhdq3 = 2 * np.array([ + -q3 * v1 - q0 * v2 + q1 * v3, + q0 * v1 - q3 * v2 + q2 * v3, + q1 * v1 + q2 * v2 + q3 * v3 + ]) + + dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) + Hx = np.zeros((3, 19)) - Hx[0:3, 3:6] = np.eye(3) - + Hx[:, 3:6] = R + Hx[:, 6:10] = dHdq + return Hx def H(self) -> np.ndarray: @@ -132,7 +116,7 @@ def h(self) -> np.ndarray: Returns: np.ndarray: The measurement model. """ - return self.nom_state.velocity # self.nom_state.R_q() @ self.nom_state.velocity + return self.nom_state.R_q() @ self.nom_state.velocity def nominal_state_discrete(self, imu_data: Measurement) -> None: """Calculates the next nominal state using the discrete-time process model defined in: diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 53ad83958..0930d9f6c 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -313,7 +313,7 @@ def simulate_eskf(): ### _______ NIS AND NEEDS _______ - +""" num_simulations = 10 NIS_runs = [] @@ -356,3 +356,4 @@ def simulate_eskf(): plt.tight_layout() plt.show() +""" \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index f7000adb4..16312935a 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -16,7 +16,7 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.weight = None self.T = self.generate_T_matrix(len(P_0)) - def generate_T_matrix(n: float) -> np.ndarray: + def generate_T_matrix(self, n: float) -> np.ndarray: """ Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. @@ -26,10 +26,10 @@ def generate_T_matrix(n: float) -> np.ndarray: Returns: T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ - T = np.zeros((n, 2 * n)) + T = np.zeros((n, n)) - for i in range(1, 2 * n + 1): - for j in range(1, (n // 2) + 1): + for i in range(n): + for j in range(n//2): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) @@ -54,8 +54,9 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: self.sigma_points_list = [StateQuat() for _ in range(2 * n)] - for state in self.sigma_points_list: - state.fill_states_different_dim(current_state.as_vector(), + for index, state in enumerate(self.sigma_points_list): + delta_x = S @ delta[:, index] + state.fill_states_different_dim(current_state.as_vector(), delta_x) return self.sigma_points_list @@ -65,20 +66,20 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: The unscented transform function generates the priori state estimate """ - _ , _ = self.sigma_points(current_state) + _ = self.sigma_points(current_state) n = len(current_state.covariance) - self.y_i = [StateQuat() for _ in range(2 * n + 1)] + self.y_i = [StateQuat() for _ in range(2 * n)] - for i in range(2 * n + 1): + for i in range(2 * n ): self.process_model.model_prediction(self.sigma_points_list[i]) self.y_i[i] = self.process_model.euler_forward() state_estimate = StateQuat() - x = mean_set(self.y_i, self.weight) + x = mean_set(self.y_i) state_estimate.fill_states(x) - state_estimate.covariance = covariance_set(self.y_i, x, self.weight) + state_estimate.covariance = covariance_set(self.y_i, x) return state_estimate def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: @@ -88,18 +89,18 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - """ n = len(current_state.covariance) - z_i = [MeasModel() for _ in range(2 * n + 1)] + z_i = [MeasModel() for _ in range(2 * n)] - for i in range(2 * n + 1): + for i in range(2 * n): z_i[i] = measurement.H(self.sigma_points_list[i]) meas_update = MeasModel() - meas_update.measurement = mean_measurement(z_i, self.weight) + meas_update.measurement = mean_measurement(z_i) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement) return meas_update, cross_correlation diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 50f1988b4..4d3281260 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -395,7 +395,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: return mean_value.measurement -def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: +def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: """ Function that calculates the covariance of a set of points """ @@ -403,9 +403,9 @@ def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: covariance = np.zeros(set_points[0].covariance.shape) mean_quat = StateQuat() - mean_quat.fill_states(mean.as_vector()) + mean_quat.fill_states(mean) - mean_q = mean.orientation + mean_q = mean_quat.orientation for state in set_points: q = state.orientation diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 5a7e9eaba..3c197ed63 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -31,293 +31,267 @@ def add_quaternion_noise(q, noise_std): if __name__ == '__main__': - # Define a mean StateQuat - mean_state = StateQuat() - mean_state.position = np.array([1.0, 2.0, 3.0]) - mean_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # Quaternion - mean_state.velocity = np.array([0.5, 0.5, 0.5]) - mean_state.angular_velocity = np.array([0.1, 0.1, 0.1]) - - test_state = StateQuat() - test_state.position = np.array([1.0, 1.0, 1.0]) - test_state.orientation = np.array([0.0, 1.0, 0.0, 0.0]) # Quaternion - test_state.velocity = np.array([0.2, 0.2, 0.2]) - test_state.angular_velocity = np.array([0.2, 0.2, 0.2]) - - # Create a set with only one element - state_set = list() - state_set.append(test_state) - print(len(state_set)) - - # Compute the mean - mean = mean_set(state_set) - - # Compute the covariance - mean_state.covariance = covariance_set(state_set, mean_state) - - # Print the results - print("Mean State:") - print_StateQuat(mean_state) - - # # Create initial state vector and covariance matrix. - # x0 = np.zeros(13) - # x0[0:3] = [0.3, 0.3, 0.3] - # x0[3] = 1 - # x0[7:10] = [0.2, 0.2, 0.2] - # dt = 0.01 - # R = (0.01) * np.eye(3) + # Create initial state vector and covariance matrix. + x0 = np.zeros(13) + x0[0:3] = [0.3, 0.3, 0.3] + x0[3] = 1 + x0[7:10] = [0.2, 0.2, 0.2] + dt = 0.01 + R = (0.01) * np.eye(3) - # Q = 0.00015 * np.eye(12) - # P0 = np.eye(12) * 0.0001 - - # model = process_model() - # model.dt = 0.01 - # 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] - # ]) - # model.m = 30.0 - # model.r_b_bg = np.array([0.01, 0.0, 0.02]) - # model.inertia = np.diag([0.68, 3.32, 3.34]) - # model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - # model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - # model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # model_ukf = process_model() - # model_ukf.dt = 0.01 - # model_ukf.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] - # ]) - # model_ukf.m = 30.0 - # model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - # model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - # model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - # model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - # model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # # Simulation parameters - # simulation_time = 20 # seconds - # num_steps = int(simulation_time / dt) - - # # Initialize a dummy StateQuat. - # new_state = StateQuat() - # new_state.fill_states(x0) - # new_state.covariance = P0 - - # test_state_x = StateQuat() - # test_state_x.fill_states(x0) - # test_state_x.covariance = P0 - - # # Initialize a estimated state - # estimated_state = StateQuat() - # estimated_state.fill_states(x0) - # estimated_state.covariance = P0 - - # # Initialize a estimated state - # noisy_state = StateQuat() - # noisy_state.fill_states(x0) - # noisy_state.covariance = P0 - - # measurment_model = MeasModel() - # measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - # measurment_model.covariance = R - - # # Initialize arrays to store the results - # positions = np.zeros((num_steps, 3)) - # orientations = np.zeros((num_steps, 3)) - # velocities = np.zeros((num_steps, 3)) - # angular_velocities = np.zeros((num_steps, 3)) - - # # Initialize arrays to store the estimates - # positions_est = np.zeros((num_steps, 3)) - # orientations_est = np.zeros((num_steps, 3)) - # velocities_est = np.zeros((num_steps, 3)) - # angular_velocities_est = np.zeros((num_steps, 3)) - - # # Initialize the okid params - # okid_params = np.zeros((num_steps, 21)) - - # model.state_vector_prev = new_state - # model.state_vector = new_state - - # model_ukf.state_vector_prev = test_state_x - # model_ukf.state_vector = test_state_x - - # # initialize the ukf - # ukf = UKF(model_ukf, x0, P0, Q, R) - - # elapsed_times = [] - - # u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) - - # # Run the simulation - # for step in range(num_steps): - # # Insert control input - # model.Control_input = u(step * dt) - # model_ukf.Control_input = u(step * dt) - - # # Perform the unscented transform - # model.model_prediction(new_state) - # new_state = model.euler_forward() - - # # Adding noise in the state vector - # estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - # estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - # estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - # estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) - - # start_time = time.time() - # estimated_state = ukf.unscented_transform(estimated_state) - # elapsed_time = time.time() - start_time - # elapsed_times.append(elapsed_time) - - # if step % 10 == 0: - # measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - # meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - # estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - - - # positions[step, :] = new_state.position - # orientations[step, :] = quat_to_euler(new_state.orientation) - # velocities[step, :] = new_state.velocity - # angular_velocities[step, :] = new_state.angular_velocity - - # positions_est[step, :] = estimated_state.position - # orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - # velocities_est[step, :] = estimated_state.velocity - # angular_velocities_est[step, :] = estimated_state.angular_velocity + Q = 0.00015 * np.eye(12) + P0 = np.eye(12) * 0.0001 + + model = process_model() + model.dt = 0.01 + 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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + model_ukf = process_model() + model_ukf.dt = 0.01 + model_ukf.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] + ]) + model_ukf.m = 30.0 + model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # Simulation parameters + simulation_time = 5 # seconds + num_steps = int(simulation_time / dt) + + # Initialize a dummy StateQuat. + new_state = StateQuat() + new_state.fill_states(x0) + new_state.covariance = P0 + + test_state_x = StateQuat() + test_state_x.fill_states(x0) + test_state_x.covariance = P0 + + # Initialize a estimated state + estimated_state = StateQuat() + estimated_state.fill_states(x0) + estimated_state.covariance = P0 + + # Initialize a estimated state + noisy_state = StateQuat() + noisy_state.fill_states(x0) + noisy_state.covariance = P0 + + measurment_model = MeasModel() + measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + measurment_model.covariance = R + + # Initialize arrays to store the results + positions = np.zeros((num_steps, 3)) + orientations = np.zeros((num_steps, 3)) + velocities = np.zeros((num_steps, 3)) + angular_velocities = np.zeros((num_steps, 3)) + + # Initialize arrays to store the estimates + positions_est = np.zeros((num_steps, 3)) + orientations_est = np.zeros((num_steps, 3)) + velocities_est = np.zeros((num_steps, 3)) + angular_velocities_est = np.zeros((num_steps, 3)) + + # Initialize the okid params + okid_params = np.zeros((num_steps, 21)) + + model.state_vector_prev = new_state + model.state_vector = new_state + + model_ukf.state_vector_prev = test_state_x + model_ukf.state_vector = test_state_x + + # initialize the ukf + ukf = UKF(model_ukf, x0, P0, Q, R) + + elapsed_times = [] + + u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # Run the simulation + for step in range(num_steps): + # Insert control input + model.Control_input = u(step * dt) + model_ukf.Control_input = u(step * dt) + + # Perform the unscented transform + model.model_prediction(new_state) + new_state = model.euler_forward() + + # Adding noise in the state vector + estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + + start_time = time.time() + estimated_state = ukf.unscented_transform(estimated_state) + print(estimated_state.as_vector()) + break + elapsed_time = time.time() - start_time + elapsed_times.append(elapsed_time) + + if step % 20 == 0: + measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) + meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + positions[step, :] = new_state.position + orientations[step, :] = quat_to_euler(new_state.orientation) + velocities[step, :] = new_state.velocity + angular_velocities[step, :] = new_state.angular_velocity + + positions_est[step, :] = estimated_state.position + orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + velocities_est[step, :] = estimated_state.velocity + angular_velocities_est[step, :] = estimated_state.angular_velocity - # # Update the state for the next iteration - # model.state_vector_prev = new_state - - # print('Average elapsed time: ', np.mean(elapsed_times)) - # print('Max elapsed time: ', np.max(elapsed_times)) - # print('Min elapsed time: ', np.min(elapsed_times)) - # print('median elapsed time: ', np.median(elapsed_times)) - # # Plot the results - # time = np.linspace(0, simulation_time, num_steps) - - # # Plot positions - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, positions[:, 0], label='True') - # plt.plot(time, positions_est[:, 0], label='Estimated') - # plt.title('Position X') - # plt.xlabel('Time [s]') - # plt.ylabel('Position X [m]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, positions[:, 1], label='True') - # plt.plot(time, positions_est[:, 1], label='Estimated') - # plt.title('Position Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Position Y [m]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, positions[:, 2], label='True') - # plt.plot(time, positions_est[:, 2], label='Estimated') - # plt.title('Position Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Position Z [m]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot orientations (Euler angles) - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, orientations[:, 0], label='True') - # plt.plot(time, orientations_est[:, 0], label='Estimated') - # plt.title('Orientation Roll') - # plt.xlabel('Time [s]') - # plt.ylabel('Roll [rad]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, orientations[:, 1], label='True') - # plt.plot(time, orientations_est[:, 1], label='Estimated') - # plt.title('Orientation Pitch') - # plt.xlabel('Time [s]') - # plt.ylabel('Pitch [rad]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, orientations[:, 2], label='True') - # plt.plot(time, orientations_est[:, 2], label='Estimated') - # plt.title('Orientation Yaw') - # plt.xlabel('Time [s]') - # plt.ylabel('Yaw [rad]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot velocities - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, velocities[:, 0], label='True') - # plt.plot(time, velocities_est[:, 0], label='Estimated') - # plt.title('Velocity X') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity X [m/s]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, velocities[:, 1], label='True') - # plt.plot(time, velocities_est[:, 1], label='Estimated') - # plt.title('Velocity Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity Y [m/s]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, velocities[:, 2], label='True') - # plt.plot(time, velocities_est[:, 2], label='Estimated') - # plt.title('Velocity Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity Z [m/s]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot angular velocities - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, angular_velocities[:, 0], label='True') - # plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - # plt.title('Angular Velocity X') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity X [rad/s]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, angular_velocities[:, 1], label='True') - # plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - # plt.title('Angular Velocity Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity Y [rad/s]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, angular_velocities[:, 2], label='True') - # plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - # plt.title('Angular Velocity Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity Z [rad/s]') - # plt.legend() - - # plt.tight_layout() - # plt.show() \ No newline at end of file + # Update the state for the next iteration + model.state_vector_prev = new_state + + print('Average elapsed time: ', np.mean(elapsed_times)) + print('Max elapsed time: ', np.max(elapsed_times)) + print('Min elapsed time: ', np.min(elapsed_times)) + print('median elapsed time: ', np.median(elapsed_times)) + # Plot the results + time = np.linspace(0, simulation_time, num_steps) + + # Plot positions + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, positions[:, 0], label='True') + plt.plot(time, positions_est[:, 0], label='Estimated') + plt.title('Position X') + plt.xlabel('Time [s]') + plt.ylabel('Position X [m]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, positions[:, 1], label='True') + plt.plot(time, positions_est[:, 1], label='Estimated') + plt.title('Position Y') + plt.xlabel('Time [s]') + plt.ylabel('Position Y [m]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, positions[:, 2], label='True') + plt.plot(time, positions_est[:, 2], label='Estimated') + plt.title('Position Z') + plt.xlabel('Time [s]') + plt.ylabel('Position Z [m]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot orientations (Euler angles) + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, orientations[:, 0], label='True') + plt.plot(time, orientations_est[:, 0], label='Estimated') + plt.title('Orientation Roll') + plt.xlabel('Time [s]') + plt.ylabel('Roll [rad]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, orientations[:, 1], label='True') + plt.plot(time, orientations_est[:, 1], label='Estimated') + plt.title('Orientation Pitch') + plt.xlabel('Time [s]') + plt.ylabel('Pitch [rad]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, orientations[:, 2], label='True') + plt.plot(time, orientations_est[:, 2], label='Estimated') + plt.title('Orientation Yaw') + plt.xlabel('Time [s]') + plt.ylabel('Yaw [rad]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, velocities[:, 0], label='True') + plt.plot(time, velocities_est[:, 0], label='Estimated') + plt.title('Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Velocity X [m/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, velocities[:, 1], label='True') + plt.plot(time, velocities_est[:, 1], label='Estimated') + plt.title('Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Y [m/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, velocities[:, 2], label='True') + plt.plot(time, velocities_est[:, 2], label='Estimated') + plt.title('Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Z [m/s]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot angular velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, angular_velocities[:, 0], label='True') + plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + plt.title('Angular Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity X [rad/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, angular_velocities[:, 1], label='True') + plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + plt.title('Angular Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Y [rad/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, angular_velocities[:, 2], label='True') + plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + plt.title('Angular Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Z [rad/s]') + plt.legend() + + plt.tight_layout() + plt.show() \ No newline at end of file From 4cef60db587e4c09a910748ae1ceef4dfee4f2f3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 3 Apr 2025 17:22:08 +0000 Subject: [PATCH 12/19] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/typedefs.hpp | 40 ++- navigation/eskf/src/eskf.cpp | 69 ++-- navigation/eskf/src/eskf_ros.cpp | 79 +++-- navigation/eskf/src/eskf_utils.cpp | 2 +- .../eskf_python/eskf_python_class.py | 38 +-- .../eskf_python/eskf_python_filter.py | 87 ++--- .../eskf_python/eskf_python_node.py | 32 +- .../eskf_python/eskf_python_utils.py | 82 ++--- .../eskf_python/eskf_python/eskf_test.py | 3 +- .../eskf_python/eskf_test_utils.py | 131 ++++++-- navigation/ukf_okid/ukf_python/rest.py | 31 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 54 +-- .../ukf_okid/ukf_python/ukf_okid_class.py | 307 +++++++++++------- navigation/ukf_okid/ukf_python/ukf_test.py | 106 +++--- navigation/ukf_okid/ukf_python/ukf_utils.py | 18 +- 15 files changed, 631 insertions(+), 448 deletions(-) diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index c0a1e41f9..925d8fd72 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,26 +5,26 @@ #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; +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 struct state_quat { @@ -89,10 +89,8 @@ struct imu_measurement { void correct() { Eigen::Matrix3d R_nb; - R_nb << 0, 0, -1, - 0, -1, 0, - -1, 0, 0; - + R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; + accel = R_nb * accel_uncorrected; gyro = R_nb * gyro_uncorrected; } diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 55d8516dd..58c532afa 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -6,8 +6,7 @@ #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -ESKF::ESKF(const eskf_params& params) : - Q_(params.Q) {} +ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} std::pair ESKF::van_loan_discretization( const Eigen::Matrix18d& A_c, @@ -48,7 +47,7 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { Eigen::Quaterniond q = nom_state.quat.normalized(); Eigen::Matrix3d R_bn = q.toRotationMatrix(); - + Eigen::Vector3d v_n = nom_state.vel; Hx.block<3, 3>(0, 3) = R_bn.transpose(); @@ -59,29 +58,25 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { double qy = q.y(); double qz = q.z(); - dR_dq.col(0) = 2 * Eigen::Vector3d( - qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), - -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), - qy * v_n.x() - qx * v_n.y() + qw * v_n.z() - ); - - dR_dq.col(1) = 2 * Eigen::Vector3d( - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z() - ); - - dR_dq.col(2) = 2 * Eigen::Vector3d( - -qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z() - ); - - dR_dq.col(3) = 2 * Eigen::Vector3d( - -qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z() - ); + dR_dq.col(0) = + 2 * Eigen::Vector3d(qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), + -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), + qy * v_n.x() - qx * v_n.y() + qw * v_n.z()); + + dR_dq.col(1) = + 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + + dR_dq.col(2) = + 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + + dR_dq.col(3) = + 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); Hx.block<3, 4>(0, 6) = dR_dq; @@ -100,7 +95,8 @@ Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { Eigen::Matrix3x1d h; - Eigen::Matrix3d R_bn = nom_state.quat.normalized().toRotationMatrix().transpose(); + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); h = R_bn * nom_state.vel; @@ -110,12 +106,15 @@ Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + nom_state.gravity; + Eigen::Vector3d acc = + nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + + nom_state.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; state_quat next_nom_state; - next_nom_state.pos = nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + next_nom_state.pos = + nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; next_nom_state.vel = nom_state.vel + dt * acc; next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); next_nom_state.quat.normalize(); @@ -188,7 +187,8 @@ std::pair ESKF::injection_and_reset( next_nom_state.pos = nom_state.pos + error_state.pos; next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = nom_state.quat * vector3d_to_quaternion(error_state.euler); + next_nom_state.quat = + nom_state.quat * vector3d_to_quaternion(error_state.euler); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; @@ -209,7 +209,8 @@ std::pair ESKF::imu_update( const imu_measurement& imu_meas, const double dt) { state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = error_state_prediction(error_state, next_nom_state, imu_meas, dt); + state_euler next_error_state = + error_state_prediction(error_state, next_nom_state, imu_meas, dt); return {next_nom_state, next_error_state}; } @@ -218,8 +219,10 @@ std::pair ESKF::dvl_update( const state_quat& nom_state, const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = injection_and_reset(nom_state, new_error_state); + state_euler new_error_state = + measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = + injection_and_reset(nom_state, new_error_state); return {updated_nom_state, updated_error_state}; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 524b48c29..d679d600e 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,11 +1,12 @@ #include "eskf/eskf_ros.hpp" +#include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -#include 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)); + odom_pub_timer_ = this->create_wall_timer( + time_step, std::bind(&ESKFNode::publish_odom, this)); set_subscribers_and_publisher(); @@ -16,49 +17,58 @@ ESKFNode::ESKFNode() : Node("eskf_node") { 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); + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); this->declare_parameter("imu_topic", "imu/data_raw"); 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)); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); this->declare_parameter("dvl_topic", "/orca/twist"); 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)); - + 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", "odom"); std::string odom_topic = this->get_parameter("odom_topic").as_string(); - odom_pub_ = this->create_publisher(odom_topic, qos_sensor_data); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); } void ESKFNode::set_parameters() { - std::vector diag_Q_std; - this->declare_parameter>("diag_Q_std"); // gyroscope bias noise + this->declare_parameter>( + "diag_Q_std"); // gyroscope bias noise diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); - + Eigen::Matrix12d Q; Q.setZero(); - spdlog::info("Q diagonal: {}",diag_Q_std[0]); - Q.diagonal() << - sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), // acceleration noise - sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), // gyroscope noise - sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), // acceleration bias noise - sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); // gyroscope bias noise + spdlog::info("Q diagonal: {}", diag_Q_std[0]); + Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), + sq(diag_Q_std[2]), // acceleration noise + sq(diag_Q_std[3]), sq(diag_Q_std[4]), + sq(diag_Q_std[5]), // gyroscope noise + sq(diag_Q_std[6]), sq(diag_Q_std[7]), + sq(diag_Q_std[8]), // acceleration bias noise + sq(diag_Q_std[9]), sq(diag_Q_std[10]), + sq(diag_Q_std[11]); // gyroscope bias noise eskf_params_.Q = Q; eskf_ = std::make_unique(eskf_params_); Eigen::Matrix18d P; P.setZero(); - P.diagonal() << 1.0, 1.0, 1.0, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.001, 0.001, 0.001, // accel bias - 0.001, 0.001, 0.001, // gyro bias - 0.001, 0.001, 0.001; // gravity + P.diagonal() << 1.0, 1.0, 1.0, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.001, 0.001, 0.001, // accel bias + 0.001, 0.001, 0.001, // gyro bias + 0.001, 0.001, 0.001; // gravity error_state_.covariance = P; } @@ -75,21 +85,28 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel_uncorrected << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; - imu_meas_.gyro_uncorrected << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + imu_meas_.accel_uncorrected << msg->linear_acceleration.x, + msg->linear_acceleration.y, msg->linear_acceleration.z; + imu_meas_.gyro_uncorrected << msg->angular_velocity.x, + msg->angular_velocity.y, msg->angular_velocity.z; imu_meas_.correct(); - std::tie(nom_state_, error_state_) = eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = + eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], - msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - - std::tie(nom_state_, error_state_) = eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = + eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 7a668adfc..930167eaa 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -28,4 +28,4 @@ Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); return q; -} \ No newline at end of file +} diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index 0ba96ba1f..65d41425d 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,7 +1,9 @@ from dataclasses import dataclass, field + import numpy as np from eskf_python_utils import quaternion_error + @dataclass class StateQuat: position: np.ndarray = field( @@ -19,10 +21,8 @@ class StateQuat: gyro_bias: np.ndarray = field( default_factory=lambda: np.zeros(3) ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field( - default_factory=lambda: np.array([0, 0, 0]) - ) # Gravity vector - + g: np.ndarray = field(default_factory=lambda: np.array([0, 0, 0])) # Gravity vector + def as_vector(self) -> np.ndarray: """Returns the state vector as a numpy array. @@ -81,7 +81,7 @@ def R_q(self) -> np.ndarray: ) return R - + def __sub__(self, other: 'StateQuat') -> 'StateQuat': """Subtracts the values of two state vectors. @@ -102,7 +102,6 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': return result - @dataclass class StateEuler: position: np.ndarray = field( @@ -172,12 +171,8 @@ def copy_state(self, wanted_state: 'StateEuler') -> None: @dataclass class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) + measurement: np.ndarray = field(default_factory=lambda: np.zeros(6)) + measurement_covariance: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) def H(self) -> np.ndarray: """Calculates the measurement matrix. @@ -190,19 +185,12 @@ def H(self) -> np.ndarray: H[0:3, 3:6] = np.eye(3) return H - + + @dataclass class Measurement: - acceleration: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) - angular_velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) - aiding: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) + acceleration: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + aiding: np.ndarray = field(default_factory=lambda: np.zeros(3)) - aiding_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((3, 3)) - ) \ No newline at end of file + aiding_covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 890226639..fac3c8526 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -4,7 +4,6 @@ import numpy as np from eskf_python_class import Measurement, StateEuler, StateQuat from eskf_python_utils import ( - R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat, quaternion_product, @@ -44,58 +43,64 @@ def Q_delta_theta(self) -> np.ndarray: return Q_delta_theta def Hx(self) -> np.ndarray: - """ - Computes the true-state measurement Jacobian for the measurement + """Computes the true-state measurement Jacobian for the measurement h(x) = R(q) * velocity, where: - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, - velocity is a 3-vector. - + The state is assumed to be ordered as: [position (3), velocity (3), quaternion (4), ...] (total length 19). - + The Jacobian Hx is a 3x19 matrix with nonzero blocks: - Columns 3:6 (velocity): R(q) - - Columns 6:10 (quaternion): + - Columns 6:10 (quaternion): """ - q = self.nom_state.orientation # shape (4,) - v = self.nom_state.velocity # shape (3,) + v = self.nom_state.velocity # shape (3,) q0, q1, q2, q3 = q v1, v2, v3 = v R = self.nom_state.R_q().transpose() # shape (3, 3) - - dhdq0 = 2 * np.array([ - q0 * v1 - q3 * v2 + q2 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - -q2 * v1 + q1 * v2 + q0 * v3 - ]) - - dhdq1 = 2 * np.array([ - q1 * v1 + q2 * v2 + q3 * v3, - q2 * v1 - q1 * v2 - q0 * v3, - q3 * v1 + q0 * v2 - q1 * v3 - ]) - - dhdq2 = 2 * np.array([ - -q2 * v1 + q1 * v2 + q0 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - -q0 * v1 + q3 * v2 - q2 * v3 - ]) - - dhdq3 = 2 * np.array([ - -q3 * v1 - q0 * v2 + q1 * v3, - q0 * v1 - q3 * v2 + q2 * v3, - q1 * v1 + q2 * v2 + q3 * v3 - ]) - + + dhdq0 = 2 * np.array( + [ + q0 * v1 - q3 * v2 + q2 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + -q2 * v1 + q1 * v2 + q0 * v3, + ] + ) + + dhdq1 = 2 * np.array( + [ + q1 * v1 + q2 * v2 + q3 * v3, + q2 * v1 - q1 * v2 - q0 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + ] + ) + + dhdq2 = 2 * np.array( + [ + -q2 * v1 + q1 * v2 + q0 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + -q0 * v1 + q3 * v2 - q2 * v3, + ] + ) + + dhdq3 = 2 * np.array( + [ + -q3 * v1 - q0 * v2 + q1 * v3, + q0 * v1 - q3 * v2 + q2 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + ] + ) + dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) - + Hx = np.zeros((3, 19)) Hx[:, 3:6] = R Hx[:, 6:10] = dHdq - + return Hx def H(self) -> np.ndarray: @@ -262,14 +267,12 @@ def reset_error_state(self) -> None: self.error_state.fill_states(np.zeros(18)) def imu_update(self, imu_data: Measurement) -> None: - """Updates the state using the IMU data. - """ + """Updates the state using the IMU data.""" self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) def dvl_update(self, dvl_measurement: Measurement) -> float: - """Updates the state using the DVL measurement. - """ + """Updates the state using the DVL measurement.""" NIS = self.measurement_update(dvl_measurement) self.injection() self.reset_error_state() @@ -278,15 +281,13 @@ def dvl_update(self, dvl_measurement: Measurement) -> float: # functions for tuning the filter def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """Calculates the Normalized Innovation Squared (NIS) value. - """ + """Calculates the Normalized Innovation Squared (NIS) value.""" return innovation.T @ np.linalg.inv(S) @ innovation def NEEDS( self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat ) -> float: - """Calculates the Normalized Estimation Error Squared (NEEDS) value. - """ + """Calculates the Normalized Estimation Error Squared (NEEDS) value.""" return ( (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index ec206ab64..7b300ecc6 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 +import numpy as np import rclpy +from geometry_msgs.msg import TwistWithCovarianceStamped from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data from sensor_msgs.msg import Imu -import numpy as np -from geometry_msgs.msg import TwistWithCovarianceStamped # NEED TO CHANGE THIS TO THE CORRECT PATH from eskf_python.eskf_python_filter import ( @@ -33,7 +33,10 @@ def __init__(self): ) self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile + TwistWithCovarianceStamped, + '/dvl/twist', + self.filter_callback, + qos_profile=qos_profile, ) # This publisher will publish the estimtaed state of the vehicle @@ -50,14 +53,22 @@ def __init__(self): self.get_logger().info("Error State Kalman Filter started") def imu_callback(self, msg: Imu): - # Get the IMU data imu_acceleartion = msg.linear_acceleration imu_angular_velocity = msg.angular_velocity # Combine the IMU data - imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) + imu_data = np.array( + [ + imu_acceleartion.x, + imu_acceleartion.y, + imu_acceleartion.z, + imu_angular_velocity.x, + imu_angular_velocity.y, + imu_angular_velocity.z, + ] + ) # Update the filter with the IMU data self.current_state_nom, self.current_state_error = ( @@ -84,8 +95,6 @@ def imu_callback(self, msg: Imu): # Publish self.state_publisher_.publish(self.odom_msg) - - def filter_callback(self, msg: TwistWithCovarianceStamped): """Callback function for the filter measurement update, this will be called when the filter needs to be updated with the DVL data. @@ -93,7 +102,13 @@ def filter_callback(self, msg: TwistWithCovarianceStamped): self.get_logger().info("Filter callback, got DVL data") # Get the DVL data (linear velocity) - dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + dvl_data = np.array( + [ + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ] + ) # Update the filter with the DVL data self.current_state_nom, self.current_state_error = ( @@ -124,7 +139,6 @@ def filter_callback(self, msg: TwistWithCovarianceStamped): self.state_publisher_.publish(self.odom_msg) - def main(args=None): rclpy.init(args=args) node = ESKalmanFilterNode() diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py index aaef5f8d6..bbe13d759 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -1,61 +1,61 @@ import numpy as np + def skew_matrix(vector: np.ndarray) -> np.ndarray: - """ - Returns the skew symmetric matrix of a 3x1 vector. + """Returns the skew symmetric matrix of a 3x1 vector. """ return np.array( [ [0, -vector[2], vector[1]], [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0] + [-vector[1], vector[0], 0], ] ) + def quat_norm(quat: np.ndarray) -> np.ndarray: - """ - Function that normalizes a quaternion + """Function that normalizes a quaternion """ quat = quat / np.linalg.norm(quat) return quat + def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. + """Calculates the quaternion super product of two quaternions. - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. - Returns: - np.ndarray: The quaternion super product. - """ + 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 - 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]) - 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 - np.dot(e_0, e_1) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) - eta_new = eta_0 * eta_1 - np.dot(e_0, e_1) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) + q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new = q_new / np.linalg.norm(q_new) - q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new = q_new / np.linalg.norm(q_new) + return q_new - return q_new def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """Calculates the error between two quaternions """ - 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_product(quat_1, quat_2_inv) return error_quat + def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: """Converts an angle-axis representation to a quaternion. @@ -71,13 +71,12 @@ def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: else: axis = vector / angle - q = np.zeros(4) q[0] = np.cos(angle / 2) q[1:] = np.sin(angle / 2) * axis return q - + def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: """Calculates the rotation matrix from the angle-axis representation. @@ -109,40 +108,41 @@ def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: 1 - 2 * q1**2 - 2 * q2**2, ], ] - ) + ) return R + def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """ - Converts Euler angles to a quaternion + """Converts Euler angles to a quaternion """ psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) - s_psi = np.sin(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 - ]) + 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 + """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)) + 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)) + 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]) - diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 0930d9f6c..2c6d94c09 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -4,7 +4,6 @@ from eskf_python_filter import ESKF from eskf_python_utils import quat_to_euler from eskf_test_utils import StateQuatModel, process_model -from scipy.stats import chi2 def simulate_eskf(): @@ -356,4 +355,4 @@ def simulate_eskf(): plt.tight_layout() plt.show() -""" \ No newline at end of file +""" diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py index 34abe8eda..a60e62ff0 100644 --- a/navigation/eskf_python/eskf_python/eskf_test_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_test_utils.py @@ -1,15 +1,23 @@ -import numpy as np from dataclasses import dataclass, field -from typing import Tuple -from eskf_python_utils import quaternion_product, euler_to_quat, quat_to_euler, quaternion_error, quat_norm, skew_matrix + +import numpy as np +from eskf_python_utils import ( + euler_to_quat, + quat_norm, + quat_to_euler, + quaternion_error, + quaternion_product, + skew_matrix, +) # This was the original code from the ukf_okid.py file + @dataclass class StateQuatModel: + """A class to represent the state to be estimated by the UKF. """ - 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)) @@ -18,7 +26,9 @@ class StateQuatModel: 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]) + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity] + ) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -27,11 +37,25 @@ def nu(self) -> np.ndarray: 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] - ]) + 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: @@ -41,10 +65,14 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + 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_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.orientation = quaternion_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] @@ -52,7 +80,9 @@ def subtract(self, other: 'StateQuatModel') -> np.ndarray: """Subtracts two StateQuatModel 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] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[3:6] = quat_to_euler( + quaternion_error(self.orientation, other.orientation) + ) new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity @@ -92,7 +122,9 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: """Inserts the weights into the covariance matrix.""" new_state = StateQuatModel() new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + 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] @@ -107,9 +139,9 @@ def add_without_quaternions(self, other: 'StateQuatModel') -> None: @dataclass class process_model: + """A class defined for a general process model. """ - A class defined for a general process model. - """ + state_vector: StateQuatModel = field(default_factory=StateQuatModel) state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) @@ -119,7 +151,7 @@ class process_model: 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))) + 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)) @@ -130,22 +162,33 @@ class process_model: 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] - ]) + 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] - ]) + 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: @@ -170,15 +213,31 @@ def model_prediction(self, state: StateQuatModel) -> 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.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) -> StateQuatModel: """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 \ No newline at end of file + 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 diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py index df7392bd4..52cffb2df 100644 --- a/navigation/ukf_okid/ukf_python/rest.py +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -1,28 +1,33 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points + """Function that calculates the mean of a set of points """ n = len(set_points[0].as_vector()) - 1 mean_value = StateQuat() if weights is None: for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + weight_temp_list = (1 / (2 * n + 1)) * np.ones(2 * n + 1) mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) - + + mean_value.orientation = iterative_quaternion_mean_statequat( + set_points, weight_temp_list + ) + else: for i in range(2 * n + 1): mean_value.add_without_quaternions(weights[i] * set_points[i]) - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - + mean_value.orientation = iterative_quaternion_mean_statequat( + set_points, weights + ) + return mean_value.as_vector() -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points + +def mean_measurement( + set_points: list[MeasModel], weights: np.ndarray = None +) -> np.ndarray: + """Function that calculates the mean of a set of points """ n = len(set_points) mean_value = MeasModel() @@ -33,5 +38,5 @@ def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> else: for i in range(n): mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement \ No newline at end of file + + return mean_value.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 16312935a..80a7c939c 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,7 +1,6 @@ -from ukf_okid_class import * + import numpy as np -import time -import matplotlib.pyplot as plt +from ukf_okid_class import * class UKF: @@ -17,8 +16,7 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.T = self.generate_T_matrix(len(P_0)) def generate_T_matrix(self, n: float) -> np.ndarray: - """ - Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + """Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. Parameters: n (int): The state dimension. @@ -29,7 +27,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: T = np.zeros((n, n)) for i in range(n): - for j in range(n//2): + for j in range(n // 2): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) @@ -41,8 +39,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: return T def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """ - Functions that generate the sigma points for the UKF + """Functions that generate the sigma points for the UKF """ n = len(current_state.covariance) @@ -60,18 +57,15 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: return self.sigma_points_list - def unscented_transform(self, current_state: StateQuat) -> StateQuat: + """The unscented transform function generates the priori state estimate """ - The unscented transform function generates the priori state estimate - """ - - _ = self.sigma_points(current_state) + _ = self.sigma_points(current_state) n = len(current_state.covariance) self.y_i = [StateQuat() for _ in range(2 * n)] - for i in range(2 * n ): + for i in range(2 * n): self.process_model.model_prediction(self.sigma_points_list[i]) self.y_i[i] = self.process_model.euler_forward() @@ -82,12 +76,12 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: state_estimate.covariance = covariance_set(self.y_i, x) return state_estimate - def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: - """ - Function that updates the state estimate with a measurement + def measurement_update( + self, current_state: StateQuat, measurement: MeasModel + ) -> tuple[MeasModel, np.ndarray]: + """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)] @@ -100,15 +94,21 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement) + cross_correlation = cross_covariance( + self.y_i, current_state.as_vector(), z_i, meas_update.measurement + ) return meas_update, cross_correlation - def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: - """ - Calculates the posteriori estimate using measurement and the prior estimate + def posteriori_estimate( + self, + current_state: StateQuat, + cross_correlation: np.ndarray, + measurement: MeasModel, + ex_measuremnt: MeasModel, + ) -> StateQuat: + """Calculates the posteriori estimate using measurement and the prior estimate """ - nu_k = MeasModel() nu_k.measurement = measurement.measurement - ex_measuremnt.measurement @@ -118,8 +118,12 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd 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))) + 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)) + ) self.process_model.state_vector_prev = posteriori_estimate diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 4d3281260..181d1f4af 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -1,15 +1,13 @@ from dataclasses import dataclass, field -import numpy as np - -from dataclasses import dataclass, field import numpy as np + @dataclass class StateQuat: + """A class to represent the state to be estimated by the UKF. """ - 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)) @@ -18,7 +16,9 @@ class StateQuat: 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]) + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity] + ) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -27,11 +27,25 @@ def nu(self) -> np.ndarray: 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] - ]) + 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: @@ -41,10 +55,14 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + 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.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] @@ -62,7 +80,9 @@ 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.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 @@ -92,7 +112,9 @@ 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.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] @@ -104,11 +126,12 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.velocity += other.velocity self.angular_velocity += other.angular_velocity + @dataclass class MeasModel: + """A class defined for a general measurement model. """ - 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))) @@ -138,11 +161,12 @@ def __sub__(self, other: 'MeasModel') -> 'MeasModel': result.measurement = self.measurement - other.measurement return result + @dataclass class process_model: + """A class defined for a general 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) @@ -152,7 +176,7 @@ class process_model: 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))) + 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)) @@ -163,22 +187,33 @@ class process_model: 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] - ]) + 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] - ]) + 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: @@ -203,138 +238,157 @@ 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.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 + 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 + """Converts Euler angles to a quaternion """ psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) - s_psi = np.sin(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 - ]) + 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 + """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)) + 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)) + 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 """ - 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. + """Calculates the skew symmetric matrix of a vector. - Args: - vector (np.ndarray): The 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], + ] + ) - 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. + """Calculates the quaternion super product of two quaternions. - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + 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 + 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]) - 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) - 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]])) - q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) + return q_new - return q_new def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """Calculates the error between two quaternions """ - 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 weighted mean of the quaternion orientations from a list of StateQuat objects + +def iterative_quaternion_mean_statequat( + state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 +) -> np.ndarray: + """Computes the weighted mean of the quaternion orientations from a list of StateQuat objects using an iterative approach, without requiring the caller to manually extract the quaternion. - + Parameters: state_list (list[StateQuat]): List of StateQuat objects. weights (np.ndarray): Weights for each state. tol (float): Convergence tolerance. max_iter (int): Maximum number of iterations. - + Returns: np.ndarray: The averaged quaternion as a 4-element 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: @@ -342,28 +396,32 @@ def iterative_quaternion_mean_statequat(state_list: list[StateQuat], tol: float 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))]) + 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: - """ - Functio calculates the mean vector of a set of points - - Args: + """Function calculates the mean vector of a set of points + + Args: set_points (list[StateQuat]): List of StateQuat objects Returns: @@ -374,30 +432,30 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: for state in set_points: mean_value.add_without_quaternions(state) - - mean_value = (1 / (n)) * mean_value + + mean_value = (1 / (n)) * mean_value 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 + """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 + """Function that calculates the covariance of a set of points """ n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -410,23 +468,25 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: 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] + 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 += 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 + """Function that calculates the covariance of a set of points """ n = len(set_points) co_size = len(set_points[0].measurement) @@ -443,9 +503,14 @@ def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np. 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 + +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) @@ -458,16 +523,18 @@ def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[Mea 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] + 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 += 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_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 3c197ed63..cebb53ac6 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,27 +1,28 @@ -from ukf_okid import UKF -from ukf_okid_class import StateQuat, process_model, MeasModel -import numpy as np import time -import matplotlib.pyplot as plt -from ukf_utils import print_StateQuat_list, print_StateQuat -from ukf_okid_class import quaternion_super_product, quat_to_euler, mean_set, covariance_set +import matplotlib.pyplot as plt +import numpy as np +from ukf_okid import UKF +from ukf_okid_class import ( + MeasModel, + StateQuat, + process_model, + quat_to_euler, + quaternion_super_product, +) def add_quaternion_noise(q, noise_std): - noise = np.random.normal(0, noise_std, 3) theta = np.linalg.norm(noise) if theta > 0: - axis = noise / theta - q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + q_noise = np.hstack((np.cos(theta / 2), np.sin(theta / 2) * axis)) else: - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) q_new = quaternion_super_product(q, q_noise) @@ -30,7 +31,6 @@ def add_quaternion_noise(q, noise_std): if __name__ == '__main__': - # Create initial state vector and covariance matrix. x0 = np.zeros(13) x0[0:3] = [0.3, 0.3, 0.3] @@ -38,20 +38,22 @@ def add_quaternion_noise(q, noise_std): x0[7:10] = [0.2, 0.2, 0.2] dt = 0.01 R = (0.01) * np.eye(3) - + Q = 0.00015 * np.eye(12) P0 = np.eye(12) * 0.0001 model = process_model() model.dt = 0.01 - 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] - ]) + 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], + ] + ) model.m = 30.0 model.r_b_bg = np.array([0.01, 0.0, 0.02]) model.inertia = np.diag([0.68, 3.32, 3.34]) @@ -61,14 +63,16 @@ def add_quaternion_noise(q, noise_std): model_ukf = process_model() model_ukf.dt = 0.01 - model_ukf.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] - ]) + model_ukf.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], + ] + ) model_ukf.m = 30.0 model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) @@ -101,7 +105,7 @@ def add_quaternion_noise(q, noise_std): measurment_model = MeasModel() measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R + measurment_model.covariance = R # Initialize arrays to store the results positions = np.zeros((num_steps, 3)) @@ -129,7 +133,16 @@ def add_quaternion_noise(q, noise_std): elapsed_times = [] - u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + u = lambda t: np.array( + [ + 2 * np.sin(1 * t), + 2 * np.sin(1 * t), + 2 * np.sin(1 * t), + 0.2 * np.cos(1 * t), + 0.2 * np.cos(1 * t), + 0.2 * np.cos(1 * t), + ] + ) # Run the simulation for step in range(num_steps): @@ -142,10 +155,18 @@ def add_quaternion_noise(q, noise_std): new_state = model.euler_forward() # Adding noise in the state vector - estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + estimated_state.position = ( + estimated_state.position + ) # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = ( + estimated_state.orientation + ) # add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = ( + estimated_state.velocity + ) # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = ( + estimated_state.angular_velocity + ) # + np.random.normal(0, 0.01, 3) start_time = time.time() estimated_state = ukf.unscented_transform(estimated_state) @@ -155,10 +176,15 @@ def add_quaternion_noise(q, noise_std): elapsed_times.append(elapsed_time) if step % 20 == 0: - measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - + measurment_model.measurement = ( + new_state.velocity + ) # + np.random.normal(0, 0.01, 3) + meas_update, covariance_matrix = ukf.measurement_update( + estimated_state, measurment_model + ) + estimated_state = ukf.posteriori_estimate( + estimated_state, covariance_matrix, measurment_model, meas_update + ) positions[step, :] = new_state.position orientations[step, :] = quat_to_euler(new_state.orientation) @@ -169,7 +195,7 @@ def add_quaternion_noise(q, noise_std): orientations_est[step, :] = quat_to_euler(estimated_state.orientation) velocities_est[step, :] = estimated_state.velocity angular_velocities_est[step, :] = estimated_state.angular_velocity - + # Update the state for the next iteration model.state_vector_prev = new_state @@ -294,4 +320,4 @@ def add_quaternion_noise(q, noise_std): plt.legend() plt.tight_layout() - plt.show() \ No newline at end of file + plt.show() diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index ad5871567..da56a7dfc 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -1,19 +1,21 @@ + import numpy as np -from dataclasses import dataclass 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. + +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. + """Custom print function to print StateQuat objects in a formatted form. """ print(f"{name}:") print(f" Position: {state.position}") @@ -24,9 +26,9 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): if print_covariance: print_matrix(state.covariance, "Covariance") + def print_matrix(matrix, name="Matrix"): - """ - Custom print function to print matrices in a formatted form. + """Custom print function to print matrices in a formatted form. """ print(f"{name}: {matrix.shape}") if isinstance(matrix, np.ndarray): From 9b44fce6228741339964ced4b14f0bba47c4d774 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 3 Apr 2025 19:31:45 +0200 Subject: [PATCH 13/19] refactor: remove python eskf --- navigation/eskf_python/CMakeLists.txt | 33 -- navigation/eskf_python/README.md | 0 .../eskf_python/config/eskf_python.yaml | 3 - .../eskf_python/eskf_python/__init__.py | 0 .../eskf_python/eskf_python_class.py | 196 ---------- .../eskf_python/eskf_python_filter.py | 295 --------------- .../eskf_python/eskf_python_node.py | 151 -------- .../eskf_python/eskf_python_utils.py | 148 -------- .../eskf_python/eskf_python/eskf_test.py | 358 ------------------ .../eskf_python/eskf_test_utils.py | 243 ------------ navigation/eskf_python/launch/eskf.launch.py | 22 -- navigation/eskf_python/package.xml | 23 -- navigation/ukf_okid/ukf_python/rest.py | 6 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 10 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 33 +- navigation/ukf_okid/ukf_python/ukf_utils.py | 10 +- 16 files changed, 19 insertions(+), 1512 deletions(-) delete mode 100644 navigation/eskf_python/CMakeLists.txt delete mode 100644 navigation/eskf_python/README.md delete mode 100644 navigation/eskf_python/config/eskf_python.yaml delete mode 100644 navigation/eskf_python/eskf_python/__init__.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_class.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_filter.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_node.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_utils.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_test.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_test_utils.py delete mode 100644 navigation/eskf_python/launch/eskf.launch.py delete mode 100644 navigation/eskf_python/package.xml diff --git a/navigation/eskf_python/CMakeLists.txt b/navigation/eskf_python/CMakeLists.txt deleted file mode 100644 index b4fc9118c..000000000 --- a/navigation/eskf_python/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(eskf_python) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - eskf_python/eskf_python_node.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) -endif() - -ament_package() diff --git a/navigation/eskf_python/README.md b/navigation/eskf_python/README.md deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/eskf_python/config/eskf_python.yaml b/navigation/eskf_python/config/eskf_python.yaml deleted file mode 100644 index 0d80b90df..000000000 --- a/navigation/eskf_python/config/eskf_python.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - eskf_python_node: diff --git a/navigation/eskf_python/eskf_python/__init__.py b/navigation/eskf_python/eskf_python/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py deleted file mode 100644 index 65d41425d..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ /dev/null @@ -1,196 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from eskf_python_utils import quaternion_error - - -@dataclass -class StateQuat: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.array([1, 0, 0, 0]) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field(default_factory=lambda: np.array([0, 0, 0])) # Gravity vector - - def as_vector(self) -> np.ndarray: - """Returns the state vector as a numpy array. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - self.g, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:10] - self.acceleration_bias = state[10:13] - self.gyro_bias = state[13:16] - self.g = state[16:19] - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. - - Returns: - np.ndarray: The rotation matrix. - """ - 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 __sub__(self, other: 'StateQuat') -> 'StateQuat': - """Subtracts the values of two state vectors. - - Args: - other (StateQuat): The state vector to subtract. - - Returns: - np.ndarray: The difference between the two state vectors. - """ - result = StateQuat() - result.position = self.position - other.position - result.velocity = self.velocity - other.velocity - result.orientation = quaternion_error(self.orientation, other.orientation) - result.acceleration_bias = self.acceleration_bias - other.acceleration_bias - result.gyro_bias = self.gyro_bias - other.gyro_bias - result.g = self.g - other.g - - return result - - -@dataclass -class StateEuler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field( - default_factory=lambda: np.array([0, 0, 9.81]) - ) # Gravity vector - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((18, 18)) - ) # Covariance matrix - - def as_vector(self) -> np.ndarray: - """Returns the state vector as a numpy array. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - self.g, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - self.g = state[15:18] - - def copy_state(self, wanted_state: 'StateEuler') -> None: - """Copies the state from a StateVector object into the current StateVector object. - - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field(default_factory=lambda: np.zeros(6)) - measurement_covariance: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - H = np.zeros((3, 15)) - - H[0:3, 3:6] = np.eye(3) - - return H - - -@dataclass -class Measurement: - acceleration: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - aiding: np.ndarray = field(default_factory=lambda: np.zeros(3)) - - aiding_covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py deleted file mode 100644 index fac3c8526..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ /dev/null @@ -1,295 +0,0 @@ -# from dataclasses import dataclass -from typing import Tuple - -import numpy as np -from eskf_python_class import Measurement, StateEuler, StateQuat -from eskf_python_utils import ( - angle_axis_to_quaternion, - euler_to_quat, - quaternion_product, - skew_matrix, -) -from scipy.linalg import block_diag, expm - - -class ESKF: - def __init__( - self, Q: np.ndarray, P0, nom_state: StateQuat, p_accBias, p_gyroBias, dt - ): - self.Q = Q - self.dt = dt - self.nom_state = nom_state - self.error_state = StateEuler() - self.error_state.covariance = P0 - self.p_accBias = p_accBias - self.p_gyroBias = p_gyroBias - - def Q_delta_theta(self) -> np.ndarray: - """Calculates the Q_delta_theta matrix. - See Joan Solà. Quaternion kinematics for the error-state Kalman filter. - chapter: 6.1.1 eq. 281 - """ - qw, qx, qy, qz = self.nom_state.orientation - - Q_delta_theta = 0.5 * np.array( - [ - [-qx, -qy, -qz], - [qw, -qz, qy], - [qz, qw, -qx], - [-qy, qx, qw], - ] - ) - - return Q_delta_theta - - def Hx(self) -> np.ndarray: - """Computes the true-state measurement Jacobian for the measurement - h(x) = R(q) * velocity, - where: - - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, - - velocity is a 3-vector. - - The state is assumed to be ordered as: - [position (3), velocity (3), quaternion (4), ...] (total length 19). - - The Jacobian Hx is a 3x19 matrix with nonzero blocks: - - Columns 3:6 (velocity): R(q) - - Columns 6:10 (quaternion): - """ - q = self.nom_state.orientation # shape (4,) - v = self.nom_state.velocity # shape (3,) - q0, q1, q2, q3 = q - v1, v2, v3 = v - - R = self.nom_state.R_q().transpose() # shape (3, 3) - - dhdq0 = 2 * np.array( - [ - q0 * v1 - q3 * v2 + q2 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - -q2 * v1 + q1 * v2 + q0 * v3, - ] - ) - - dhdq1 = 2 * np.array( - [ - q1 * v1 + q2 * v2 + q3 * v3, - q2 * v1 - q1 * v2 - q0 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - ] - ) - - dhdq2 = 2 * np.array( - [ - -q2 * v1 + q1 * v2 + q0 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - -q0 * v1 + q3 * v2 - q2 * v3, - ] - ) - - dhdq3 = 2 * np.array( - [ - -q3 * v1 - q0 * v2 + q1 * v3, - q0 * v1 - q3 * v2 + q2 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - ] - ) - - dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) - - Hx = np.zeros((3, 19)) - Hx[:, 3:6] = R - Hx[:, 6:10] = dHdq - - return Hx - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - - H = self.Hx() @ X_deltax - - return H - - def h(self) -> np.ndarray: - """Calculates the measurement model. - - Returns: - np.ndarray: The measurement model. - """ - return self.nom_state.R_q() @ self.nom_state.velocity - - def nominal_state_discrete(self, imu_data: Measurement) -> None: - """Calculates the next nominal state using the discrete-time process model defined in: - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter: 5.4.1 The nominal state kinematics - - Args: - imu_data (np.ndarray): The IMU data. - """ - # Rectify measurements. - acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias - gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - - R = self.nom_state.R_q() - - self.nom_state.position = ( - self.nom_state.position - + self.nom_state.velocity * self.dt - + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 - ) - self.nom_state.velocity = ( - self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt - ) - self.nom_state.orientation = quaternion_product( - self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt) - ) - self.nom_state.acceleration_bias = self.nom_state.acceleration_bias - self.nom_state.gyro_bias = self.nom_state.gyro_bias - self.nom_state.g = self.nom_state.g - - def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: - """Calculates the Van Loan discretization of a continuous-time system. - - Args: - A_c (np.ndarray): The A matrix. - G_c (np.ndarray): The G matrix. - - Returns: - Tuple: The A_d and GQG_d matrices. - """ - GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) - - matrix_exp = ( - np.block( - [ - [-A_c, GQG_T], - [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)], - ] - ) - * self.dt - ) - - van_loan_matrix = expm(matrix_exp) - - V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] - V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] - - A_d = V1.T - GQG_d = A_d @ V2 - - return A_d, GQG_d - - def error_state_prediction(self, imu_data: Measurement) -> None: - # Rectify measurements. - acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias - gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - - R = self.nom_state.R_q() - - A_c = np.zeros((18, 18)) - - A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -R @ skew_matrix(acc_rect) - A_c[6:9, 6:9] = -skew_matrix(gyro_rect) - A_c[3:6, 9:12] = -R - A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) - A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) - A_c[6:9, 12:15] = -np.eye(3) - A_c[3:6, 15:18] = np.eye(3) - - G_c = np.zeros((18, 12)) - - G_c[3:6, 0:3] = -R - G_c[6:9, 3:6] = -np.eye(3) - G_c[9:12, 6:9] = np.eye(3) - G_c[12:15, 9:12] = np.eye(3) - - A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - - self.error_state.covariance = A_d @ self.error_state.covariance @ A_d.T + GQG_d - - def measurement_update(self, dvl_measurement: Measurement) -> float: - """Updates the error state using the DVL measurement. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter: 6.1 eq. 274-276 - - Args: - dvl_measurement (np.ndarray): The DVL measurement. - """ - H = self.H() - P = self.error_state.covariance - R = dvl_measurement.aiding_covariance - - S = H @ P @ H.T + R - K = P @ H.T @ np.linalg.inv(S) - innovation = dvl_measurement.aiding - self.h() - - NIS_value = self.NIS(S, innovation) - - self.error_state.fill_states(K @ innovation) - - I_KH = np.eye(18) - K @ H - self.error_state.covariance = ( - I_KH @ P @ I_KH.T + K @ R @ K.T - ) # Joseph form for more stability - return NIS_value - - def injection(self) -> None: - """Injects the error state into the nominal state to produce the estimated state. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter 6.2 eq. 282-283 - - """ - self.nom_state.position = self.nom_state.position + self.error_state.position - self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity - self.nom_state.orientation = quaternion_product( - self.nom_state.orientation, euler_to_quat(self.error_state.orientation) - ) - self.nom_state.acceleration_bias = ( - self.nom_state.acceleration_bias + self.error_state.acceleration_bias - ) - self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias - self.nom_state.g = self.nom_state.g + self.error_state.g - - def reset_error_state(self) -> None: - """Resets the error state after injection. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter 6.3 eq. 284-286 - """ - G = np.eye(18) # Neglecting the delta_theta as this is most common in practice - - self.error_state.covariance = G @ self.error_state.covariance @ G.T - self.error_state.fill_states(np.zeros(18)) - - def imu_update(self, imu_data: Measurement) -> None: - """Updates the state using the IMU data.""" - self.nominal_state_discrete(imu_data) - self.error_state_prediction(imu_data) - - def dvl_update(self, dvl_measurement: Measurement) -> float: - """Updates the state using the DVL measurement.""" - NIS = self.measurement_update(dvl_measurement) - self.injection() - self.reset_error_state() - - return NIS - - # functions for tuning the filter - def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """Calculates the Normalized Innovation Squared (NIS) value.""" - return innovation.T @ np.linalg.inv(S) @ innovation - - def NEEDS( - self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat - ) -> float: - """Calculates the Normalized Estimation Error Squared (NEEDS) value.""" - return ( - (true_state - estimate_state).as_vector().T - @ np.linalg.inv(P) - @ (true_state - estimate_state).as_vector() - ) diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py deleted file mode 100644 index 7b300ecc6..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ /dev/null @@ -1,151 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import rclpy -from geometry_msgs.msg import TwistWithCovarianceStamped -from nav_msgs.msg import Odometry -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu - -# NEED TO CHANGE THIS TO THE CORRECT PATH -from eskf_python.eskf_python_filter import ( - ErrorStateKalmanFilter, - MeasurementModel, - StateVector_euler, - StateVector_quaternion, -) - -qos_profile = QoSProfile( - depth=1, - history=qos_profile_sensor_data.history, - reliability=qos_profile_sensor_data.reliability, -) - - -class ESKalmanFilterNode(Node): - def __init__(self): - super().__init__("eskf_python_node") - - # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - self.imu_subscriber_ = self.create_subscription( - Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile - ) - - self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, - '/dvl/twist', - self.filter_callback, - qos_profile=qos_profile, - ) - - # This publisher will publish the estimtaed state of the vehicle - self.state_publisher_ = self.create_publisher( - Odometry, '/orca/odom', qos_profile=qos_profile - ) - - self.eskf_modual = ErrorStateKalmanFilter() - self.current_state_nom = StateVector_quaternion() - self.current_state_error = StateVector_euler() - self.measurement_pred = MeasurementModel() - self.odom_msg = Odometry() - - self.get_logger().info("Error State Kalman Filter started") - - def imu_callback(self, msg: Imu): - # Get the IMU data - - imu_acceleartion = msg.linear_acceleration - imu_angular_velocity = msg.angular_velocity - - # Combine the IMU data - imu_data = np.array( - [ - imu_acceleartion.x, - imu_acceleartion.y, - imu_acceleartion.z, - imu_angular_velocity.x, - imu_angular_velocity.y, - imu_angular_velocity.z, - ] - ) - - # Update the filter with the IMU data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.imu_update_states( - self.current_state_nom, self.current_state_error, imu_data - ) - ) - - # Inserting the nominal state into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x - self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y - self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z - - # Publish - self.state_publisher_.publish(self.odom_msg) - - def filter_callback(self, msg: TwistWithCovarianceStamped): - """Callback function for the filter measurement update, - this will be called when the filter needs to be updated with the DVL data. - """ - self.get_logger().info("Filter callback, got DVL data") - - # Get the DVL data (linear velocity) - dvl_data = np.array( - [ - msg.twist.twist.linear.x, - msg.twist.twist.linear.y, - msg.twist.twist.linear.z, - ] - ) - - # Update the filter with the DVL data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.dvl_update_states( - self.current_state_nom, self.current_state_error, dvl_data - ) - ) - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.injection_and_reset( - self.current_state_nom, self.current_state_error - ) - ) - - # Inserting data into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - - # Publishing the data - self.state_publisher_.publish(self.odom_msg) - - -def main(args=None): - rclpy.init(args=args) - node = ESKalmanFilterNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py deleted file mode 100644 index bbe13d759..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ /dev/null @@ -1,148 +0,0 @@ -import numpy as np - - -def skew_matrix(vector: np.ndarray) -> np.ndarray: - """Returns the skew symmetric matrix of a 3x1 vector. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion - """ - quat = quat / np.linalg.norm(quat) - - return quat - - -def quaternion_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 - np.dot(e_0, e_1) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) - - q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new = q_new / np.linalg.norm(q_new) - - 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_product(quat_1, quat_2_inv) - - return error_quat - - -def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: - """Converts an angle-axis representation to a quaternion. - - Args: - vector (np.ndarray): The angle-axis representation. - - Returns: - np.ndarray: The quaternion representation. - """ - angle = np.linalg.norm(vector) - if angle < 1e-8: - return np.array([1, 0, 0, 0]) - else: - axis = vector / angle - - q = np.zeros(4) - q[0] = np.cos(angle / 2) - q[1:] = np.sin(angle / 2) * axis - - return q - - -def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: - """Calculates the rotation matrix from the angle-axis representation. - - Args: - vector (np.ndarray): The angle-axis representation. - - Returns: - np.ndarray: The rotation matrix. - """ - quaternion = angle_axis_to_quaternion(vector) - q0, q1, q2, q3 = quaternion - - 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 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]) diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py deleted file mode 100644 index 2c6d94c09..000000000 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ /dev/null @@ -1,358 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -from eskf_python_class import Measurement, StateQuat -from eskf_python_filter import ESKF -from eskf_python_utils import quat_to_euler -from eskf_test_utils import StateQuatModel, process_model - - -def simulate_eskf(): - # Simulation parameters - simulation_time = 20.0 # seconds - dt = 0.01 - num_steps = int(simulation_time / dt) - time = np.linspace(0, simulation_time, num_steps) - - # ----------------------- Setup Initial States, Filter & Model ----------------------- - # True initial state - true_state_init = StateQuat() - true_state_init.position = np.array([0.1, 0.0, 0.0]) - true_state_init.velocity = np.array([0.1, 0.0, 0.0]) - P0 = np.diag( - [ - 0.3, - 0.3, - 0.3, # Position - 0.2, - 0.2, - 0.2, # Velocity - 0.2, - 0.2, - 0.2, # Orientation - 0.0001, - 0.0001, - 0.0001, # Acceleration bias - 0.00001, - 0.00001, - 0.00001, # Gyro bias - 0.00001, - 0.00001, - 0.00001, # Gravity - ] - ) - # Noise parameters - Q = np.diag( - [ - (0.13**2), - (0.13**2), - (0.13**2), # Adjusted Accelerometer noise - (0.13**2), - (0.13**2), - (0.13**2), # Adjusted Gyroscope noise - 0.0001, - 0.0001, - 0.0001, # Adjusted Acceleration bias random walk - 0.0001, - 0.0001, - 0.0001, # Adjusted Gyro bias random walk - ] - ) - - # Create filter object - eskf = ESKF(Q, P0, true_state_init, 1e-13, 1e-13, dt) - - # Create measurement objects - imu_data = Measurement() - dvl_data = Measurement() - - # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag( - [(0.01) ** 2, (0.01) ** 2, (0.01) ** 2] - ) # Adjusted DVL aiding covariance - - # Setup the process model for simulation of AUV - model = process_model() - model.dt = dt - 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], - ] - ) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) - - # Initialize a dummy state for simulation dynamics. - # Two where made since there seems to be an issue with declaring two identical objects. - new_state = StateQuatModel() - new_state.position = np.array([0.1, 0.0, 0.0]) - new_state.velocity = np.array([0.1, 0.0, 0.0]) - - new_state_prev = StateQuatModel() - new_state_prev.position = np.array([0.1, 0.0, 0.0]) - new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) - - model.state_vector = new_state - model.state_vector_prev = new_state_prev - - # Initialize arrays to store true and estimated states - true_positions = np.zeros((num_steps, 3)) - true_orientations = np.zeros((num_steps, 3)) - true_velocities = np.zeros((num_steps, 3)) - - est_positions = np.zeros((num_steps, 3)) - est_orientations = np.zeros((num_steps, 3)) - est_velocities = np.zeros((num_steps, 3)) - - # covariance arrays - pos_cov = np.zeros((num_steps, 3)) - vel_cov = np.zeros((num_steps, 3)) - ori_cov = np.zeros((num_steps, 3)) - - prev_velocity = np.zeros(3) - u = lambda t: np.array( - [ - 0.5 * np.sin(0.1 * t), - 0.5 * np.sin(0.1 * t + 0.3), - 0.5 * np.sin(0.1 * t + 0.6), - 0.05 * np.cos(0.1 * t), - 0.05 * np.cos(0.1 * t + 0.3), - 0.05 * np.cos(0.1 * t + 0.6), - ] - ) - - NIS_list = [] - NIS_value = 0.0 - - # Sim - for step in range(num_steps): - t = step * dt - - model.Control_input = u(t) - model.model_prediction(new_state) - new_state = model.euler_forward() - - imu_data.acceleration = ( - (new_state.velocity - prev_velocity) / dt - ) + np.random.normal(0, 0.13, 3) - imu_data.angular_velocity = new_state.angular_velocity + np.random.normal( - 0, 0.13, 3 - ) - - eskf.imu_update(imu_data) - - if step % 200 == 0: - dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) - NIS_value = eskf.dvl_update(dvl_data) - NIS_list.append(NIS_value) - - true_positions[step, :] = np.copy(new_state.position) - true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) - true_velocities[step, :] = np.copy(new_state.velocity) - - est_positions[step, :] = np.copy(eskf.nom_state.position) - est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) - est_velocities[step, :] = np.copy(eskf.nom_state.velocity) - - P_diag = np.diag(eskf.error_state.covariance) - pos_cov[step, :] = P_diag[0:3] - vel_cov[step, :] = P_diag[3:6] - ori_cov[step, :] = P_diag[6:9] - - prev_velocity = new_state.velocity - model.state_vector_prev = new_state - - return ( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - NIS_list, - ) - - -( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - _, -) = simulate_eskf() - -# Plotting -axis_labels_pos = ["X", "Y", "Z"] -axis_labels_vel = ["X", "Y", "Z"] -axis_labels_ori = ["Roll", "Pitch", "Yaw"] - -# Plot Position -fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) -fig_pos.suptitle("True Data vs Filter Estimates for Position") -for i in range(3): - ax_pos = axs_pos[i] - ax_pos.plot( - time, - true_positions[:, i], - label=f"True Pos {axis_labels_pos[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_pos.plot( - time, - est_positions[:, i], - label=f"Est Pos {axis_labels_pos[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between( - time, - est_positions[:, i] - sigma_pos, - est_positions[:, i] + sigma_pos, - color=f"C{i}", - alpha=0.2, - ) - ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") - ax_pos.set_xlabel("Time [s]") - ax_pos.set_ylabel("Position") - ax_pos.grid(True) - ax_pos.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - -# Plot Velocity -fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) -fig_vel.suptitle("True Data vs Filter Estimates for Velocity") -for i in range(3): - ax_vel = axs_vel[i] - ax_vel.plot( - time, - true_velocities[:, i], - label=f"True Vel {axis_labels_vel[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_vel.plot( - time, - est_velocities[:, i], - label=f"Est Vel {axis_labels_vel[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between( - time, - est_velocities[:, i] - sigma_vel, - est_velocities[:, i] + sigma_vel, - color=f"C{i}", - alpha=0.2, - ) - ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") - ax_vel.set_xlabel("Time [s]") - ax_vel.set_ylabel("Velocity") - ax_vel.grid(True) - ax_vel.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - -# Plot Orientation -fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) -fig_ori.suptitle("True Data vs Filter Estimates for Orientation") -for i in range(3): - ax_ori = axs_ori[i] - ax_ori.plot( - time, - true_orientations[:, i], - label=f"True Ori {axis_labels_ori[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_ori.plot( - time, - est_orientations[:, i], - label=f"Est Ori {axis_labels_ori[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between( - time, - est_orientations[:, i] - sigma_ori, - est_orientations[:, i] + sigma_ori, - color=f"C{i}", - alpha=0.2, - ) - ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") - ax_ori.set_xlabel("Time [s]") - ax_ori.set_ylabel("Orientation") - ax_ori.grid(True) - ax_ori.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - - -### _______ NIS AND NEEDS _______ -""" -num_simulations = 10 -NIS_runs = [] - -for sim in range(num_simulations): - ( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - NIS_list, - ) = simulate_eskf() - - NIS_runs.append(np.array(NIS_list)) - -NIS_runs = np.vstack(NIS_runs) -ANIS = np.mean(NIS_runs, axis=0) - -measurement_dimension = 3 - -chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations -chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations - -time_steps = np.arange(len(ANIS)) * 0.01 * 20 - -fig, ax = plt.subplots(figsize=(10, 6)) -ax.plot(time_steps, ANIS, label="ANIS", color="C0") -ax.axhline(chi2_lower, color="C1", linestyle="--", label="95% CI Lower") -ax.axhline(chi2_upper, color="C2", linestyle="--", label="95% CI Upper") -ax.set_title("Average Normalized Innovation Squared (ANIS)") -ax.set_xlabel("Time [s]") -ax.set_ylabel("ANIS") -ax.grid(True) -ax.legend() - -plt.tight_layout() -plt.show() -""" diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py deleted file mode 100644 index a60e62ff0..000000000 --- a/navigation/eskf_python/eskf_python/eskf_test_utils.py +++ /dev/null @@ -1,243 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from eskf_python_utils import ( - euler_to_quat, - quat_norm, - quat_to_euler, - quaternion_error, - quaternion_product, - skew_matrix, -) - -# This was the original code from the ukf_okid.py file - - -@dataclass -class StateQuatModel: - """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)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) - - 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] - ) - - 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] - - 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_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 subtract(self, other: 'StateQuatModel') -> np.ndarray: - """Subtracts two StateQuatModel 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] = quat_to_euler( - quaternion_error(self.orientation, other.orientation) - ) - new_array[6:9] = self.velocity - other.velocity - new_array[9:12] = self.angular_velocity - other.angular_velocity - - return new_array - - def __add__(self, other: 'StateQuatModel') -> 'StateQuatModel': - """Adds two StateQuatModel objects.""" - new_state = StateQuatModel() - new_state.position = self.position + other.position - new_state.orientation = quaternion_product(self.orientation, other.orientation) - new_state.velocity = self.velocity + other.velocity - new_state.angular_velocity = self.angular_velocity + other.angular_velocity - - return new_state - - def __sub__(self, other: 'StateQuatModel') -> 'StateQuatModel': - """Subtracts two StateQuatModel objects.""" - new_state = StateQuatModel() - 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 - - return new_state.as_vector() - - def __rmul__(self, scalar: float) -> 'StateQuatModel': - """Multiplies the StateQuatModel object by a scalar.""" - new_state = StateQuatModel() - 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 - - return new_state - - def insert_weights(self, weights: np.ndarray) -> np.ndarray: - """Inserts the weights into the covariance matrix.""" - new_state = StateQuatModel() - 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] - - return new_state.as_vector() - - def add_without_quaternions(self, other: 'StateQuatModel') -> 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 - - -@dataclass -class process_model: - """A class defined for a general process model. - """ - - state_vector: StateQuatModel = field(default_factory=StateQuatModel) - state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) - state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) - 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_matrix(ang_vel) - lever_arm_skew = skew_matrix(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_matrix(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: StateQuatModel) -> 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) -> StateQuatModel: - """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 diff --git a/navigation/eskf_python/launch/eskf.launch.py b/navigation/eskf_python/launch/eskf.launch.py deleted file mode 100644 index 3cae83dce..000000000 --- a/navigation/eskf_python/launch/eskf.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - eskf_python_node = Node( - package='eskf_python', - executable='eskf_python_node.py', - name='eskf_python_node', - parameters=[ - os.path.join( - get_package_share_directory('eskf_python'), - 'config', - 'eskf_python.yaml', - ), - ], - output='screen', - ) - return LaunchDescription([eskf_python_node]) diff --git a/navigation/eskf_python/package.xml b/navigation/eskf_python/package.xml deleted file mode 100644 index 980653c40..000000000 --- a/navigation/eskf_python/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - eskf_python - 1.0.0 - This package provides the implementation of a error-state kalman filter in python - talhanc - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - - ament_cmake - - diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py index 52cffb2df..c8b4f3b14 100644 --- a/navigation/ukf_okid/ukf_python/rest.py +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -1,6 +1,5 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points[0].as_vector()) - 1 mean_value = StateQuat() @@ -27,8 +26,7 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndar def mean_measurement( set_points: list[MeasModel], weights: np.ndarray = None ) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points) mean_value = MeasModel() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 80a7c939c..50c68e08c 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,4 +1,3 @@ - import numpy as np from ukf_okid_class import * @@ -39,8 +38,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: return T def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """Functions that generate the sigma points for the UKF - """ + """Functions that generate the sigma points for the UKF""" n = len(current_state.covariance) I = np.hstack([np.eye(n), -np.eye(n)]) @@ -58,8 +56,7 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: return self.sigma_points_list def unscented_transform(self, current_state: StateQuat) -> StateQuat: - """The unscented transform function generates the priori state estimate - """ + """The unscented transform function generates the priori state estimate""" _ = self.sigma_points(current_state) n = len(current_state.covariance) @@ -107,8 +104,7 @@ def posteriori_estimate( measurement: MeasModel, ex_measuremnt: MeasModel, ) -> StateQuat: - """Calculates the posteriori estimate using measurement and the prior estimate - """ + """Calculates the posteriori estimate using measurement and the prior estimate""" nu_k = MeasModel() nu_k.measurement = measurement.measurement - ex_measuremnt.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 181d1f4af..45bbffcfe 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -5,8 +5,7 @@ @dataclass class StateQuat: - """A class to represent the state to be estimated by the UKF. - """ + """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])) @@ -129,8 +128,7 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: @dataclass class MeasModel: - """A class defined for a general measurement model. - """ + """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))) @@ -164,8 +162,7 @@ def __sub__(self, other: 'MeasModel') -> 'MeasModel': @dataclass class process_model: - """A class defined for a general 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) @@ -269,8 +266,7 @@ def euler_forward(self) -> StateQuat: def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """Converts Euler angles to a quaternion - """ + """Converts Euler angles to a quaternion""" psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) s_psi = np.sin(psi / 2) @@ -292,8 +288,7 @@ def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles - """ + """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)) @@ -304,8 +299,7 @@ def quat_to_euler(quat: np.ndarray) -> np.ndarray: def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion - """ + """Function that normalizes a quaternion""" quat = quat / np.linalg.norm(quat) return quat @@ -354,8 +348,7 @@ def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions - """ + """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) @@ -441,8 +434,7 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points) mean_value = MeasModel() @@ -455,8 +447,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points - """ + """Function that calculates the covariance of a set of points""" n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -486,8 +477,7 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points - """ + """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)) @@ -510,8 +500,7 @@ def cross_covariance( set_z: list[MeasModel], mean_z: np.ndarray, ) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction - """ + """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))) diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index da56a7dfc..cb92d9393 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -1,4 +1,3 @@ - import numpy as np from ukf_okid_class import StateQuat @@ -6,8 +5,7 @@ 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. - """ + """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}:") @@ -15,8 +13,7 @@ def print_StateQuat_list( def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): - """Custom print function to print StateQuat objects in a formatted form. - """ + """Custom print function to print StateQuat objects in a formatted form.""" print(f"{name}:") print(f" Position: {state.position}") print(f" Orientation: {state.orientation}") @@ -28,8 +25,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): def print_matrix(matrix, name="Matrix"): - """Custom print function to print matrices in a formatted form. - """ + """Custom print function to print matrices in a formatted form.""" print(f"{name}: {matrix.shape}") if isinstance(matrix, np.ndarray): for row in matrix: From ea29da3c0e4418cdb6484ad071bfc31c0eb31ddb Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sat, 5 Apr 2025 18:27:32 +0200 Subject: [PATCH 14/19] fix: added errorstate and nominalstate variables into the eskf class --- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf.hpp | 56 +++--- navigation/eskf/include/eskf/eskf_ros.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 13 +- navigation/eskf/src/eskf.cpp | 202 ++++++++++------------ navigation/eskf/src/eskf_node.cpp | 2 +- navigation/eskf/src/eskf_ros.cpp | 40 ++--- navigation/eskf/src/eskf_utils.cpp | 5 +- 8 files changed, 149 insertions(+), 174 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index f89b62f79..87d50ee44 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,4 +3,5 @@ eskf_node: imu_topic: imu/data_raw dvl_twist: /orca/twist odom_topic: odom - diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.000003, 0.000003, 0.000003] + diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + diag_p_init: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index b30dc35b0..5cc3e0708 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -10,52 +10,41 @@ 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 state_quat& nom_state, - const state_euler& error_state, 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 state_quat& nom_state, - const state_euler& error_state, const dvl_measurement& dvl_meas); private: // @brief Predict the nominal state - // @param nom_state: Nominal state // @param imu_meas: IMU measurement + // @param dt: Time step // @return Predicted nominal state - state_quat nominal_state_discrete(const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt); + void nominal_state_discrete(const imu_measurement& imu_meas, + const double dt); // @brief Predict the error state - // @param error_state: Error state - // @param nom_state: Nominal state // @param imu_meas: IMU measurement + // @param dt: Time step // @return Predicted error state - state_euler error_state_prediction(const state_euler& error_state, - const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt); + void error_state_prediction(const imu_measurement& imu_meas, + const double dt); // @brief Update the error state - // @param error_state: Error state // @param dvl_meas: DVL measurement - // @return Updated error state - state_euler measurement_update(const state_quat& nom_state, - const state_euler& error_state, - const dvl_measurement& dvl_meas); + void measurement_update(const dvl_measurement& dvl_meas); // @brief Inject the error state into the nominal state and reset the error - // state - // @param nom_state: Nominal state - // @param error_state: Error state - // @return Injected and reset state - std::pair injection_and_reset( - const state_quat& nom_state, - const state_euler& error_state); + void injection_and_reset(); // @brief Van Loan discretization // @param A_c: Continuous state transition matrix @@ -69,24 +58,31 @@ class ESKF { // @brief Calculate the delta quaternion matrix // @param nom_state: Nominal state // @return Delta quaternion matrix - Eigen::Matrix4x3d calculate_Q_delta(const state_quat& nom_state); + Eigen::Matrix4x3d calculate_q_delta(); // @brief Calculate the measurement matrix jakobian // @param nom_state: Nominal state // @return Measurement matrix - Eigen::Matrix3x19d calculate_Hx(const state_quat& nom_state); + Eigen::Matrix3x19d calculate_hx(); // @brief Calculate the full measurement matrix // @param nom_state: Nominal state // @return Measurement matrix - Eigen::Matrix3x18d calculate_H(const state_quat& nom_state); + Eigen::Matrix3x18d calculate_h_jacobian(); // @brief Calculate the measurement // @param nom_state: Nominal state // @return Measurement - Eigen::Vector3d calculate_h(const state_quat& nom_state); + 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 index 88f97459f..b5c0b1dab 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -13,7 +13,7 @@ #include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" -#include "typedefs.hpp" +#include "spdlog/spdlog.h" class ESKFNode : public rclcpp::Node { public: diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 925d8fd72..9be435753 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -27,6 +27,13 @@ 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(); @@ -51,8 +58,6 @@ struct state_quat { diff.accel_bias = accel_bias - other.accel_bias; return diff; } - - Eigen::Matrix3d get_R() const { return quat.toRotationMatrix(); } }; struct state_euler { @@ -91,8 +96,8 @@ struct imu_measurement { Eigen::Matrix3d R_nb; R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; - accel = R_nb * accel_uncorrected; - gyro = R_nb * gyro_uncorrected; + accel = (R_nb * accel_uncorrected); + gyro = (R_nb * gyro_uncorrected); } }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 58c532afa..06c9c44c8 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -30,27 +30,27 @@ std::pair ESKF::van_loan_discretization( return {A_d, GQG_d}; } -Eigen::Matrix4x3d ESKF::calculate_Q_delta(const state_quat& nom_state) { - Eigen::Matrix4x3d Q_delta_theta = Eigen::Matrix4x3d::Zero(); - double qw = nom_state.quat.w(); - double qx = nom_state.quat.x(); - double qy = nom_state.quat.y(); - double qz = nom_state.quat.z(); +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 << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; - Q_delta_theta *= 0.5; - return Q_delta_theta; + q_delta_theta *= 0.5; + return q_delta_theta; } -Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { +Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); - Eigen::Quaterniond q = nom_state.quat.normalized(); + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); Eigen::Matrix3d R_bn = q.toRotationMatrix(); - Eigen::Vector3d v_n = nom_state.vel; + Eigen::Vector3d v_n = current_nom_state_.vel; - Hx.block<3, 3>(0, 3) = R_bn.transpose(); + Hx.block<3, 3>(0, 3) = R_bn; Eigen::Matrix dR_dq; double qw = q.w(); @@ -58,80 +58,79 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { double qy = q.y(); double qz = q.z(); + Eigen::Vector3d epsilon(qx, qy, qz); + + Eigen::Vector3d e_1(1, 0, 0); + Eigen::Vector3d e_2(0, 1, 0); + Eigen::Vector3d e_3(0, 0, 1); + dR_dq.col(0) = - 2 * Eigen::Vector3d(qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), - -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), - qy * v_n.x() - qx * v_n.y() + qw * v_n.z()); + ((4 * qw * Eigen::Matrix3d::Identity()) + (2 * skew(epsilon))) * v_n; - dR_dq.col(1) = - 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + dR_dq.col(1) = 2 * + ((e_1 * epsilon.transpose()) + (epsilon * e_1.transpose()) + + (qw * skew(e_1))) * + v_n; - dR_dq.col(2) = - 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + dR_dq.col(2) = 2 * + ((e_2 * epsilon.transpose()) + (epsilon * e_2.transpose()) + + (qw * skew(e_2))) * + v_n; - dR_dq.col(3) = - 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + dR_dq.col(3) = 2 * + ((e_3 * epsilon.transpose()) + (epsilon * e_3.transpose()) + + (qw * skew(e_3))) * + v_n; Hx.block<3, 4>(0, 6) = dR_dq; return Hx; } -Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { - 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(nom_state); - X_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); +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(nom_state) * X_delta; + Eigen::Matrix3x18d H = calculate_hx() * x_delta; return H; } -Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { +Eigen::Matrix3x1d ESKF::calculate_h() { Eigen::Matrix3x1d h; Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); + current_nom_state_.quat.normalized().toRotationMatrix(); - h = R_bn * nom_state.vel; + h = R_bn * current_nom_state_.vel; return h; } -state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt) { +void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, + const double dt) { Eigen::Vector3d acc = - nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + - nom_state.gravity; - Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; - - state_quat next_nom_state; - - next_nom_state.pos = - nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; - next_nom_state.vel = nom_state.vel + dt * acc; - next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); - next_nom_state.quat.normalize(); - next_nom_state.gyro_bias = nom_state.gyro_bias; - next_nom_state.accel_bias = nom_state.accel_bias; - next_nom_state.gravity = nom_state.gravity; - - return next_nom_state; + 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; } -state_euler ESKF::error_state_prediction(const state_euler& error_state, - const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt) { - Eigen::Matrix3d R = nom_state.get_R(); - Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); - Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias); +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(); @@ -152,77 +151,60 @@ state_euler ESKF::error_state_prediction(const state_euler& error_state, auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; - next_error_state.covariance = - A_d * error_state.covariance * A_d.transpose() + GQG_d; - - return next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } -state_euler ESKF::measurement_update(const state_quat& nom_state, - const state_euler& error_state, - const dvl_measurement& dvl_meas) { - state_euler new_error_state; - - Eigen::Matrix3x18d H = calculate_H(nom_state); - Eigen::Matrix18d P = error_state.covariance; +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(nom_state); - new_error_state.set_from_vector(K * innovation); + Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + current_error_state_.set_from_vector(K * innovation); Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; - new_error_state.covariance = + current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations - - return new_error_state; } -std::pair ESKF::injection_and_reset( - const state_quat& nom_state, - const state_euler& error_state) { - state_quat next_nom_state; - - next_nom_state.pos = nom_state.pos + error_state.pos; - next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = - nom_state.quat * vector3d_to_quaternion(error_state.euler); - next_nom_state.quat.normalize(); - next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; - next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; - next_nom_state.gravity = nom_state.gravity + error_state.gravity; - - state_euler new_error_state; +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(); - new_error_state.covariance = G * error_state.covariance * G.transpose(); - - return {next_nom_state, new_error_state}; + 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 state_quat& nom_state, - const state_euler& error_state, const imu_measurement& imu_meas, const double dt) { - state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = - error_state_prediction(error_state, next_nom_state, imu_meas, dt); + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); - return {next_nom_state, next_error_state}; + return {current_nom_state_, current_error_state_}; } std::pair ESKF::dvl_update( - const state_quat& nom_state, - const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = - measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = - injection_and_reset(nom_state, new_error_state); + measurement_update(dvl_meas); + injection_and_reset(); - return {updated_nom_state, updated_error_state}; + return {current_nom_state_, current_error_state_}; } diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp index e90cebde3..196fa7916 100644 --- a/navigation/eskf/src/eskf_node.cpp +++ b/navigation/eskf/src/eskf_node.cpp @@ -2,7 +2,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started ESKF Node"); + 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 index d679d600e..06bb047b9 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -20,20 +20,20 @@ void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - this->declare_parameter("imu_topic", "imu/data_raw"); + 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", "/orca/twist"); + 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", "odom"); + 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); @@ -41,34 +41,24 @@ void ESKFNode::set_subscribers_and_publisher() { void ESKFNode::set_parameters() { std::vector diag_Q_std; - this->declare_parameter>( - "diag_Q_std"); // gyroscope bias noise + this->declare_parameter>("diag_Q_std"); diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); Eigen::Matrix12d Q; Q.setZero(); spdlog::info("Q diagonal: {}", diag_Q_std[0]); - Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), - sq(diag_Q_std[2]), // acceleration noise - sq(diag_Q_std[3]), sq(diag_Q_std[4]), - sq(diag_Q_std[5]), // gyroscope noise - sq(diag_Q_std[6]), sq(diag_Q_std[7]), - sq(diag_Q_std[8]), // acceleration bias noise - sq(diag_Q_std[9]), sq(diag_Q_std[10]), - sq(diag_Q_std[11]); // gyroscope bias noise + 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_); - Eigen::Matrix18d P; - P.setZero(); - P.diagonal() << 1.0, 1.0, 1.0, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.001, 0.001, 0.001, // accel bias - 0.001, 0.001, 0.001, // gyro bias - 0.001, 0.001, 0.001; // gravity + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + Eigen::Matrix18d P = createDiagonalMatrix<18>(diag_p_init); error_state_.covariance = P; } @@ -91,8 +81,7 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { msg->angular_velocity.y, msg->angular_velocity.z; imu_meas_.correct(); - std::tie(nom_state_, error_state_) = - eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } void ESKFNode::dvl_callback( @@ -105,8 +94,7 @@ void ESKFNode::dvl_callback( msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - std::tie(nom_state_, error_state_) = - eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); } void ESKFNode::publish_odom() { @@ -125,6 +113,6 @@ void ESKFNode::publish_odom() { 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(); // Add timestamp to the message + 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 index 930167eaa..133589d05 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -18,7 +18,9 @@ Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } else { Eigen::Vector3d axis = vector / angle; - return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + Eigen::Quaterniond quat = + Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + return quat; } } @@ -27,5 +29,6 @@ Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { 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; } From f0079088ace3225432cc897cca063ae2daf340a2 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 17 Apr 2025 18:27:14 +0200 Subject: [PATCH 15/19] feat: modified imu correction and added in tested imu noise --- navigation/eskf/config/eskf_params.yaml | 7 +- navigation/eskf/include/eskf/eskf.hpp | 1 + navigation/eskf/include/eskf/eskf_ros.hpp | 2 + navigation/eskf/include/eskf/typedefs.hpp | 16 +- navigation/eskf/src/eskf.cpp | 21 +- navigation/eskf/src/eskf_ros.cpp | 21 +- navigation/eskf/src/eskf_utils.cpp | 2 +- navigation/ukf_okid/CMakeLists.txt | 23 + navigation/ukf_okid/launch/ukf.launch.py | 16 + navigation/ukf_okid/package.xml | 22 + .../ukf_python/{__ini__.py => __init__.py} | 0 navigation/ukf_okid/ukf_python/rest.py | 40 - navigation/ukf_okid/ukf_python/ukf_okid.py | 100 +-- .../ukf_okid/ukf_python/ukf_okid_class.py | 216 +++++- navigation/ukf_okid/ukf_python/ukf_ros.py | 117 +++ navigation/ukf_okid/ukf_python/ukf_test.py | 721 ++++++++++-------- navigation/ukf_okid/ukf_python/ukf_test_2.py | 40 + navigation/ukf_okid/ukf_python/ukf_utils.py | 1 + 18 files changed, 926 insertions(+), 440 deletions(-) create mode 100644 navigation/ukf_okid/CMakeLists.txt create mode 100644 navigation/ukf_okid/launch/ukf.launch.py create mode 100644 navigation/ukf_okid/package.xml rename navigation/ukf_okid/ukf_python/{__ini__.py => __init__.py} (100%) delete mode 100644 navigation/ukf_okid/ukf_python/rest.py create mode 100755 navigation/ukf_okid/ukf_python/ukf_ros.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_test_2.py diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 87d50ee44..639af1fc9 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,7 +1,8 @@ eskf_node: ros__parameters: imu_topic: imu/data_raw - dvl_twist: /orca/twist + dvl_topic: /orca/twist odom_topic: odom - diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] - diag_p_init: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + 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, -1, 0, -1, 0, -1, 0, 0 ] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 5cc3e0708..80b086d26 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -25,6 +25,7 @@ class ESKF { const dvl_measurement& dvl_meas); private: + // @brief Predict the nominal state // @param imu_meas: IMU measurement // @param dt: Time step diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index b5c0b1dab..e0b777c86 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -64,6 +64,8 @@ class ESKFNode : public rclcpp::Node { 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/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 9be435753..fac89baa7 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -7,6 +7,7 @@ #include #include +#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -40,7 +41,11 @@ struct state_quat { Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + + state_quat() { + gravity << 0, 0, 9.81; + } Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; @@ -89,16 +94,7 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); - Eigen::Vector3d accel_uncorrected = Eigen::Vector3d::Zero(); - Eigen::Vector3d gyro_uncorrected = Eigen::Vector3d::Zero(); - void correct() { - Eigen::Matrix3d R_nb; - R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; - - accel = (R_nb * accel_uncorrected); - gyro = (R_nb * gyro_uncorrected); - } }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 06c9c44c8..bbd367c7f 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -107,20 +107,24 @@ Eigen::Matrix3x1d ESKF::calculate_h() { 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_.quat.normalized().toRotationMatrix() * imu_meas.accel + current_nom_state_.gravity; - Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + Eigen::Vector3d gyro = imu_meas.gyro * dt/2; 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 = (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; @@ -129,8 +133,8 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, 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::Vector3d acc = imu_meas.accel; + Eigen::Vector3d gyro = imu_meas.gyro; Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -148,7 +152,8 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); - auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); + 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 = diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 06bb047b9..a35273103 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -40,6 +40,11 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { + std::vector R_imu_correction; + this->declare_parameter>("imu_frame"); + R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_eskf_ = Eigen::Map>(R_imu_correction.data()); + std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -75,11 +80,17 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel_uncorrected << msg->linear_acceleration.x, - msg->linear_acceleration.y, msg->linear_acceleration.z; - imu_meas_.gyro_uncorrected << msg->angular_velocity.x, - msg->angular_velocity.y, msg->angular_velocity.z; - imu_meas_.correct(); + 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); } diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 133589d05..a07f3acda 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -20,7 +20,7 @@ Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { Eigen::Vector3d axis = vector / angle; Eigen::Quaterniond quat = Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); - return quat; + return quat.normalized(); } } 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..62a439b94 --- /dev/null +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -0,0 +1,16 @@ +import os + +from ament_index_python.packages import get_package_share_directory +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/__ini__.py b/navigation/ukf_okid/ukf_python/__init__.py similarity index 100% rename from navigation/ukf_okid/ukf_python/__ini__.py rename to navigation/ukf_okid/ukf_python/__init__.py diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py deleted file mode 100644 index c8b4f3b14..000000000 --- a/navigation/ukf_okid/ukf_python/rest.py +++ /dev/null @@ -1,40 +0,0 @@ -def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """Function that calculates the mean of a set of points""" - n = len(set_points[0].as_vector()) - 1 - mean_value = StateQuat() - - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1 / (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat( - set_points, weight_temp_list - ) - - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat( - set_points, weights - ) - - return mean_value.as_vector() - - -def mean_measurement( - set_points: list[MeasModel], weights: np.ndarray = None -) -> np.ndarray: - """Function that calculates the mean of a set of points""" - n = len(set_points) - mean_value = MeasModel() - - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 50c68e08c..af61d1a79 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,69 +1,77 @@ import numpy as np -from ukf_okid_class import * +from ukf_utils import print_StateQuat +from ukf_okid_class import ( + MeasModel, + StateQuat, + covariance_measurement, + covariance_set, + cross_covariance, + mean_measurement, + mean_set, + process_model, + okid_process_model, +) class UKF: - def __init__(self, process_model: process_model, x_0, P_0, Q, R): + 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.R = R + 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.T = self.generate_T_matrix(len(P_0)) + self.delta = self.generate_delta_matrix(len(x_0.as_vector()) - 1) + self.cross_correlation = None - def generate_T_matrix(self, n: float) -> np.ndarray: - """Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + 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: - T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ - T = np.zeros((n, n)) + delta = np.zeros((n, 2 * n)) + k = 0.01 #Tuning parameter to ensure pos def - for i in range(n): + for i in range(2 * n): for j in range(n // 2): - T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) - T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) - - if n % 2 == 1: # if n is odd - T[n - 1, i - 1] = (-1) ** i - - T = T / np.sqrt(2) - - return T + 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""" + """Functions that generate the sigma points for the UKF.""" n = len(current_state.covariance) - I = np.hstack([np.eye(n), -np.eye(n)]) - my = np.sqrt(n) * I - delta = self.T @ my - 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 @ delta[:, index] - state.fill_states_different_dim(current_state.as_vector(), delta_x) + 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) + """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 in range(2 * n): - self.process_model.model_prediction(self.sigma_points_list[i]) + 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() @@ -75,42 +83,36 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: def measurement_update( self, current_state: StateQuat, measurement: MeasModel - ) -> tuple[MeasModel, np.ndarray]: - """Function that updates the state estimate with a measurement + ) -> 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 in range(2 * n): - z_i[i] = measurement.H(self.sigma_points_list[i]) + for i, state in enumerate(self.sigma_points_list): + z_i[i] = measurement.H(state) - meas_update = MeasModel() + self.measurement_updated.measurement = mean_measurement(z_i) - meas_update.measurement = mean_measurement(z_i) + self.measurement_updated.covariance = covariance_measurement(z_i, self.measurement_updated.measurement) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - - cross_correlation = cross_covariance( - self.y_i, current_state.as_vector(), z_i, meas_update.measurement + self.cross_correlation = cross_covariance( + self.y_i, current_state.as_vector(), z_i, self.measurement_updated.measurement ) - return meas_update, cross_correlation - def posteriori_estimate( self, current_state: StateQuat, - cross_correlation: np.ndarray, measurement: MeasModel, - ex_measuremnt: MeasModel, ) -> StateQuat: - """Calculates the posteriori estimate using measurement and the prior estimate""" + """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 - nu_k.measurement = measurement.measurement - ex_measuremnt.measurement - nu_k.covariance = ex_measuremnt.covariance + measurement.covariance - - K_k = np.dot(cross_correlation, np.linalg.inv(nu_k.covariance)) + K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) posteriori_estimate = StateQuat() @@ -121,6 +123,4 @@ def posteriori_estimate( K_k, np.dot(nu_k.covariance, np.transpose(K_k)) ) - self.process_model.state_vector_prev = posteriori_estimate - 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 index 45bbffcfe..5558ad754 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -2,6 +2,54 @@ 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: @@ -11,10 +59,16 @@ class StateQuat: 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)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + 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] ) @@ -53,6 +107,16 @@ def fill_states(self, state: np.ndarray) -> None: 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 @@ -64,6 +128,7 @@ def fill_states_different_dim( ) 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.""" @@ -72,6 +137,7 @@ def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: 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 @@ -84,6 +150,7 @@ def __add__(self, other: 'StateQuat') -> 'StateQuat': ) 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 @@ -94,6 +161,7 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': 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() @@ -104,6 +172,7 @@ def __rmul__(self, scalar: float) -> 'StateQuat': 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 @@ -116,6 +185,7 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: ) 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() @@ -124,6 +194,7 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.position += other.position self.velocity += other.velocity self.angular_velocity += other.angular_velocity + self.okid_params += other.okid_params @dataclass @@ -136,9 +207,9 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" H = np.zeros((3, 13)) - H[0:3, 7:10] = np.eye(3) + H[:, 7:10] = np.eye(3) z_i = MeasModel() - z_i.measurement = np.dot(H, state.as_vector()) + z_i.measurement = np.dot(H, state.dynamic_part()) return z_i def __add__(self, other: 'MeasModel') -> 'MeasModel': @@ -263,10 +334,112 @@ def euler_forward(self) -> StateQuat: + 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""" + """Converts Euler angles to a quaternion.""" psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) s_psi = np.sin(psi / 2) @@ -288,7 +461,7 @@ def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles""" + """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)) @@ -299,7 +472,7 @@ def quat_to_euler(quat: np.ndarray) -> np.ndarray: def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion""" + """Function that normalizes a quaternion.""" quat = quat / np.linalg.norm(quat) return quat @@ -348,7 +521,7 @@ def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions""" + """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) @@ -359,17 +532,15 @@ def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: def iterative_quaternion_mean_statequat( state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 ) -> np.ndarray: - """Computes the weighted mean of the quaternion orientations from a list of StateQuat objects - using an iterative approach, without requiring the caller to manually extract the quaternion. + """Computes the iterative mean of quaternion orientations from StateQuat objects. - Parameters: - state_list (list[StateQuat]): List of StateQuat objects. - weights (np.ndarray): Weights for each state. - tol (float): Convergence tolerance. - max_iter (int): Maximum number of iterations. + Args: + state_list: List of StateQuat objects + tol: Convergence tolerance + max_iter: Maximum iterations Returns: - np.ndarray: The averaged quaternion as a 4-element numpy array. + Mean quaternion as numpy array """ sigma_quats = [state.orientation for state in state_list] n = len(state_list) @@ -412,7 +583,7 @@ def iterative_quaternion_mean_statequat( def mean_set(set_points: list[StateQuat]) -> np.ndarray: - """Function calculates the mean vector of a set of points + """Function calculates the mean vector of a set of points. Args: set_points (list[StateQuat]): List of StateQuat objects @@ -426,7 +597,10 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: for state in set_points: mean_value.add_without_quaternions(state) - mean_value = (1 / (n)) * mean_value + 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) @@ -434,7 +608,7 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points""" + """Function that calculates the mean of a set of points.""" n = len(set_points) mean_value = MeasModel() @@ -447,7 +621,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points""" + """Function that calculates the covariance of a set of points.""" n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -477,7 +651,7 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points""" + """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)) @@ -500,7 +674,7 @@ def cross_covariance( set_z: list[MeasModel], mean_z: np.ndarray, ) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction""" + """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))) 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..66d01175c --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_ros.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +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 std_msgs.msg import Bool, String +from ukf_okid import UKF +from ukf_okid_class import StateQuat, process_model, MeasModel +import numpy as np + +class UKFNode(Node): + def __init__(self): + super().__init__("UKFNode") + + best_effort_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + + #subcribers + 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() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index cebb53ac6..80ca90e86 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,323 +1,440 @@ -import time - -import matplotlib.pyplot as plt import numpy as np -from ukf_okid import UKF +import matplotlib.pyplot as plt +from ukf_utils import print_StateQuat +# Import your classes and functions. +# Adjust the import paths as necessary based on your module organization. from ukf_okid_class import ( - MeasModel, StateQuat, - process_model, - quat_to_euler, + MeasModel, + iterative_quaternion_mean_statequat, + mean_set, + mean_measurement, + covariance_set, + covariance_measurement, + cross_covariance, quaternion_super_product, + quaternion_error, + quat_to_euler, + quat_norm, ) - -def add_quaternion_noise(q, noise_std): - noise = np.random.normal(0, noise_std, 3) - - theta = np.linalg.norm(noise) - - if theta > 0: - axis = noise / theta - - q_noise = np.hstack((np.cos(theta / 2), np.sin(theta / 2) * axis)) - +# 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: - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) - - q_new = quaternion_super_product(q, q_noise) - - return q_new / np.linalg.norm(q_new) - - -if __name__ == '__main__': - # Create initial state vector and covariance matrix. - x0 = np.zeros(13) - x0[0:3] = [0.3, 0.3, 0.3] - x0[3] = 1 - x0[7:10] = [0.2, 0.2, 0.2] - dt = 0.01 - R = (0.01) * np.eye(3) - - Q = 0.00015 * np.eye(12) - P0 = np.eye(12) * 0.0001 + 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) - model = process_model() - model.dt = 0.01 - 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], - ] - ) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - model_ukf = process_model() - model_ukf.dt = 0.01 - model_ukf.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], - ] - ) - model_ukf.m = 30.0 - model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) +import time +import numpy as np +import matplotlib.pyplot as plt - # Simulation parameters - simulation_time = 5 # seconds +# Import your classes and functions. +from ukf_okid_class import ( + StateQuat, + MeasModel, + iterative_quaternion_mean_statequat, + mean_set, + mean_measurement, + covariance_set, + covariance_measurement, + cross_covariance, + quaternion_super_product, + quaternion_error, + quat_norm, +) +from ukf_okid import UKF +from ukf_okid_class import process_model, okid_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) - - # Initialize a dummy StateQuat. - new_state = StateQuat() - new_state.fill_states(x0) - new_state.covariance = P0 - - test_state_x = StateQuat() - test_state_x.fill_states(x0) - test_state_x.covariance = P0 - - # Initialize a estimated state - estimated_state = StateQuat() - estimated_state.fill_states(x0) - estimated_state.covariance = P0 - - # Initialize a estimated state - noisy_state = StateQuat() - noisy_state.fill_states(x0) - noisy_state.covariance = P0 - - measurment_model = MeasModel() - measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R - - # Initialize arrays to store the results - positions = np.zeros((num_steps, 3)) - orientations = np.zeros((num_steps, 3)) - velocities = np.zeros((num_steps, 3)) - angular_velocities = np.zeros((num_steps, 3)) - - # Initialize arrays to store the estimates - positions_est = np.zeros((num_steps, 3)) - orientations_est = np.zeros((num_steps, 3)) - velocities_est = np.zeros((num_steps, 3)) - angular_velocities_est = np.zeros((num_steps, 3)) - - # Initialize the okid params - okid_params = np.zeros((num_steps, 21)) - - model.state_vector_prev = new_state - model.state_vector = new_state - - model_ukf.state_vector_prev = test_state_x - model_ukf.state_vector = test_state_x - - # initialize the ukf - ukf = UKF(model_ukf, x0, P0, Q, R) - - elapsed_times = [] - - u = lambda t: np.array( - [ - 2 * np.sin(1 * t), - 2 * np.sin(1 * t), - 2 * np.sin(1 * t), - 0.2 * np.cos(1 * t), - 0.2 * np.cos(1 * t), - 0.2 * np.cos(1 * t), - ] - ) - - # Run the simulation - for step in range(num_steps): - # Insert control input - model.Control_input = u(step * dt) - model_ukf.Control_input = u(step * dt) - - # Perform the unscented transform - model.model_prediction(new_state) - new_state = model.euler_forward() - - # Adding noise in the state vector - estimated_state.position = ( - estimated_state.position - ) # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = ( - estimated_state.orientation - ) # add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = ( - estimated_state.velocity - ) # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = ( - estimated_state.angular_velocity - ) # + np.random.normal(0, 0.01, 3) - - start_time = time.time() - estimated_state = ukf.unscented_transform(estimated_state) - print(estimated_state.as_vector()) - break - elapsed_time = time.time() - start_time - elapsed_times.append(elapsed_time) - - if step % 20 == 0: - measurment_model.measurement = ( - new_state.velocity - ) # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update( - estimated_state, measurment_model - ) - estimated_state = ukf.posteriori_estimate( - estimated_state, covariance_matrix, measurment_model, meas_update - ) - - positions[step, :] = new_state.position - orientations[step, :] = quat_to_euler(new_state.orientation) - velocities[step, :] = new_state.velocity - angular_velocities[step, :] = new_state.angular_velocity - - positions_est[step, :] = estimated_state.position - orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - velocities_est[step, :] = estimated_state.velocity - angular_velocities_est[step, :] = estimated_state.angular_velocity - - # Update the state for the next iteration - model.state_vector_prev = new_state - - print('Average elapsed time: ', np.mean(elapsed_times)) - print('Max elapsed time: ', np.max(elapsed_times)) - print('Min elapsed time: ', np.min(elapsed_times)) - print('median elapsed time: ', np.median(elapsed_times)) - # Plot the results - time = np.linspace(0, simulation_time, num_steps) - - # Plot positions - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, positions[:, 0], label='True') - plt.plot(time, positions_est[:, 0], label='Estimated') - plt.title('Position X') - plt.xlabel('Time [s]') - plt.ylabel('Position X [m]') + + # 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.subplot(3, 1, 2) - plt.plot(time, positions[:, 1], label='True') - plt.plot(time, positions_est[:, 1], label='Estimated') - plt.title('Position Y') - plt.xlabel('Time [s]') - plt.ylabel('Position Y [m]') + 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.subplot(3, 1, 3) - plt.plot(time, positions[:, 2], label='True') - plt.plot(time, positions_est[:, 2], label='Estimated') - plt.title('Position Z') - plt.xlabel('Time [s]') - plt.ylabel('Position Z [m]') + 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 orientations (Euler angles) - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, orientations[:, 0], label='True') - plt.plot(time, orientations_est[:, 0], label='Estimated') - plt.title('Orientation Roll') - plt.xlabel('Time [s]') - plt.ylabel('Roll [rad]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, orientations[:, 1], label='True') - plt.plot(time, orientations_est[:, 1], label='Estimated') - plt.title('Orientation Pitch') - plt.xlabel('Time [s]') - plt.ylabel('Pitch [rad]') + + # 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.subplot(3, 1, 3) - plt.plot(time, orientations[:, 2], label='True') - plt.plot(time, orientations_est[:, 2], label='Estimated') - plt.title('Orientation Yaw') - plt.xlabel('Time [s]') - plt.ylabel('Yaw [rad]') - plt.legend() - - plt.tight_layout() + plt.title("Error Metrics over Time") + plt.xlabel("Time (s)") + plt.ylabel("Error magnitude") plt.show() - # Plot velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, velocities[:, 0], label='True') - plt.plot(time, velocities_est[:, 0], label='Estimated') - plt.title('Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Velocity X [m/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, velocities[:, 1], label='True') - plt.plot(time, velocities_est[:, 1], label='Estimated') - plt.title('Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Y [m/s]') - plt.legend() +# 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() - plt.subplot(3, 1, 3) - plt.plot(time, velocities[:, 2], label='True') - plt.plot(time, velocities_est[:, 2], label='Estimated') - plt.title('Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Z [m/s]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot angular velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, angular_velocities[:, 0], label='True') - plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - plt.title('Angular Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity X [rad/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, angular_velocities[:, 1], label='True') - plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - plt.title('Angular Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Y [rad/s]') - plt.legend() +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() - plt.subplot(3, 1, 3) - plt.plot(time, angular_velocities[:, 2], label='True') - plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - plt.title('Angular Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Z [rad/s]') - plt.legend() - plt.tight_layout() - plt.show() 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..bf58b7a1a --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -0,0 +1,40 @@ +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) \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index cb92d9393..7bf7cd4e3 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -19,6 +19,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): 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") From 20280c2d22337cd780bfc840253e0c4b8f1a9b1a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 17 Apr 2025 16:27:59 +0000 Subject: [PATCH 16/19] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/eskf.hpp | 1 - navigation/eskf/include/eskf/typedefs.hpp | 7 +- navigation/eskf/src/eskf.cpp | 11 +- navigation/eskf/src/eskf_ros.cpp | 18 +- navigation/ukf_okid/launch/ukf.launch.py | 3 - navigation/ukf_okid/ukf_python/ukf_okid.py | 29 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 43 ++- navigation/ukf_okid/ukf_python/ukf_ros.py | 102 +++++-- navigation/ukf_okid/ukf_python/ukf_test.py | 268 ++++++++++-------- navigation/ukf_okid/ukf_python/ukf_test_2.py | 22 +- 10 files changed, 293 insertions(+), 211 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 80b086d26..5cc3e0708 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -25,7 +25,6 @@ class ESKF { const dvl_measurement& dvl_meas); private: - // @brief Predict the nominal state // @param imu_meas: IMU measurement // @param dt: Time step diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index fac89baa7..748804be3 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,9 +5,9 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H +#include #include #include -#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -43,9 +43,7 @@ struct state_quat { Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); - state_quat() { - gravity << 0, 0, 9.81; - } + state_quat() { gravity << 0, 0, 9.81; } Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; @@ -94,7 +92,6 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); - }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index bbd367c7f..84fb5e8f3 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -107,21 +107,20 @@ Eigen::Matrix3x1d ESKF::calculate_h() { 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_.quat.normalized().toRotationMatrix() * + imu_meas.accel + current_nom_state_.gravity; - Eigen::Vector3d gyro = imu_meas.gyro * dt/2; + Eigen::Vector3d gyro = imu_meas.gyro * dt / 2; 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 = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a35273103..e805a0c39 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -42,8 +42,9 @@ void ESKFNode::set_subscribers_and_publisher() { void ESKFNode::set_parameters() { std::vector R_imu_correction; this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); - R_imu_eskf_ = Eigen::Map>(R_imu_correction.data()); + R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -81,15 +82,14 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { last_imu_time_ = current_time; Eigen::Vector3d raw_accel(msg->linear_acceleration.x, - msg->linear_acceleration.y, - msg->linear_acceleration.z); + 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); - + + 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); diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py index 62a439b94..baf6fb645 100644 --- a/navigation/ukf_okid/launch/ukf.launch.py +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -1,12 +1,9 @@ -import os -from ament_index_python.packages import get_package_share_directory 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", diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index af61d1a79..474b94ac6 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,5 +1,4 @@ import numpy as np -from ukf_utils import print_StateQuat from ukf_okid_class import ( MeasModel, StateQuat, @@ -8,7 +7,6 @@ cross_covariance, mean_measurement, mean_set, - process_model, okid_process_model, ) @@ -37,15 +35,17 @@ def generate_delta_matrix(self, n: float) -> np.ndarray: 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 + k = 0.01 # Tuning parameter to ensure pos def - for i in range(2 * n): + 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) - + 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 + delta[n - 1, i] = (-1) ** i return delta def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: @@ -96,10 +96,15 @@ def measurement_update( self.measurement_updated.measurement = mean_measurement(z_i) - self.measurement_updated.covariance = covariance_measurement(z_i, self.measurement_updated.measurement) + 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 + self.y_i, + current_state.as_vector(), + z_i, + self.measurement_updated.measurement, ) def posteriori_estimate( @@ -109,7 +114,9 @@ def posteriori_estimate( ) -> 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.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)) diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 5558ad754..0b329cb84 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -2,13 +2,22 @@ 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])) + 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.""" @@ -27,6 +36,7 @@ def __add__(self, other: 'okid') -> 'okid': 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() @@ -34,22 +44,23 @@ def __sub__(self, other: 'okid') -> 'okid': 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.""" + """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 + return result + @dataclass class StateQuat: @@ -65,8 +76,15 @@ class StateQuat: 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()] + [ + 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( @@ -108,7 +126,7 @@ def fill_states(self, state: np.ndarray) -> None: 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] @@ -334,6 +352,8 @@ def euler_forward(self) -> StateQuat: + self.state_vector_dot.angular_velocity * self.dt ) return self.state_vector + + @dataclass class okid_process_model: state_vector: StateQuat = field(default_factory=StateQuat) @@ -396,8 +416,8 @@ def D(self) -> np.ndarray: """Calculates the damping matrix.""" D_l = -np.diag(self.damping_linear) - return D_l - + return D_l + def model_prediction(self, state: StateQuat) -> None: """Calculates the model of the system.""" self.state_vector = state @@ -438,6 +458,7 @@ def euler_forward(self) -> None: ) 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 diff --git a/navigation/ukf_okid/ukf_python/ukf_ros.py b/navigation/ukf_okid/ukf_python/ukf_ros.py index 66d01175c..a40c2f7c3 100755 --- a/navigation/ukf_okid/ukf_python/ukf_ros.py +++ b/navigation/ukf_okid/ukf_python/ukf_ros.py @@ -1,13 +1,13 @@ #!/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 std_msgs.msg import Bool, String from ukf_okid import UKF -from ukf_okid_class import StateQuat, process_model, MeasModel -import numpy as np +from ukf_okid_class import MeasModel, StateQuat, process_model + class UKFNode(Node): def __init__(self): @@ -19,16 +19,24 @@ def __init__(self): depth=1, ) - #subcribers - self.dvl_subscriber = self.create_subscription(TwistWithCovarianceStamped, + # 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.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) + 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) @@ -38,24 +46,23 @@ def __init__(self): 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.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]) + 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 @@ -65,18 +72,51 @@ def dvl_callback(self, msg: TwistWithCovarianceStamped): 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]]]) - + 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.current_state = self.ukf.posteriori_estimate( + self.current_state, dvl_measurement + ) self.ukf_flagg = True - - def control_callback(self, msg:WrenchStamped): + + 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]) + 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): @@ -90,7 +130,7 @@ def odom_publisher(self): 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] @@ -107,11 +147,13 @@ def odom_publisher(self): 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() \ No newline at end of file + main() diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 80ca90e86..d80427364 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,27 +1,31 @@ -import numpy as np import matplotlib.pyplot as plt -from ukf_utils import print_StateQuat +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 ( - StateQuat, MeasModel, - iterative_quaternion_mean_statequat, - mean_set, - mean_measurement, - covariance_set, + StateQuat, covariance_measurement, + covariance_set, cross_covariance, - quaternion_super_product, - quaternion_error, - quat_to_euler, + 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. +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. @@ -37,19 +41,22 @@ def create_statequat(base_vector, position_perturbation, orientation_perturbatio 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)) + 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: + + # 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) @@ -58,7 +65,7 @@ def test_state_statistics(): 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 = [] @@ -68,27 +75,32 @@ def test_state_statistics(): 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)) - + 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) @@ -100,17 +112,17 @@ def test_measurement_statistics(): 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) @@ -118,6 +130,7 @@ def test_measurement_statistics(): print("Eigenvalues of measurement covariance:") print(eigvals) + def test_cross_covariance(): # Create a set of StateQuat and corresponding MeasModel objects. np.random.seed(99) @@ -129,58 +142,50 @@ def test_cross_covariance(): 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 = 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 -import numpy as np -import matplotlib.pyplot as plt + +from ukf_okid import UKF # Import your classes and functions. from ukf_okid_class import ( - StateQuat, - MeasModel, - iterative_quaternion_mean_statequat, - mean_set, - mean_measurement, - covariance_set, - covariance_measurement, - cross_covariance, - quaternion_super_product, - quaternion_error, - quat_norm, -) -from ukf_okid import UKF -from ukf_okid_class import process_model, okid_process_model # Your process model classes + 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: + """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, @@ -194,49 +199,60 @@ def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise 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)) + 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])))) + 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 + 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[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[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 + 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 - + 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) @@ -250,19 +266,21 @@ def run_ukf_simulation(): # 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.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]) + 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() @@ -275,18 +293,14 @@ def run_ukf_simulation(): 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 = 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 = 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) @@ -307,14 +321,16 @@ def run_ukf_simulation(): # 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) - ]) + 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 @@ -325,19 +341,19 @@ def control_input(t): # Lists for timing diagnostics. ukf_transform_times = [] ukf_update_times = [] - + # Simulation loop. for i in range(num_steps): - t_current = i*dt - + 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) @@ -347,7 +363,7 @@ def control_input(t): 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: @@ -357,8 +373,10 @@ def control_input(t): 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 - + 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 @@ -368,7 +386,7 @@ def control_input(t): # 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 @@ -376,7 +394,7 @@ def control_input(t): # 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) @@ -384,31 +402,31 @@ def control_input(t): 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.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.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.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.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") @@ -418,6 +436,7 @@ def control_input(t): 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:") @@ -429,12 +448,11 @@ def run_diagnostics(): 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 index bf58b7a1a..d3a75313b 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test_2.py +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -2,10 +2,10 @@ # 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 +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) @@ -14,10 +14,11 @@ 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. - + """Print a matrix with fancy formatting. + Args: matrix: numpy array to print name: name of the matrix to display @@ -26,15 +27,16 @@ def fancy_print_matrix(matrix, name="Matrix", precision=4): 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) \ No newline at end of file +fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) From 65e846764553db49185f2c13a3e3af664a063d53 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 8 May 2025 20:19:53 +0200 Subject: [PATCH 17/19] feat: added NIS plots --- navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/include/eskf/eskf.hpp | 8 ++++++++ navigation/eskf/include/eskf/eskf_ros.hpp | 3 +++ navigation/eskf/src/eskf.cpp | 6 ++++++ navigation/eskf/src/eskf_ros.cpp | 9 +++++++-- 5 files changed, 25 insertions(+), 3 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 639af1fc9..961575447 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,4 +5,4 @@ eskf_node: odom_topic: odom diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] 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, -1, 0, -1, 0, -1, 0, 0 ] + 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 index 5cc3e0708..214f462c4 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -24,6 +24,9 @@ class ESKF { std::pair dvl_update( const dvl_measurement& dvl_meas); + // NIS + double NIS_; + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -39,6 +42,11 @@ class ESKF { void error_state_prediction(const imu_measurement& imu_meas, const double dt); + // @brief Calculate the NIS + // @param innovation: Innovation vector + // @param S: Innovation covariance matrix + void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + // @brief Update the error state // @param dvl_meas: DVL measurement void measurement_update(const dvl_measurement& dvl_meas); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index e0b777c86..44743fd75 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr nis_pub_; + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 84fb5e8f3..c02bf57df 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -159,6 +159,11 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, 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::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3x18d H = calculate_h_jacobian(); Eigen::Matrix18d P = current_error_state_.covariance; @@ -167,6 +172,7 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { 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; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index e805a0c39..9d46969d9 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -37,12 +37,14 @@ void ESKFNode::set_subscribers_and_publisher() { std::string odom_topic = this->get_parameter("odom_topic").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); + + nis_pub_ = create_publisher("dvl/nis", 10); } void ESKFNode::set_parameters() { std::vector R_imu_correction; this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_correction = get_parameter("imu_frame").as_double_array(); R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); @@ -53,7 +55,6 @@ void ESKFNode::set_parameters() { Eigen::Matrix12d Q; Q.setZero(); - spdlog::info("Q diagonal: {}", diag_Q_std[0]); 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]), @@ -106,6 +107,10 @@ void ESKFNode::dvl_callback( msg->twist.covariance[14]; 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); } void ESKFNode::publish_odom() { From dd0f97a9d6f9ecf70b88a702180871bff1759f19 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Wed, 14 May 2025 00:00:00 +0200 Subject: [PATCH 18/19] fix: issues in innovation and jacobian of the measurement --- navigation/eskf/CMakeLists.txt | 2 + navigation/eskf/config/eskf_params.yaml | 6 +- navigation/eskf/include/eskf/eskf.hpp | 8 ++ navigation/eskf/include/eskf/eskf_ros.hpp | 21 +++- navigation/eskf/include/eskf/eskf_utils.hpp | 3 + navigation/eskf/include/eskf/typedefs.hpp | 18 +++- navigation/eskf/src/eskf.cpp | 103 ++++++++++++-------- navigation/eskf/src/eskf_ros.cpp | 67 +++++++++++-- navigation/eskf/src/eskf_utils.cpp | 5 + 9 files changed, 172 insertions(+), 61 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 6c8167609..c431c235f 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -18,6 +18,7 @@ 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}) @@ -42,6 +43,7 @@ ament_target_dependencies(eskf_node vortex_msgs spdlog fmt + stonefish_ros2 ) target_link_libraries(eskf_node diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 961575447..98be8e789 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,8 +1,8 @@ eskf_node: ros__parameters: imu_topic: imu/data_raw - dvl_topic: /orca/twist + dvl_topic: /dvl/sim odom_topic: odom - diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + 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] + 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 index 214f462c4..6d8d33bd5 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -27,6 +27,12 @@ class ESKF { // NIS double NIS_; + // NEES + double NEES_; + + // ground truth + state_quat ground_truth_; + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -47,6 +53,8 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + void NEES(); + // @brief Update the error state // @param dvl_meas: DVL measurement void measurement_update(const dvl_measurement& dvl_meas); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 44743fd75..8cb32b173 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -7,11 +7,11 @@ #include #include #include +#include #include #include #include #include -#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" @@ -21,14 +21,18 @@ class ESKFNode : public rclcpp::Node { 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 geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -41,19 +45,26 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + rclcpp::Subscription::SharedPtr dvl_sub_; + + rclcpp::Subscription::SharedPtr pose_sub_; + + rclcpp::Subscription::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_; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 100f7673d..0cc2f6887 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -3,11 +3,14 @@ #include "eigen3/Eigen/Dense" #include "eskf/typedefs.hpp" +#include 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); diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 748804be3..ab86b0a6c 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -48,7 +48,22 @@ struct state_quat { Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, - accel_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; } @@ -59,6 +74,7 @@ struct state_quat { 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; } }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index c02bf57df..9325378bb 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -5,6 +5,7 @@ #include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" +#include "iostream" ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} @@ -50,37 +51,38 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Vector3d v_n = current_nom_state_.vel; - Hx.block<3, 3>(0, 3) = R_bn; + Hx.block<3, 3>(0, 3) = R_bn.transpose(); - Eigen::Matrix dR_dq; double qw = q.w(); double qx = q.x(); double qy = q.y(); double qz = q.z(); - Eigen::Vector3d epsilon(qx, qy, qz); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d e_1(1, 0, 0); - Eigen::Vector3d e_2(0, 1, 0); - Eigen::Vector3d e_3(0, 0, 1); + Eigen::Vector3d eps(qx, qy, qz); - dR_dq.col(0) = - ((4 * qw * Eigen::Matrix3d::Identity()) + (2 * skew(epsilon))) * v_n; + Eigen::Matrix3d dR_deta = 2*qw * I3 - 2*skew(eps); - dR_dq.col(1) = 2 * - ((e_1 * epsilon.transpose()) + (epsilon * e_1.transpose()) + - (qw * skew(e_1))) * - v_n; + Eigen::Vector3d e1_vec(1,0,0), e2_vec(0,1,0), e3_vec(0,0,1); - dR_dq.col(2) = 2 * - ((e_2 * epsilon.transpose()) + (epsilon * e_2.transpose()) + - (qw * skew(e_2))) * - v_n; + Eigen::Matrix3d dR_dqx = -2*qx*I3 + + 2*(e1_vec*eps.transpose() + eps*e1_vec.transpose()) + - 2*qw*skew(e1_vec); - dR_dq.col(3) = 2 * - ((e_3 * epsilon.transpose()) + (epsilon * e_3.transpose()) + - (qw * skew(e_3))) * - v_n; + 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; @@ -100,7 +102,7 @@ Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { Eigen::Matrix3x1d ESKF::calculate_h() { Eigen::Matrix3x1d h; Eigen::Matrix3d R_bn = - current_nom_state_.quat.normalized().toRotationMatrix(); + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; @@ -109,19 +111,13 @@ Eigen::Matrix3x1d ESKF::calculate_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_.gravity; - Eigen::Vector3d gyro = imu_meas.gyro * dt / 2; - - 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; + 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_.quat = - (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + 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; @@ -132,8 +128,8 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, 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; - Eigen::Vector3d gyro = imu_meas.gyro; + 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(); @@ -164,6 +160,30 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { NIS_ = innovation.transpose() * S_inv * innovation; } +void ESKF::NEES() { + + 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; @@ -179,21 +199,18 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations + + NEES(); } 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 = 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; + 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(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 9d46969d9..04b6dd9c6 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -20,6 +20,16 @@ void ESKFNode::set_subscribers_and_publisher() { 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( @@ -29,7 +39,7 @@ void ESKFNode::set_subscribers_and_publisher() { 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>( + stonefish_ros2::msg::DVL>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -39,6 +49,7 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); + nees_pub_ = create_publisher("dvl/nees", 10); } void ESKFNode::set_parameters() { @@ -70,6 +81,20 @@ void ESKFNode::set_parameters() { 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; @@ -97,20 +122,44 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { } void ESKFNode::dvl_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - dvl_meas_.cov << 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]; + 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() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index a07f3acda..88d04c3d5 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -11,6 +11,11 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { 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(); From 103a057f0b2a90894e6eac149ad6bc10ab9685e5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 May 2025 22:01:55 +0000 Subject: [PATCH 19/19] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/eskf.hpp | 4 +- navigation/eskf/include/eskf/eskf_ros.hpp | 17 ++-- navigation/eskf/include/eskf/eskf_utils.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 11 ++- navigation/eskf/src/eskf.cpp | 86 ++++++++++++--------- navigation/eskf/src/eskf_ros.cpp | 45 +++++------ navigation/ukf_okid/launch/ukf.launch.py | 1 - navigation/ukf_okid/ukf_python/ukf_okid.py | 4 +- 8 files changed, 92 insertions(+), 78 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 6d8d33bd5..d9b7d4fa0 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -27,7 +27,7 @@ class ESKF { // NIS double NIS_; - // NEES + // NEEDS double NEES_; // ground truth @@ -53,7 +53,7 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); - void NEES(); + void NEEDS(); // @brief Update the error state // @param dvl_meas: DVL measurement diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 8cb32b173..c3d09e820 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -7,11 +7,11 @@ #include #include #include -#include #include #include #include #include +#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" @@ -21,10 +21,11 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::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 @@ -47,9 +48,11 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr dvl_sub_; - rclcpp::Subscription::SharedPtr pose_sub_; - - rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr odom_pub_; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 0cc2f6887..4fcaed412 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -1,9 +1,9 @@ #ifndef ESKF_UTILS_HPP #define ESKF_UTILS_HPP +#include #include "eigen3/Eigen/Dense" #include "eskf/typedefs.hpp" -#include Eigen::Matrix3d skew(const Eigen::Vector3d& v); diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index ab86b0a6c..47dfe06e1 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -56,13 +56,12 @@ struct state_quat { Eigen::Vector18d vec; Eigen::Vector3d euler_diff; - euler_diff = (quat * other.quat.inverse()).toRotationMatrix().eulerAngles(0, 1, 2); + 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, + vec << pos - other.pos, vel - other.vel, euler_diff, + gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, gravity - other.gravity; return vec; } diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 9325378bb..e0e7c7a98 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -58,31 +58,34 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { double qy = q.y(); double qz = q.z(); - Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); Eigen::Vector3d eps(qx, qy, qz); - Eigen::Matrix3d dR_deta = 2*qw * I3 - 2*skew(eps); + 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::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_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_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::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; + 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; @@ -111,13 +114,18 @@ Eigen::Matrix3x1d ESKF::calculate_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 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_.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 = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; @@ -160,17 +168,17 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { NIS_ = innovation.transpose() * S_inv * innovation; } -void ESKF::NEES() { - +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::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); @@ -178,9 +186,10 @@ void ESKF::NEES() { singular_values_inv(i) = 0.0; } } - - Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose(); - + + Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * + svd.matrixU().transpose(); + NEES_ = error_state.transpose() * cov_inv * error_state; } @@ -199,18 +208,23 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations - - NEES(); + + 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 = + 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; + 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(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 04b6dd9c6..54daadb4c 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -24,7 +24,7 @@ void ESKFNode::set_subscribers_and_publisher() { 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, @@ -38,8 +38,7 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription< - stonefish_ros2::msg::DVL>( + dvl_sub_ = this->create_subscription( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -49,7 +48,7 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); - nees_pub_ = create_publisher("dvl/nees", 10); + nees_pub_ = create_publisher("dvl/needs", 10); } void ESKFNode::set_parameters() { @@ -81,18 +80,20 @@ void ESKFNode::set_parameters() { 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; +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::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) { @@ -121,30 +122,30 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { 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; +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]; - + + // 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; diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py index baf6fb645..5d075259f 100644 --- a/navigation/ukf_okid/launch/ukf.launch.py +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -1,4 +1,3 @@ - from launch import LaunchDescription from launch_ros.actions import Node diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 474b94ac6..e50c65c3a 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -39,9 +39,7 @@ def generate_delta_matrix(self, n: float) -> np.ndarray: 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 + 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: