From bc38a3a21c930f6bc86ae69a87b387131aef78eb Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 7 Mar 2024 19:23:30 +0100
Subject: [PATCH 01/12] Update covariance

---
 panther_controller/config/WH01_controller.yaml | 6 +++---
 panther_controller/config/WH02_controller.yaml | 4 ++--
 panther_controller/config/WH04_controller.yaml | 4 ++--
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/panther_controller/config/WH01_controller.yaml b/panther_controller/config/WH01_controller.yaml
index f7608694a..ace9a5739 100644
--- a/panther_controller/config/WH01_controller.yaml
+++ b/panther_controller/config/WH01_controller.yaml
@@ -14,8 +14,8 @@
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [1.1e-4, 0.0, 0.0, 0.0, 1.1e-4, 0.0, 0.0, 0.0, 1.1e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [6.2e-6, 0.0, 0.0, 0.0, 6.2e-6, 0.0, 0.0, 0.0, 6.2e-6] # Values measured experimentally
+      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
+      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
 
   panther_base_controller:
     ros__parameters:
@@ -35,7 +35,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [5.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 3.6e-5] # Values measured experimentally
+      twist_covariance_diagonal: [4.2e-6, 4.2e-6, 0.0, 0.0, 0.0, 1.48e-5] # Values measured experimentally
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false
diff --git a/panther_controller/config/WH02_controller.yaml b/panther_controller/config/WH02_controller.yaml
index e04d750e9..2cf12d0d5 100644
--- a/panther_controller/config/WH02_controller.yaml
+++ b/panther_controller/config/WH02_controller.yaml
@@ -15,7 +15,7 @@
       sensor_name: imu
       frame_id: imu_link
       static_covariance_angular_velocity: [1.1e-4, 0.0, 0.0, 0.0, 1.1e-4, 0.0, 0.0, 0.0, 1.1e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [6.2e-6, 0.0, 0.0, 0.0, 6.2e-6, 0.0, 0.0, 0.0, 6.2e-6] # Values measured experimentally
+      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
 
   panther_base_controller:
     ros__parameters:
@@ -36,7 +36,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [5.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 3.6e-5] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [5.0e-6, 5.0e-6, 0.0, 0.0, 0.0, 3.6e-5] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false
diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
index c497c6f65..a44625a0a 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/panther_controller/config/WH04_controller.yaml
@@ -15,7 +15,7 @@
       sensor_name: imu
       frame_id: imu_link
       static_covariance_angular_velocity: [1.1e-4, 0.0, 0.0, 0.0, 1.1e-4, 0.0, 0.0, 0.0, 1.1e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [6.2e-6, 0.0, 0.0, 0.0, 6.2e-6, 0.0, 0.0, 0.0, 6.2e-6] # Values measured experimentally
+      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
 
   panther_base_controller:
     ros__parameters:
@@ -36,7 +36,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [1.3e-6, 2.7e-7, 0.0, 0.0, 0.0, 9.3e-6] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [1.3e-6, 1.3e-6, 0.0, 0.0, 0.0, 9.3e-6] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false

From 0f6e0dae29b5823b536333b0b8c53c0a9fc06d4e Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 8 Mar 2024 09:57:25 +0100
Subject: [PATCH 02/12] EKF config

---
 panther_bringup/config/ekf.yaml | 69 ++++++++++++++-------------------
 1 file changed, 29 insertions(+), 40 deletions(-)

diff --git a/panther_bringup/config/ekf.yaml b/panther_bringup/config/ekf.yaml
index d791a8231..4e280459a 100644
--- a/panther_bringup/config/ekf.yaml
+++ b/panther_bringup/config/ekf.yaml
@@ -1,13 +1,14 @@
 /**:
   ekf_node:
+    # Default
     # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
     ros__parameters:
       frequency: 50.0
-      sensor_timeout: 0.2
+      sensor_timeout: 0.05
       two_d_mode: true
 
       transform_time_offset: 0.0
-      transform_timeout: 0.1
+      transform_timeout: 0.05
 
       map_frame: map
       odom_frame: odom
@@ -16,27 +17,27 @@
       publish_tf: true
       publish_acceleration: false
 
-      odom0: panther_base_controller/odom
-      odom0_config: [false,  false,  false,
+      odom0: odom/wheels
+      odom0_config: [false, false, false,
                     false, false, false,
                     true, true, false,
-                    false, false, true,
+                    false, false, false,
                     false, false, false]
-      odom0_queue_size: 3
-      odom0_nodelay: false
+      odom0_queue_size: 5
+      odom0_nodelay: true
       odom0_differential: false
       odom0_relative: true
 
-      imu0: imu_broadcaster/imu
+      imu0: imu/data
       imu0_config: [false, false, false,
                     false, false, false,
                     false, false, false,
                     false, false, true,
                     false, false, false]
-      imu0_nodelay: false
+      imu0_queue_size: 5
+      imu0_nodelay: true
       imu0_differential: false
       imu0_relative: true
-      imu0_queue_size: 3
       imu0_remove_gravitational_acceleration: false
 
       use_control: true
@@ -44,34 +45,22 @@
       control_config: [true, true, false, false, false, true]
       acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config
 
-      process_noise_covariance: [1e-4,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    1e-4,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    1e-4,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    3e-3,   0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    3e-3,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    3e-3,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    2e-5,    0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     2e-5,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     2e-5,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    2e-4,   0.0,    0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    2e-4,   0.0,    0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    2e-4,   0.0,    0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    5e-5,   0.0,    0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    5e-5,   0.0,
-                                 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    5e-5]
+      predict_to_current_time: true
 
-      initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,    0.0,     0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,    0.0,     0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,    0.0,    0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9,   0.0,    0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9,   0.0,
-                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]
+      # Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values)
+      dynamic_process_noise_covariance: true
+      process_noise_covariance: [1e-4,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,  1e-4,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,  1e-4,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,  3e-3,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,  3e-3,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,  3e-3,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-5,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-5,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-5,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-4,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-4,   0.0,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  2e-4,   0.0,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5,   0.0,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5,   0.0,
+                                  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5]

From f24e3eb4226d0872296de31f9c36214142b78ce5 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 8 Mar 2024 10:13:28 +0100
Subject: [PATCH 03/12] fix controller topic

---
 panther_description/urdf/gazebo.urdf.xacro | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index fa5da161b..31d5c110b 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -48,7 +48,8 @@
         name="ign_ros2_control::IgnitionROS2ControlPlugin">
         <parameters>${config_file}</parameters>
         <ros>
-          <remapping>/panther_base_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
+          <remapping>panther_base_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
+          <remapping>panther_base_controller/odom:=odom/wheels</remapping>
         </ros>
       </plugin>
     </gazebo>

From aa79987aa875e194abf914326070e091cf047570 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 8 Mar 2024 11:11:57 +0100
Subject: [PATCH 04/12] fix imu topic

---
 panther_description/urdf/gazebo.urdf.xacro | 1 +
 1 file changed, 1 insertion(+)

diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index 31d5c110b..babaf30b7 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -50,6 +50,7 @@
         <ros>
           <remapping>panther_base_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
           <remapping>panther_base_controller/odom:=odom/wheels</remapping>
+          <remapping>/imu_broadcaster/imu:=imu/data</remapping>
         </ros>
       </plugin>
     </gazebo>

From 06f4562c87c9106630e7dd2b224763dc7c70061d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 19 Mar 2024 13:35:09 +0100
Subject: [PATCH 05/12] Change relative to abs speed frame

---
 .../src/roboteq_data_converters.cpp                 | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/panther_hardware_interfaces/src/roboteq_data_converters.cpp b/panther_hardware_interfaces/src/roboteq_data_converters.cpp
index 132f44050..7d7be2467 100644
--- a/panther_hardware_interfaces/src/roboteq_data_converters.cpp
+++ b/panther_hardware_interfaces/src/roboteq_data_converters.cpp
@@ -50,13 +50,16 @@ MotorState::MotorState(const DrivetrainSettings & drivetrain_settings)
                                      (1.0f / drivetrain_settings.gear_ratio) * (2.0f * M_PI);
 
   // Convert speed feedback from Roboteq (RPM) to wheel speed in rad/s. Steps:
-  // 1. Convert motor rotation per minute feedback speed to wheel rotation per minute speed
+  // 1. Convert the Roboteq velocity value (± 1000), relative to the max_rpm_motor_speed parameter
+  // (Roboteq MXRPM), into rotations per minute.
+  // 2. Convert motor rotation per minute feedback speed to wheel rotation per minute speed
   //    (multiplication by (1.0/gear_ratio))
-  // 2. Convert wheel rotation per minute speed to wheel rotation per second speed (multiplication
+  // 3. Convert wheel rotation per minute speed to wheel rotation per second speed (multiplication
   //    by (1.0/60.0))
-  // 3. Convert wheel rotation per second speed to wheel rad/s speed (multiplication by 2.0*pi)
-  roboteq_vel_feedback_to_radians_per_second_ = (1.0f / drivetrain_settings.gear_ratio) *
-                                                (1.0f / 60.0f) * (2.0f * M_PI);
+  // 4. Convert wheel rotation per second speed to wheel rad/s speed (multiplication by 2.0*pi)
+  roboteq_vel_feedback_to_radians_per_second_ =
+    (drivetrain_settings.max_rpm_motor_speed / 1000.0f) * (1.0f / drivetrain_settings.gear_ratio) *
+    (1.0f / 60.0f) * (2.0f * M_PI);
 
   // Convert current feedback from Roboteq (A*10.) to wheel torque in Nm. Steps:
   // 1. Convert motor A*10.0 current feedback to motor A current (multiplication by (1.0/10.0))

From 922253a41e1dae56667d4d2e46a079eff6f15b75 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 19 Mar 2024 13:37:07 +0100
Subject: [PATCH 06/12] update covariances

---
 panther_controller/config/WH01_controller.yaml | 2 +-
 panther_controller/config/WH02_controller.yaml | 4 ++--
 panther_controller/config/WH04_controller.yaml | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/panther_controller/config/WH01_controller.yaml b/panther_controller/config/WH01_controller.yaml
index ace9a5739..ed0405904 100644
--- a/panther_controller/config/WH01_controller.yaml
+++ b/panther_controller/config/WH01_controller.yaml
@@ -35,7 +35,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [4.2e-6, 4.2e-6, 0.0, 0.0, 0.0, 1.48e-5] # Values measured experimentally
+      twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Values measured experimentally
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false
diff --git a/panther_controller/config/WH02_controller.yaml b/panther_controller/config/WH02_controller.yaml
index 2cf12d0d5..5a6f4cf24 100644
--- a/panther_controller/config/WH02_controller.yaml
+++ b/panther_controller/config/WH02_controller.yaml
@@ -14,7 +14,7 @@
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [1.1e-4, 0.0, 0.0, 0.0, 1.1e-4, 0.0, 0.0, 0.0, 1.1e-4]  # Values measured experimentally
+      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
       static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
 
   panther_base_controller:
@@ -36,7 +36,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [5.0e-6, 5.0e-6, 0.0, 0.0, 0.0, 3.6e-5] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false
diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
index a44625a0a..cd52c31b9 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/panther_controller/config/WH04_controller.yaml
@@ -14,7 +14,7 @@
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [1.1e-4, 0.0, 0.0, 0.0, 1.1e-4, 0.0, 0.0, 0.0, 1.1e-4]  # Values measured experimentally
+      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
       static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
 
   panther_base_controller:
@@ -36,7 +36,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [1.3e-6, 1.3e-6, 0.0, 0.0, 0.0, 9.3e-6] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [1.1e-6, 1.1e-6, 0.0, 0.0, 0.0, 3.8e-4] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false

From 29fa4f8bfcb9e901689bdb96db207ad79e0bbe5f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 19 Mar 2024 14:36:33 +0100
Subject: [PATCH 07/12] Add comments

---
 panther_controller/config/WH01_controller.yaml |  9 +++++++--
 panther_controller/config/WH02_controller.yaml |  9 +++++++--
 panther_controller/config/WH04_controller.yaml |  9 +++++++--
 panther_description/urdf/gazebo.urdf.xacro     | 13 +++++++------
 4 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/panther_controller/config/WH01_controller.yaml b/panther_controller/config/WH01_controller.yaml
index ed0405904..511b38eaa 100644
--- a/panther_controller/config/WH01_controller.yaml
+++ b/panther_controller/config/WH01_controller.yaml
@@ -10,12 +10,17 @@
       imu_broadcaster:
         type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
+  # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
+      # orientation_stddev: 1.0e-2 for madgwick filter
+      static_covariance_orientation: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, as per manual
+      static_covariance_angular_velocity: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # linear_acceleration_stdev: 2.8 mg (27.5 mm/s^2) accelerometer white noise sigma, as per manual
+      static_covariance_linear_acceleration: [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4]
 
   panther_base_controller:
     ros__parameters:
diff --git a/panther_controller/config/WH02_controller.yaml b/panther_controller/config/WH02_controller.yaml
index 5a6f4cf24..a9fce427f 100644
--- a/panther_controller/config/WH02_controller.yaml
+++ b/panther_controller/config/WH02_controller.yaml
@@ -10,12 +10,17 @@
       imu_broadcaster:
         type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
+  # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
+      # orientation_stddev: 1.0e-2 for madgwick filter
+      static_covariance_orientation: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, as per manual
+      static_covariance_angular_velocity: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # linear_acceleration_stdev: 2.8 mg (27.5 mm/s^2) accelerometer white noise sigma, as per manual
+      static_covariance_linear_acceleration: [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4]
 
   panther_base_controller:
     ros__parameters:
diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
index cd52c31b9..4133a72d6 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/panther_controller/config/WH04_controller.yaml
@@ -10,12 +10,17 @@
       imu_broadcaster:
         type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
+  # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
       sensor_name: imu
       frame_id: imu_link
-      static_covariance_angular_velocity: [6.4e-4, 0.0, 0.0, 0.0, 6.4e-4, 0.0, 0.0, 0.0, 6.4e-4]  # Values measured experimentally
-      static_covariance_linear_acceleration: [2.54e-1, 0.0, 0.0, 0.0, 2.54e-1, 0.0, 0.0, 0.0, 2.54e-1] # Values measured experimentally
+      # orientation_stddev: 1.0e-2 for madgwick filter
+      static_covariance_orientation: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, as per manual
+      static_covariance_angular_velocity: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # linear_acceleration_stdev: 2.8 mg (27.5 mm/s^2) accelerometer white noise sigma, as per manual
+      static_covariance_linear_acceleration: [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4]
 
   panther_base_controller:
     ros__parameters:
diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index babaf30b7..4226bb8dd 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -56,7 +56,7 @@
     </gazebo>
   </xacro:macro>
 
-  <!-- IMU - noise -->
+  <!-- Gauss noise macro -->
   <xacro:macro name="gauss_noise"
     params="mean:=0.0 stddev:=0.0 bias_mean:=0.0 bias_stddev:=0.0 precision:=0.0">
     <noise type="gaussian">
@@ -68,8 +68,7 @@
     </noise>
   </xacro:macro>
 
-  <!-- IMU -->
-  <!-- Noise ref: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications -->
+  <!-- IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications -->
   <xacro:macro name="imu" params="reference_frame name:='imu'">
     <gazebo reference="${reference_frame}">
       <sensor name="${name}" type="imu">
@@ -83,6 +82,7 @@
         <ignition_frame_id>imu_link</ignition_frame_id>
         <imu>
           <angular_velocity>
+            <!-- rad/s -->
             <x>
               <xacro:gauss_noise stddev="0.01" bias_mean="3.3e-5" precision="1.2e-3" />
             </x>
@@ -94,14 +94,15 @@
             </z>
           </angular_velocity>
           <linear_acceleration>
+            <!-- m/s^2 -->
             <x>
-              <xacro:gauss_noise stddev="2.8e-3" bias_mean="1.9e-3" precision="9.8e-4" />
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
             </x>
             <y>
-              <xacro:gauss_noise stddev="2.8e-3" bias_mean="1.9e-3" precision="9.8e-4" />
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
             </y>
             <z>
-              <xacro:gauss_noise stddev="2.8e-3" bias_mean="1.9e-3" precision="9.8e-4" />
+              <xacro:gauss_noise stddev="27.5e-3" bias_mean="18.6e-3" precision="9.6e-3" />
             </z>
           </linear_acceleration>
         </imu>

From ab2533cbbf4f2c7800b2b0d072c0680c121b694b Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 21 Mar 2024 08:40:36 +0100
Subject: [PATCH 08/12] Update panther_controller/config/WH04_controller.yaml

---
 panther_controller/config/WH04_controller.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
index 4133a72d6..8c3de7fb8 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/panther_controller/config/WH04_controller.yaml
@@ -41,7 +41,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [1.1e-6, 1.1e-6, 0.0, 0.0, 0.0, 3.8e-4] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [1.1e-6, 1.1e-6, 0.0, 0.0, 0.0, 1.0e-5] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false

From dfe6475d20a11d381fec56143306b99774f81015 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 21 Mar 2024 08:42:34 +0100
Subject: [PATCH 09/12] Update panther_controller/config/WH04_controller.yaml

---
 panther_controller/config/WH04_controller.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
index 8c3de7fb8..3e8b50ea1 100644
--- a/panther_controller/config/WH04_controller.yaml
+++ b/panther_controller/config/WH04_controller.yaml
@@ -41,7 +41,7 @@
       publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
-      twist_covariance_diagonal: [1.1e-6, 1.1e-6, 0.0, 0.0, 0.0, 1.0e-5] # Selected intuitively based on WH01 results
+      twist_covariance_diagonal: [2.7e-5, 2.7e-5, 0.0, 0.0, 0.0, 1.0e-4] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
       open_loop: false

From 3918b6fe9afebed90aa9cd8ed7f653a09438f10d Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Fri, 22 Mar 2024 14:06:18 +0100
Subject: [PATCH 10/12] Update panther_bringup/config/ekf.yaml

---
 panther_bringup/config/ekf.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/panther_bringup/config/ekf.yaml b/panther_bringup/config/ekf.yaml
index 4e280459a..621b21a60 100644
--- a/panther_bringup/config/ekf.yaml
+++ b/panther_bringup/config/ekf.yaml
@@ -1,6 +1,5 @@
 /**:
   ekf_node:
-    # Default
     # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
     ros__parameters:
       frequency: 50.0

From b402b84713c009e8cc3224b8138d1589e79aa70a Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 22 Mar 2024 15:00:56 +0100
Subject: [PATCH 11/12] Add initial ekf setting

---
 panther_bringup/config/ekf.yaml | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/panther_bringup/config/ekf.yaml b/panther_bringup/config/ekf.yaml
index 621b21a60..d1119562d 100644
--- a/panther_bringup/config/ekf.yaml
+++ b/panther_bringup/config/ekf.yaml
@@ -63,3 +63,25 @@
                                   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5,   0.0,   0.0,
                                   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5,   0.0,
                                   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5]
+
+      initial_state: [1.0,  0.0,  0.0,
+                      0.0,  0.0,  0.0,
+                      0.0,  0.0,  0.0,
+                      0.0,  0.0,  0.0,
+                      0.0,  0.0,  0.0]
+
+      initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,    0.0,     0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,    0.0,     0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,    0.0,    0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9,   0.0,    0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9,   0.0,
+                                    0.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]

From 8fef167edb6832adcdd45a39e351b09468578a57 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 22 Mar 2024 15:11:33 +0100
Subject: [PATCH 12/12] fix

---
 panther_bringup/config/ekf.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/panther_bringup/config/ekf.yaml b/panther_bringup/config/ekf.yaml
index d1119562d..969b4338d 100644
--- a/panther_bringup/config/ekf.yaml
+++ b/panther_bringup/config/ekf.yaml
@@ -64,7 +64,7 @@
                                   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5,   0.0,
                                   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,  5e-5]
 
-      initial_state: [1.0,  0.0,  0.0,
+      initial_state: [0.0,  0.0,  0.0,
                       0.0,  0.0,  0.0,
                       0.0,  0.0,  0.0,
                       0.0,  0.0,  0.0,