Skip to content

Commit

Permalink
Tuned parameters for IMU & EKF
Browse files Browse the repository at this point in the history
  • Loading branch information
@AnielAlexa
AnielAlexa committed Oct 20, 2023
1 parent 3c654bd commit 8a6ca81
Show file tree
Hide file tree
Showing 3 changed files with 27 additions and 18 deletions.
8 changes: 8 additions & 0 deletions rae_camera/config/camera.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,16 @@
i_pipeline_type: rae
i_enable_ir: false
i_enable_imu: true

i_calibration_dump: false
i_pipeline_dump: false
imu:
i_acc_cov: 0.1
i_gyro_cov: 0.001
i_batch_report_threshold: 5
i_max_batch_reports: 5
i_gyro_freq: 400
i_acc_freq: 65
nn:
i_nn_config_path: /workspaces/rae_ws/src/rae-ros/rae_camera/config/yolo.json
i_disable_resize: true
Expand Down
34 changes: 17 additions & 17 deletions rae_hw/config/ekf.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ ekf_filter_node:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 25.0
frequency: 50.0

# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
Expand Down Expand Up @@ -85,9 +85,9 @@ ekf_filter_node:
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, true, false, # X , Y , Z
false, false, false, # roll , pitch ,yaw
false, false, false, # dX , dY , dZ
odom0_config: [false, false, false, # X , Y , Z
false, false,false, # roll , pitch ,yaw
true, false, false, # dX , dY , dZ
false, false, false, # droll , dpitch ,dyaw
false, false, false] # ddX , ddY , ddZ

Expand Down Expand Up @@ -124,27 +124,27 @@ ekf_filter_node:
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
#odom0_pose_rejection_threshold: 5.0
#odom0_twist_rejection_threshold: 1.0

# Further input parameter examples

imu0: imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_relative: false
imu0_queue_size: 1
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #

# # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
#imu0_remove_gravitational_acceleration: true

# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
Expand All @@ -158,7 +158,7 @@ ekf_filter_node:
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: true
use_control: false

# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
Expand Down Expand Up @@ -199,16 +199,16 @@ ekf_filter_node:
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.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.0, 0.1]

# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
Expand All @@ -220,7 +220,7 @@ ekf_filter_node:
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, 1e-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, 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,
Expand Down
3 changes: 2 additions & 1 deletion rae_hw/launch/control.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,9 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{'do_bias_estimation': True},
{'do_adaptive_gain': True},
{'orientation_stddev': 0.001},
{'use_mag': False},
{'gain_acc': 0.01},
{'gain_acc': 0.04},
{'gain_mag': 0.01},
],
remappings=[
Expand Down

0 comments on commit 8a6ca81

Please sign in to comment.