Ekf localization (#127)

* ekf initial

* ekf working on bag

* update

* atfer panther tests

* bringup - include ekf

* Maciej suggestions

* Add dynamic process noice

* Final ekf config

* wheek param

* import

* refactor

* Update panther_bringup/config/ekf_config.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

* Update panther_bringup/config/imu_config.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

* Update panther_bringup/config/imu_config.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

* Sugestions

* Update panther_description/config/WH01.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

* Update panther_description/config/WH02.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

* Update panther_description/config/WH04.yaml

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

---------

Co-authored-by: Maciej Stępień <maciek1284@outlook.com>

panther_bringup/config/ekf_config.yaml

@@ -0,0 +1,63 @@
+# Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
+
+frequency: 20
+sensor_timeout: 0.2
+two_d_mode: true
+
+transform_time_offset: 0.0
+transform_timeout: 0.1
+
+map_frame: map
+odom_frame: odom
+base_link_frame: base_link
+world_frame: odom
+publish_tf: true
+publish_acceleration: false
+
+odom0: odom/wheels
+odom0_config: [false,  false,  false,
+               false, false, false,
+               true, true, false,
+               false, false, true,
+               false, false, false]
+odom0_queue_size: 2 
+odom0_nodelay: false
+odom0_differential: false
+odom0_relative: true
+
+imu0: imu/data
+imu0_config: [false, false, false,
+              false, false, false,
+              false, false, false,
+              false, false, true,
+              true, true, false]
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 5
+
+imu0_remove_gravitational_acceleration: true
+
+reset_on_time_jump: false
+predict_to_current_time: false
+print_diagnostics: false
+debug: false
+debug_out_file: /path/to/debug/file.txt
+
+# 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,    1e-4, 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,    3e-3, 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,    3e-3, 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,    2e-5, 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,    5e-5, 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,    5e-5, 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,    5e-5, 0,
+                           0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    5e-5]

panther_bringup/config/imu_config.yaml

@@ -1,19 +1,27 @@
 ImuFilterNodelet:
+  gain: 0.1
+  zeta: 0.0
+  mag_bias_x: 0.0
+  mag_bias_y: 0.0
+  mag_bias_z: 0.0
+  orientation_stddev: 1e-2
   publish_tf: false
   use_mag: false
-  use_magnetic_field_msg: false
+  use_magnetic_field_msg: true # If set to true, subscribe to the /imu/mag topic as a sensor_msgs/MagneticField; if set to false (deprecated), use geometry_msgs/Vector3Stamped. Only changes type of the message, whether mag data is used is controlled by the use_mag parameter
   fixed_frame: imu_link
   stateless: false
+  remove_gravity_vector: false
 
 PhidgetsSpatialNodelet:
   data_interval_ms: 8 # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms)
   publish_rate: 100.0 # optional param publish_rate, defaults to 0
   # serial: 123456  # optional param serial, default is -1
   # hub_port: 0 # optional param hub_port, used if connected to a VINT hub
   # frame_id: imu_link  # optional param frame_id, default is "imu_link"
-  # linear_acceleration_stdev: 0.002745862  # optional param linear_acceleration_stdev, default is 280ug
-  # angular_velocity_stdev: 0.000349  # optional param angular_velocity_stdev, default is 0.095 deg/s
-  # magnetic_field_stdev: 1.1e-7  # optional param magnetic_field_stdev, default is 1.1 milligauss
+  # stdev values from imu specification (https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1205)
+  linear_acceleration_stdev: 3.5e-3   # Currently used IMU specification 10 * (2mg ± 1.5mg) (Accelerometer Drift Max ± Accelerometer Noise for 100 Hz)
+  angular_velocity_stdev: 1.5e-2      # Currently used IMU specification 0.1°/s ± 0.05°/s (Gyroscope Drift Max ± Gyroscope Noise for 100 Hz)
+  # magnetic_field_stdev: 10.0          # Currently used IMU specification ± 10mG (± Magnetometer Noise)
 
   # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide)
   # cc_mag_field: 0.52859

panther_bringup/config/panther_common.yaml

@@ -1,7 +1,7 @@
 can_interface: panther_can
 odom_frame: odom
 base_link_frame: base_link
-publish_tf: true
+publish_tf: false
 publish_odom: true
 publish_pose: false
 publish_joints: true

