Se wip

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)]
+        ),
+    ])

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",

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,

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

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]

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