Skip to content

Kálmán filter based ROS 1 / ROS 2 node (geometry_msgs/pose, sensor_msgs/imu)

License

Notifications You must be signed in to change notification settings

jkk-research/kalman_pos

Folders and files

NameName
Last commit message
Last commit date

Latest commit

7e8d708 · Dec 11, 2024

History

85 Commits
Nov 28, 2024
Dec 11, 2024
Dec 11, 2024
Jun 7, 2022
Aug 22, 2024
Nov 28, 2024
May 10, 2024
Nov 21, 2024
May 23, 2024

Repository files navigation

kalman_pos ROS 2 package

Kálmán filter based ROS 2 node (geometry_msgs/PoseStamped, sensor_msgs/Imu)

Static Badge

Build

IMU transformer is a dependency, it might be needed if the IMU is not in the center of gravity (COG)

sudo apt install ros-$ROS_DISTRO-imu-transformer
cd ~/ros2_ws/src 

Caution

If you want the full compatiblity with our paper, please use release version 0.3.0. Newer releases use modified paramters. Either clone this version:

git clone https://github.com/jkk-research/kalman_pos --branch v.0.3.0

Or clone the latest version:

git clone https://github.com/jkk-research/kalman_pos
cd ~/ros2_ws
colcon build --symlink-install --packages-select kalman_pos

ROS publications / subscriptions

The main node is kalman_pos_node, also there is a vehicle_status_convert node for converting the vehicle status message to the required format.

Loading
flowchart LR

A[ /imu<br/>sensor_msgs/Imu] --> F(kalman_pos)
B[ /current_pose<br/>geometry_msgs/PoseStamped] --> F
C[ /vehicle_status<br/>geometry_msgs/Twist] --> F
D[ /nova_fix<br/>sensor_msgs/NavSatFix] --> F
E[ /duro_status<br/>std_msgs/String] --> F
F -->  G[ /estimated_pose_cog<br/>geometry_msgs/PoseStamped]
F -->  H[ /estimated_pose_baselink<br/>geometry_msgs/PoseStamped]
F -->  I[ /distance<br/>std_msgs/Float32]
F -->  J[ /estimated_trav_dist_est_pos<br/>std_msgs/Float32]
F -->  K[ /estimation_accuracy<br/>visualization_msgs/Marker]

V1(vehicle_status_convert <br> -optional-) -.-> C
V3[ /vehicle_speed <br/> std_msgs/Float32] --> V1
V4[ /vehicle_steering <br/> std_msgs/Float32] --> V1


classDef light fill:#34aec5,stroke:#152742,stroke-width:2px,color:#152742  
classDef dark fill:#152742,stroke:#34aec5,stroke-width:2px,color:#34aec5
classDef white fill:#ffffff,stroke:#152742,stroke-width:2px,color:#152742
classDef red fill:#ef4638,stroke:#152742,stroke-width:2px,color:#fff
classDef dashed fill:#ef4638,stroke:#152742,stroke-width:3px,stroke-dasharray:5,5,color:#fff

class F red
class V1 dashed
class A,B,C,D,E,G,H,I,J,K,V3,V4 light

Run

Don't forget to source before ROS commands.
source ~/ros2_ws/install/setup.bash
ros2 launch kalman_pos kalman_pos_node.launch.py