panther_bringup/launch/bringup.launch

@@ -62,6 +62,8 @@
 
     <include file="$(find panther_bringup)/launch/imu.launch" />
 
+    <include file="$(find panther_bringup)/launch/ekf.launch" />
+
   </group>
 
   <include file="$(find panther_description)/launch/panther_state_publisher.launch" if="$(arg publish_robot_state)">

panther_bringup/launch/ekf.launch

@@ -0,0 +1,7 @@
+<launch>
+
+    <node pkg="robot_localization" type="ekf_localization_node" name="panther_ekf" clear_params="true">
+      <rosparam command="load" file="$(find panther_bringup)/config/ekf_config.yaml" />
+    </node>
+
+</launch>

panther_bringup/package.xml

@@ -24,5 +24,6 @@
   <depend>panther_msgs</depend>
   <depend>panther_power_control</depend>
   <depend>phidgets_spatial</depend>
+  <depend>robot_localization</depend>
 
 </package>

panther_description/config/WH01.yaml

@@ -6,3 +6,8 @@ inertia_y_offset: 0.0
 mesh_package: panther_description
 folder_path: meshes/WH01
 kinematics: differential
+
+# Value measured (using Husarion research toolkit)
+velocity_x_stderr: 3.2e-3
+velocity_y_stderr: 1.4e-3
+velocity_yaw_stderr: 8.5e-3

panther_description/config/WH02.yaml

@@ -6,3 +6,8 @@ inertia_y_offset: 0.0
 mesh_package: panther_description
 folder_path: meshes/WH02
 kinematics: mecanum
+
+# Selected intuitively based on WH01 results
+velocity_x_stderr: 3.2e-3
+velocity_y_stderr: 3.2e-3
+velocity_yaw_stderr: 8.5e-3

panther_description/config/WH04.yaml

@@ -6,3 +6,8 @@ inertia_y_offset: 0.0
 mesh_package: panther_description
 folder_path: meshes/WH04
 kinematics: differential
+
+# Selected intuitively based on WH01 results
+velocity_x_stderr: 1.6e-3
+velocity_y_stderr: 7.3e-4
+velocity_yaw_stderr: 4.3e-3

panther_driver/src/driver_node.py

@@ -76,6 +76,9 @@ def __init__(self, name: str) -> None:
         self._motor_torque_constant = rospy.get_param('~motor_torque_constant', 2.6149)
         self._gear_ratio = rospy.get_param('~gear_ratio', 30.08)
         self._encoder_resolution = rospy.get_param('~encoder_resolution', 400 * 4)
+        self._v_x_var = float(rospy.get_param('~velocity_x_stderr', 3.2e-3))**2
+        self._v_y_var = float(rospy.get_param('~velocity_y_stderr', 3.2e-3))**2
+        self._v_yaw_var = float(rospy.get_param('~velocity_yaw_stderr', 8.5e-3))**2
 
         self._publish_tf = rospy.get_param('~publish_tf', True)
         self._publish_odom = rospy.get_param('~publish_odometry', True)
@@ -199,7 +202,14 @@ def __init__(self, name: str) -> None:
         if self._publish_odom:
             self._odom_msg = Odometry()
             self._odom_msg.pose.covariance = [0.1 if (i % 7) == 0 else 0.0 for i in range(36)]
-            self._odom_msg.twist.covariance = [0.1 if (i % 7) == 0 else 0.0 for i in range(36)]
+
+            not_used_var = 1e6
+            self._odom_msg.twist.covariance = [self._v_x_var, 0.0, 0.0, 0.0, 0.0, 0.0,
+                                               0.0, self._v_y_var, 0.0, 0.0, 0.0, 0.0, 
+                                               0.0, 0.0, not_used_var, 0.0, 0.0, 0.0,
+                                               0.0, 0.0, 0.0, not_used_var, 0.0, 0.0, 
+                                               0.0, 0.0, 0.0, 0.0, not_used_var, 0.0, 
+                                               0.0, 0.0, 0.0, 0.0, 0.0, self._v_yaw_var]
             self._odom_msg.header.frame_id = self._odom_frame
             self._odom_msg.child_frame_id = self._base_link_frame
             self._odom_pub = rospy.Publisher('odom/wheels', Odometry, queue_size=1)