Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ekf optimalization #240

Merged
merged 13 commits into from
Mar 22, 2024
Merged

Ekf optimalization #240

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
bc38a3a
Update covariance
rafal-gorecki Mar 7, 2024
0f6e0da
EKF config
rafal-gorecki Mar 8, 2024
f24e3eb
fix controller topic
rafal-gorecki Mar 8, 2024
aa79987
fix imu topic
rafal-gorecki Mar 8, 2024
06f4562
Change relative to abs speed frame
rafal-gorecki Mar 19, 2024
922253a
update covariances
rafal-gorecki Mar 19, 2024
29fa4f8
Add comments
rafal-gorecki Mar 19, 2024
ab2533c
Update panther_controller/config/WH04_controller.yaml
rafal-gorecki Mar 21, 2024
dfe6475
Update panther_controller/config/WH04_controller.yaml
rafal-gorecki Mar 21, 2024
c812742
Merge branch 'ros2-devel' into ros2-ekf-optimalization
rafal-gorecki Mar 22, 2024
3918b6f
Update panther_bringup/config/ekf.yaml
rafal-gorecki Mar 22, 2024
b402b84
Add initial ekf setting
rafal-gorecki Mar 22, 2024
8fef167
fix
rafal-gorecki Mar 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 35 additions & 25 deletions panther_bringup/config/ekf.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,11 @@
# 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
Expand All @@ -16,49 +16,59 @@
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
control_timeout: 0.5
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

# 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]

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,
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,
Expand Down
11 changes: 8 additions & 3 deletions panther_controller/config/WH01_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: [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
# 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:
Expand All @@ -35,7 +40,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: [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
Expand Down
11 changes: 8 additions & 3 deletions panther_controller/config/WH02_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: [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
# 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:
Expand All @@ -36,7 +41,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.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
Expand Down
11 changes: 8 additions & 3 deletions panther_controller/config/WH04_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: [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
# 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:
Expand All @@ -36,7 +41,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: [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
Expand Down
17 changes: 10 additions & 7 deletions panther_description/urdf/gazebo.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,15 @@
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>
<remapping>/imu_broadcaster/imu:=imu/data</remapping>
</ros>
</plugin>
</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">
Expand All @@ -66,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">
Expand All @@ -81,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>
Expand All @@ -92,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>
Expand Down
13 changes: 8 additions & 5 deletions panther_hardware_interfaces/src/roboteq_data_converters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down