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.

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

Kalman Filter #234

Open
wants to merge 18 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
Fixed timestamps. Wip.
  • Loading branch information
VladimirVincan committed Dec 12, 2022

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
commit b04943dd9f972daba5e97d8d6a55ece3818d1a12
64 changes: 30 additions & 34 deletions mep3_localization/config/ekf.yaml
Original file line number Diff line number Diff line change
@@ -23,7 +23,7 @@ ekf_filter_node:
# unspecified.
transform_time_offset: 0.0

# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0

@@ -70,14 +70,14 @@ ekf_filter_node:
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: map # Defaults to the value of odom_frame if unspecified
world_frame: map # Defaults to the value of odom_frame if unspecified

# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
# odom0: example/odom
odom0: /big/odom

# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
@@ -86,20 +86,20 @@ 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,
# false, false, false,
# false, false, false,
# false, false, true,
# false, false, false]
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]

# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
# odom0_queue_size: 2
odom0_queue_size: 2

# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
# odom0_nodelay: false
odom0_nodelay: false

# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
@@ -109,36 +109,20 @@ ekf_filter_node:
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
# odom0_differential: false
odom0_differential: false

# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
# odom0_relative: false
odom0_relative: false

# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# 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

# Odometry big
# Input parameters are vx and vyaw
odom0: /big/odom
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]
odom0_differential: false
odom0_relative: false
odom0_queue_size: 1
odom0_nodelay: true
# ArUco pose big
pose0: /camera/aruco_5
pose0_config: [true, true, true,
true, true, true,
@@ -147,8 +131,20 @@ ekf_filter_node:
false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 1
pose0_nodelay: true
pose0_queue_size: 5
pose0_rejection_threshold: 2.0 # Note the difference in parameter name
pose0_nodelay: false

# twist0: /big/cmd_vel
# twist0_config: [false, false, false,
# false, false, false,
# true, false, false,
# false, false, true,
# false, false, false]
# twist0_queue_size: 3
# twist0_rejection_threshold: 2.0
# twist0_nodelay: false


# [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.
@@ -175,15 +171,15 @@ ekf_filter_node:
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [5.0, 0.0, 0.0, 0.0, 0.0, 12.0]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [5.0, 0.0, 0.0, 0.0, 0.0, 12.0]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [4.0, 0.0, 0.0, 0.0, 0.0, 10.0]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [4.0, 0.0, 0.0, 0.0, 0.0, 10.0]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
15 changes: 12 additions & 3 deletions mep3_localization/launch/localization_launch.py
Original file line number Diff line number Diff line change
@@ -76,17 +76,26 @@ def generate_launch_description():
output='screen', # debugging
emulate_tty=True, # debugging
parameters=[
{'debug': debug}
{
'use_sim_time': use_sim_time,
'debug': debug
}
]
),
# Start robot localization using an Extended Kalman filter
Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
namespace='big',
output='screen',
ros_arguments=['--log-level', 'info'],
emulate_tty=True,
parameters=[robot_localization_file_path,
{'use_sim_time': use_sim_time}])

{'use_sim_time': use_sim_time}],
remappings=[
('/tf_static', 'tf_static'),
('/tf', 'tf')
],
)
])
5 changes: 3 additions & 2 deletions mep3_simulation/launch/simulation_launch.py
Original file line number Diff line number Diff line change
@@ -46,6 +46,7 @@ def generate_launch_description():
emulate_tty=True, # debugging
parameters=[
{
'use_sim_time': True,
'robot_description': robot_description_big
},
controller_params_file_big
@@ -69,6 +70,7 @@ def generate_launch_description():
emulate_tty=True, # debugging
parameters=[
{
'use_sim_time': True,
'robot_description': robot_description_small
},
controller_params_file_small
@@ -92,8 +94,7 @@ def generate_launch_description():
output='screen', # debugging
emulate_tty=True, # debugging
parameters=[{
'robot_description': camera_description
}, {
'robot_description': camera_description,
'use_sim_time': True
}],
ros_arguments=['--log-level', 'warn'],