diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py new file mode 100644 index 0000000..3edff03 --- /dev/null +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py @@ -0,0 +1,34 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + +import os +from ament_index_python.packages import get_package_share_directory +import launch +import launch_ros.actions +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +state_estimation_path = get_package_share_directory('state_estimation_2d') +rviz_path = os.path.join(state_estimation_path, 'rviz', 'default.rviz') + +def generate_launch_description(): + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='true' + ), + Node( + package="rviz2", + executable="rviz2", + output='screen', + arguments=['-d'+str(rviz_path)] + ), + ]) \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py index 34f26b3..f0dd695 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py @@ -19,18 +19,50 @@ enable_viz = 'true' def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + return LaunchDescription([ launch.actions.DeclareLaunchArgument( 'viz', default_value=enable_viz, - description='update rate' + description='enable_visualization' + ), + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='true' ), Node( package="state_estimation_2d", executable="state_estimation_2d", output='screen', emulate_tty=True, - parameters=[{"use_sim_time": False}], + parameters=[ + {"use_sim_time": use_sim_time}, + {"use_nn_model": False}, + {"odom_topic": "velocity"}, + {"imu_topic": "imu"}, + {"imu_accel_topic": "/camera/accel/sample"}, + {"imu_gyro_topic": "/camera/gyro/sample"}, + {"cmd_topic": "cmd_vel"}, + {"odom_sim_topic": "odom_noised"}, + {"publish_topic": "odom_filtered"}, + {"imu_frame": "camera_gyro_optical_frame"}, + {"robot_base_frame": "base_link"}, + {"path_to_nn_model": "http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx"}, + {"time_step": 0.1}, + {"odom_sim_covariance": [0.5, 0, + 0, 0.1]}, + {"imu_sim_covariance": [0.5, 0, + 0, 0.1]}, + {"gyro_robot_covariance": 200.0}, + {"accel_robot_covariance": 200.0}, + ], + ), + Node( + package="odom_noiser", + executable="odom_noiser", + output='screen', + emulate_tty=True ), Node( package="path_visualizer", diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py deleted file mode 100644 index 4159b71..0000000 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py +++ /dev/null @@ -1,157 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -import launch -import launch_ros -from ament_index_python.packages import get_package_share_directory - -import os -from ament_index_python.packages import get_package_share_directory -import launch -import launch_ros.actions -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): -# ROSBOT - world_file_name = 'willow_garage.world' - - urdf_file_name = 'urdf/rosbot.urdf' - urdf = os.path.join( - get_package_share_directory('rosbot_description'), - urdf_file_name) - with open(urdf, 'r') as infp: - robot_desc = infp.read() - - gazebo_ros = get_package_share_directory('gazebo_ros') - rosbot_description = get_package_share_directory('rosbot_description') - - gazebo_client = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzclient.launch.py')), - condition=launch.conditions.IfCondition(launch.substitutions.LaunchConfiguration('gui')) - ) - - gazebo_server = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzserver.launch.py')) - ) - - spawn_rosbot = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(rosbot_description, 'launch', 'rosbot_spawn.launch.py')) - ) - - rviz_path = "/home/user/ros2_ws/src/state_estimation_2d/rviz/default.rviz" - -#Teleop - default_update_rate = '20' - update_rate = launch.substitutions.LaunchConfiguration( - 'update_rate', - default=default_update_rate - ) - - keyboard_listener_dir = get_package_share_directory('teleop') - keyboard_listener_launch = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - keyboard_listener_dir + '/keyboard_listener.launch.py' - ), - launch_arguments = {'output_path': update_rate}.items() - ) - - return LaunchDescription([ - - DeclareLaunchArgument( - 'world', - default_value=[os.path.join(rosbot_description, 'worlds', world_file_name), ''], - description='SDF world file'), - DeclareLaunchArgument( - name='gui', - default_value='true' - ), - DeclareLaunchArgument( - name='use_sim_time', - default_value='true' - ), - DeclareLaunchArgument('verbose', default_value='true', - description='Set "true" to increase messages written to terminal.'), - DeclareLaunchArgument('gdb', default_value='false', - description='Set "true" to run gzserver with gdb'), - DeclareLaunchArgument('state', default_value='true', - description='Set "false" not to load "libgazebo_ros_state.so"'), - - launch.actions.DeclareLaunchArgument( - 'update_rate', - default_value=default_update_rate, - description='update rate' - ), - - Node( - package="rviz2", - executable="rviz2", - output='screen', - arguments=['-d'+str(rviz_path)] - ), - Node( - package="odom_noiser", - executable="odom_noiser", - output='screen', - emulate_tty=True - ), - Node( - package="state_estimation_2d", - executable="state_estimation_2d", - output='screen', - emulate_tty=True, - parameters=[{"use_sim_time": False}], - ), - Node( - package="path_visualizer", - executable="path_visualizer" - ), - Node( - package="metric_calculator", - executable="metric_calculator", - output='screen', - emulate_tty=True - ), - # launh Teleop node - Node( - package='rosbot_controller', - executable='rosbot_teleop', - name='rosbot_teleop', - output='screen', - emulate_tty=True, - parameters=[ - {"update_rate": update_rate}, - {'keyboard_topic': "/keyboard"}, - {'control_topic': "/cmd_vel"}, - {'joystick_topic': "/joy"}, - {'movable_camera': "False"}, - {'v_limit': "0.5"}, - {'w_limit': "2.5"}, - {'lin_a': "0.1"}, - {'ang_a': "0.25"}, - ] - ), - Node( - package="robot_localization", - executable='ekf_node', - name='ekf_filter_node', - output='screen', - emulate_tty=True, - parameters=[{os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml')}, - {"use_sim_time": True}], - ), - - # launh listener node - gazebo_server, - gazebo_client, - spawn_rosbot, - keyboard_listener_launch - - ]) \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py index 35436b7..efaf3e6 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py @@ -13,6 +13,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')) ], install_requires=['setuptools'], zip_safe=True, diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py index 8f2c192..7f0c42d 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py @@ -3,6 +3,7 @@ from numpy.linalg import inv import math import scipy +from numba import njit from state_estimation_2d.model import * from state_estimation_2d.measurement import * @@ -133,39 +134,49 @@ def update_odom(self, z_odom, R_odom): """ Update state vector using odometry measurements""" H = get_jacobian_odom(self.x_opt) y = z_odom - H @ self.x_opt - G = H @ self.P_opt @ H.T + R_odom - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_odom, + H + ) def update_imu(self, z_imu, R_imu): """ Update state vector using imu measurements""" H = get_jacobian_imu(self.x_opt) - y = z_imu - imu(self.x_opt) - G = H @ self.P_opt @ H.T + R_imu - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y - + y = z_imu - h_imu(self.x_opt) + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_imu, + H + ) + def update_imu_accel(self, z_accel, R_accel): + """ Update state vector using imu measurements""" H = get_jacobian_accel(self.x_opt) - y = z_accel - accel(self.x_opt) - G = H @ self.P_opt @ H.T + R_accel - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + y = z_accel - h_accel(self.x_opt) + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_accel, + H + ) def update_imu_gyro(self, z_gyro, R_gyro): + """ Update state vector using imu measurements""" H = get_jacobian_gyro(self.x_opt) - y = z_gyro - accel(self.x_opt) - G = H @ self.P_opt @ H.T + R_gyro - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + y = z_gyro - h_gyro(self.x_opt) + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_gyro, + H + ) @property def v(self): @@ -194,3 +205,11 @@ def yaw(self): @yaw.setter def yaw(self, value): self.x_opt[3] = value + +@njit +def kalman_update(x, P, y, R, H): + G = H @ P @ H.T + R + K = P @ H.T @ inv(G) + new_x = x + K @ y + new_P = (np.eye(x.shape[0]) - K @ H) @ P + return new_x, new_P \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py index 5515f88..d816b7f 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py @@ -32,20 +32,20 @@ def get_jacobian_gyro(x): H[0, 4] = 1 return H -def imu(x): +def h_imu(x): """Imu measurement function""" z = np.zeros(2) z[0] = x[2] * x[4] z[1] = x[4] return z -def accel(x): +def h_accel(x): """Imu measurement function""" z = np.zeros(1) z[0] = x[2] * x[4] return z -def gyro(x): +def h_gyro(x): """Imu measurement function""" z = np.zeros(1) z[0] = x[4] diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py index 65a57c2..5403a9a 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py @@ -1,12 +1,17 @@ import numpy as np -"""Model dynamics functions""" def transform_jacobian(x, dt): - """Dynamic model function jacobian""" + """Dynamic model function jacobian + State vector x: + 0. x + 1. y + 2. V_parallel (velocity along the robot direction) + 3. yaw + 4. yaw_vel + """ J_f = np.eye(5) J_f[0, 2] = np.cos(x[3]) * dt - # J_f[1, 2] = np.sin(x[4]) * dt J_f[0, 3] = -x[2] * np.sin(x[3]) * dt J_f[1, 2] = np.sin(x[3]) * dt J_f[1, 3] = x[2] * np.cos(x[3]) * dt diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py index bcb84cf..193caa3 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py @@ -82,30 +82,45 @@ class StateEstimation2D(Node): def __init__(self): super().__init__('state_estimation_2d') # ROS Subscribers + self.init_parameters() + self.update_parameters() + self.odom_sub = self.create_subscription( Twist, - 'velocity', + self.odom_topic, self.odometry_callback, 10) + self.odom_sim_sub = self.create_subscription( + Odometry, + self.odom_sim_topic, + self.odom_sim_callback, + 10) self.cmd_vel_sub = self.create_subscription( Twist, - 'cmd_vel', + self.cmd_topic, self.control_callback, 15) self.imu_accel_sub = self.create_subscription( Imu, - '/camera/accel/sample', + self.imu_accel_topic, self.imu_accel_callback, 15) self.imu_gyro_sub = self.create_subscription( Imu, - '/camera/gyro/sample', + self.imu_gyro_topic, self.imu_gyro_callback, 15) - self.pose_pub = self.create_publisher(Odometry, '/odom_filtered', 10) + self.imu_sub = self.create_subscription( + Imu, + self.imu_topic, + self.imu_callback, + 15) + + self.pose_pub = self.create_publisher(Odometry, self.publish_topic, 10) self.tf2_broadcaster = tf2_ros.TransformBroadcaster(self) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + # ROS variables self.odom_noised = Odometry() self.odom_gt = Odometry() @@ -113,38 +128,85 @@ def __init__(self): self.got_measurements = 0 self.control = np.zeros(2) self.z_odom = np.zeros(2) - self.z_accel = np.zeros(1) + self.z_imu = np.zeros(2) + self.z_accel = None self.z_gyro = None self.imu_extrinsic = None self.rot_extrinsic = None + # Upload NN control model - self.model_path = 'http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx' - self.model = nnio.ONNXModel(self.model_path) + if self.use_nn_model: + self.model = nnio.ONNXModel(self.model_path) + # Filter parameters - self.dt = 0.1 - self.R_odom = np.array([[0.5, 0], - [0, 0.1]]) - self.R_accel = np.array([[200]]) - self.R_gyro = np.array([[200]]) - self.filter = Filter2D( x_init=np.zeros(5), P_init=np.eye(5) * 0.01, dt=self.dt, - v_var=0.25, - w_var=0.01, + v_var=self.R_odom[0][0]**2, + w_var=self.R_odom[1][1]**2, ) + self.distance = 0 self.x_prev = 0 self.y_prev = 0 self.odom_gt_prev = Odometry() self.ate = ErrorEstimator() + # Timer for update function self.predict_timer = self.create_timer( self.dt, self.step_filter ) + def init_parameters(self): + self.declare_parameters( + namespace="", + parameters=[ + ("use_nn_model", False), + ("odom_topic", "odom"), + ("imu_topic", "imu"), + ("imu_accel_topic", "/camera/accel/sample"), + ("imu_gyro_topic", "/camera/gyro/sample"), + ("cmd_topic", "cmd"), + ("odom_sim_topic", "odom_noised"), + ("publish_topic", "odom_filtered"), + ("imu_frame", "camera_gyro_optical_frame"), + ("robot_base_frame", "base_link"), + ("path_to_nn_model", "http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx"), + ("odom_noised_sim_topic", "odom_noised"), + ("time_step", 0.1), + ("odom_sim_covariance", [0.5, 0, + 0, 0.1]), + ("imu_sim_covariance", [0.5, 0, + 0, 0.1]), + ("gyro_robot_covariance", 200.0), + ("accel_robot_covariance", 200.0), + ] + ) + + def update_parameters(self): + self.use_nn_model = self.get_parameter('use_nn_model').get_parameter_value().bool_value + self.odom_topic = self.get_parameter('odom_topic').get_parameter_value().string_value + self.imu_topic = self.get_parameter('imu_topic').get_parameter_value().string_value + self.imu_accel_topic = self.get_parameter('imu_accel_topic').get_parameter_value().string_value + self.imu_gyro_topic = self.get_parameter('imu_gyro_topic').get_parameter_value().string_value + self.cmd_topic = self.get_parameter('cmd_topic').get_parameter_value().string_value + self.odom_sim_topic = self.get_parameter('odom_sim_topic').get_parameter_value().string_value + self.publish_topic = self.get_parameter('publish_topic').get_parameter_value().string_value + self.imu_frame = self.get_parameter('imu_frame').get_parameter_value().string_value + self.robot_base_frame = self.get_parameter('robot_base_frame').get_parameter_value().string_value + self.model_path = self.get_parameter('path_to_nn_model').get_parameter_value().string_value + self.odom_noised_topic = self.get_parameter('odom_noised_sim_topic').get_parameter_value().string_value + self.dt = self.get_parameter('time_step').get_parameter_value().double_value + R_odom_vec = self.get_parameter('odom_sim_covariance').get_parameter_value().double_array_value + R_imu_vec = self.get_parameter('imu_sim_covariance').get_parameter_value().double_array_value + self.R_accel = np.array(self.get_parameter('accel_robot_covariance').get_parameter_value().double_value) + self.R_gyro = np.array(self.get_parameter('gyro_robot_covariance').get_parameter_value().double_value) + + self.R_odom = np.array(R_odom_vec).reshape(len(R_odom_vec)//2, len(R_odom_vec)//2) + self.R_imu = np.array(R_imu_vec).reshape(len(R_imu_vec)//2, len(R_imu_vec)//2) + def control_callback(self, msg): """ Callback from /cmd_vel topic with control commands @@ -163,6 +225,14 @@ def odometry_callback(self, msg): self.z_odom = self.odometry_to_vector(msg) self.got_measurements = 1 + def odom_sim_callback(self, msg): + self.odom_sim = msg + self.z_odom = np.array([ + msg.twist.twist.linear.x, + msg.twist.twist.angular.z, + ]) + self.got_measurements = 1 + def odometry_to_vector(self, odom): """ Transfer odometry message to measurement vector for filter @@ -172,7 +242,7 @@ def odometry_to_vector(self, odom): """ z_odom = np.zeros(2) z_odom[0] = odom.linear.x - z_odom[1] = odom.angular.z + z_odom[1] = odom.angular.z * 2 return z_odom def imu_accel_callback(self, msg): @@ -184,10 +254,9 @@ def imu_accel_callback(self, msg): self.z_accel = accel[1] def imu_gyro_callback(self, msg): - # print(self.imu_extrinsic) if self.imu_extrinsic is None: print("Finding imu extrinsics") - self.imu_extrinsic = self.get_extrinsic("camera_gyro_optical_frame", "base_link") + self.imu_extrinsic = self.get_extrinsic(self.imu_frame, self.robot_base_frame) if self.imu_extrinsic is not None and self.rot_extrinsic is None: self.rot_extrinsic = np.ascontiguousarray(self.imu_extrinsic[:3,:3]) self.imu_gyro = msg @@ -196,6 +265,22 @@ def imu_gyro_callback(self, msg): self.imu_gyro.angular_velocity.y, self.imu_gyro.angular_velocity.z]) self.z_gyro = gyro[2] + + def imu_callback(self, msg): + """ + Callback from /imu topic + @ parameters + msg: Imu + """ + self.z_imu = self.imu_to_vector(msg) + self.got_measurements = 1 + + def imu_to_vector(self, imu): + """Transfer imu message to measurement vector for filter""" + z_imu = np.zeros(2) + z_imu[0] = imu.linear_acceleration.y + z_imu[1] = imu.angular_velocity.z + return z_imu def step_filter(self): """ @@ -204,10 +289,14 @@ def step_filter(self): """ if self.got_measurements: # Predict step - # self.filter.predict_by_nn_model(self.model, self.control) - self.filter.predict_by_naive_model(self.control) + if self.use_nn_model: + self.filter.predict_by_nn_model(self.model, self.control) + else: + self.filter.predict_by_naive_model(self.control) # Measurement update step self.filter.update_odom(self.z_odom, self.R_odom) + if self.z_imu is not None: + self.filter.update_imu(self.z_imu, self.R_imu) # if self.z_gyro is not None: if self.z_gyro is not None: self.filter.update_imu_gyro(self.z_gyro, self.R_gyro) @@ -278,10 +367,16 @@ def state_to_odometry(self, x, P): self.odom_filtered.pose.pose.orientation.z = q[2] self.odom_filtered.pose.pose.orientation.w = q[3] self.odom_filtered.twist.twist.linear.x = x[2] - self.odom_filtered.twist.twist.linear.y = self.odom_noised.linear.y - self.odom_filtered.twist.twist.linear.z = self.odom_noised.linear.z - self.odom_filtered.twist.twist.angular.x = self.odom_noised.angular.x - self.odom_filtered.twist.twist.angular.y = self.odom_noised.angular.y + if self.z_accel is not None: + self.odom_filtered.twist.twist.linear.y = self.odom_noised.linear.y + self.odom_filtered.twist.twist.linear.z = self.odom_noised.linear.z + self.odom_filtered.twist.twist.angular.x = self.odom_noised.angular.x + self.odom_filtered.twist.twist.angular.y = self.odom_noised.angular.y + else: + self.odom_filtered.twist.twist.linear.y = self.odom_noised.twist.twist.linear.y + self.odom_filtered.twist.twist.linear.z = self.odom_noised.twist.twist.linear.z + self.odom_filtered.twist.twist.angular.x = self.odom_noised.twist.twist.angular.x + self.odom_filtered.twist.twist.angular.y = self.odom_noised.twist.twist.angular.y self.odom_filtered.twist.twist.angular.z = x[4] # Fill the odometry message covariance matrix with computed KF covariance @@ -303,8 +398,8 @@ def pose_covariance_to_vector(self, P): Transfer filter computed pose covariance matrix to vector http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseWithCovariance.html @ parameters - P: np.array - Covariance matrix + P: np.array of shape (dim(x), dim(x)) + Covariance matrix for corresponding variables of state vector """ cov_vector = self.odom_noised.pose.covariance cov_vector[14] = 0.1 @@ -326,8 +421,8 @@ def twist_covariance_to_vector(self, P): Transfer filter computed twist covariance matrix to vector http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TwistWithCovariance.html @ parameters - P: np.array - Covariance matrix + P: np.array of shape (dim(x), dim(x)) + Covariance matrix for corresponding variables of state vector """ cov_vector = self.odom_noised.twist.covariance cov_vector[0] = P[2,2]