From 47778c44ea6ce1ca55217c015dcc855ff1675bdd Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:19:54 +0200 Subject: [PATCH] Ekf localization (#127) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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ń * Update panther_bringup/config/imu_config.yaml Co-authored-by: Maciej Stępień * Update panther_bringup/config/imu_config.yaml Co-authored-by: Maciej Stępień * Sugestions * Update panther_description/config/WH01.yaml Co-authored-by: Maciej Stępień * Update panther_description/config/WH02.yaml Co-authored-by: Maciej Stępień * Update panther_description/config/WH04.yaml Co-authored-by: Maciej Stępień --------- Co-authored-by: Maciej Stępień --- panther_bringup/config/ekf_config.yaml | 63 ++++++++++++++++++++++ panther_bringup/config/imu_config.yaml | 16 ++++-- panther_bringup/config/panther_common.yaml | 2 +- panther_bringup/launch/bringup.launch | 2 + panther_bringup/launch/ekf.launch | 7 +++ panther_bringup/package.xml | 1 + panther_description/config/WH01.yaml | 5 ++ panther_description/config/WH02.yaml | 5 ++ panther_description/config/WH04.yaml | 5 ++ panther_driver/src/driver_node.py | 12 ++++- 10 files changed, 112 insertions(+), 6 deletions(-) create mode 100644 panther_bringup/config/ekf_config.yaml create mode 100644 panther_bringup/launch/ekf.launch diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml new file mode 100644 index 000000000..d7e00d79b --- /dev/null +++ b/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] diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index 8a1eed524..1e51fd0a1 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -1,9 +1,16 @@ 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) @@ -11,9 +18,10 @@ PhidgetsSpatialNodelet: # 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 diff --git a/panther_bringup/config/panther_common.yaml b/panther_bringup/config/panther_common.yaml index a6a3a9ae6..b385b4969 100644 --- a/panther_bringup/config/panther_common.yaml +++ b/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 diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index 64f2f8ef0..d969af727 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -62,6 +62,8 @@ + + diff --git a/panther_bringup/launch/ekf.launch b/panther_bringup/launch/ekf.launch new file mode 100644 index 000000000..5dee16fa9 --- /dev/null +++ b/panther_bringup/launch/ekf.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index d06c5492e..fefddaaca 100644 --- a/panther_bringup/package.xml +++ b/panther_bringup/package.xml @@ -24,5 +24,6 @@ panther_msgs panther_power_control phidgets_spatial + robot_localization \ No newline at end of file diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index 0f4dc7353..3097414d8 100644 --- a/panther_description/config/WH01.yaml +++ b/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 \ No newline at end of file diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index d25f0ae71..582646916 100644 --- a/panther_description/config/WH02.yaml +++ b/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 \ No newline at end of file diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index bfc64e845..340ade38e 100644 --- a/panther_description/config/WH04.yaml +++ b/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 \ No newline at end of file diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index f0ce09ce0..7ec46f337 100755 --- a/panther_driver/src/driver_node.py +++ b/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)