Parameters

  • pose_topic
    • type: string
    • default value: gps/duro/current_pose
    • description: the name of the GNSS position topic (subscriber, geometry_msgs::PoseStamped)
  • vehicle_status_topic
    • type: string
    • default value: vehicle_status
    • description: the name of the vehicle status topic (subscriber, autoware_msgs::VehicleStatus)
  • nav_sat_fix_topic
    • type: string
    • default value: gps/nova/fix
    • description: the name of the Novatel NavSatFix topic (relevant only for Novatel GNSS) (subscriber, sensor_msgs::NavSatFix)
  • imu_topic
    • type: string
    • default value: imu/data
    • description: the name of the IMU data topic (subscriber, sensor_msgs::Imu)
  • est_cog_topic
    • type: string
    • default value: estimated_pose_cog
    • description: the name of the estimated position topic (transformed into the CoG) (Publisher, geometry_msgs::PoseStamped)
  • est_trav_distance_odom_topic
    • type: string
    • default value: distance
    • description: the name of the estimated traveled distance position topic (calculation is based on the odemetry) (Publisher, std_msgs::Float32)
  • est_trav_distance_est_pos_topic
    • type: string
    • default value: estimated_trav_dist_est_pos
    • description: the name of the estimated traveled distance position topic (calculation is based on the estimated position) (Publisher, std_msgs::Float32)
  • est_baselink_topic
    • type: string
    • default value: estimated_pose_baselink
    • description: the name of the estimated position topic (transformed into the baselink) (Publisher, geometry_msgs::PoseStamped)
  • est_accuracy_topic
    • type: string
    • default value: estimation_accuracy
    • description: the name of the estimattion accuracy marker topic (Publisher, visualization_msgs::Marker)
  • loop_rate_hz
    • type: int
    • default value: 60
    • description: the ROS loop rate of the node (in Hz)
  • estimation_method
    • type: int
    • default value: 8
    • description: the estimation method
      • 0: Kinematic model with EKF and without GNSS position; initial GNSS based orientation estimation disabled
      • 1: Kinematic + dynamic model without EKF and GNSS position; initial GNSS based orientation estimation disabled
      • 2: Kinematic model without EKF and GNSS position; initial GNSS based orientation estimation enabled
      • 3: Kinematic + dynamic model without EKF and GNSS position; initial GNSS based orientation estimation enabled
      • 4: Currently not used
      • 5: Kinematic model with EKF and without GNSS; initial GNSS based orientation estimation disabled
      • 6: Kinematic + dynamic model with EKF and without GNSS position; initial GNSS based orientation estimation disabled (USE THIS AS DEFAULT FOR ESTIMATION WITHOUT GNSS)
      • 7: Kinematic model with EKF and without GNSS position; initial GNSS based orientation estimation enabled
      • 8: Kinematic + dynamic model with EKF and without GNSS position; initial GNSS based orientation estimation enabled
      • 9: Currently used for debugging
      • 10: Automatically switch between the different estimation methods
  • dynamic_time_calc
    • type: bool
    • default value: true
    • description: true if the time difference is calculated between each step, false if fix value is used (1/lROSLoopRate_cl_hz)
  • kinematic_model_max_speed
    • type: double
    • default value: 0.3
    • description: the speed where the algorithm switch to the dynamic model from the kinematic model
  • do_not_wait_for_gnss_msgs
    • type: bool
    • default value: true
    • description: true if the algrithm in not waiting for the first positon message (use this for the algorithms without GNSS position and orientation estimation)
  • msg_timeout
    • type: double
    • default value: 2000
    • description: timeout for vehicle status and IMU message, if these messages does not arrive until timeout then the estimation will stop [ms]
  • vehicle_param_c1
    • type: double
    • default value: 3000
    • description: front wheel cornering stiffness (for single track model) [N/rad]
  • vehicle_param_c2
    • type: double
    • default value: 3000
    • description: rear wheel cornering stiffness (for single track model) [N/rad]
  • vehicle_param_m
    • type: double
    • default value: 180
    • description: mass of the vehicle [kg]
  • vehicle_param_jz
    • type: double
    • default value: 270
    • description: moment of inertia (z axle) [kg*m2]
  • vehicle_param_l1
    • type: double
    • default value: 0.324
    • description: CoG distance from the front axle [m]
  • vehicle_param_l1
    • type: double
    • default value: 0.976
    • description: CoG distance from the rear axle [m]
  • vehicle_param_swr
    • type: double
    • default value: 1.0
    • description: Steering wheel ratio

Rosbag

Download: jkk-research.github.io/dataset

Direct download of zipped MCAPs: download zip (~15 MB)

Make sure you have unzip (sudo apt-get install unzip) and:

unzip jkkds02.zip
ros2 bag play nissan_zala_50_zeg_1_0.mcap

This example bag (mcap) file can be used with:

ros2 launch kalman_pos kalman_pos_nissan1.launch.py

Cite & paper

If you use any of this code please consider citing the paper:

@Article{doi:10.1177/09544070241266281,
    title = {Localization robustness improvement for an autonomous race car using multiple extended Kalman filters},
    author = {Krisztián Enisz and István Szalay and Ernő Horváth},
    journal = {Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering},
    volume = {0},
    url = {https://doi.org/10.1177/09544070241266281},
    eprint = {https://doi.org/10.1177/09544070241266281},
    doi = {10.1177/09544070241266281}
}