Kalman Filter Localization
Kalman Filter Localization is a ros2 package of Kalman Filter Based Localization in 3D using GNSS/IMU/Odometry(Visual Odometry/Lidar Odometry).
node
ekf_localization_node
- input
/initial_pose (geometry_msgs/PoseStamed)
/gnss_pose (geometry_msgs/PoseStamed)
/imu (sensor_msgs/Imu)
/odom (nav_msgs/Odometry)
/tf(/base_link(robot frame) → /imu_link(imu frame)) - output
/curent_pose (geometry_msgs/PoseStamped)
params
| Name | Type | Default value | Description |
|---|---|---|---|
| pub_period | int | 10 | publish period[ms] |
| var_gnss_xy | double | 0.1 | variance of a gnss receiver about position xy[m^2] |
| var_gnss_z | double | 0.15 | variance of a gnss receiver about position z[m^2] |
| var_odom_xyz | double | 0.1 | variance of an odometry[m^2] |
| var_imu_w | double | 0.01 | variance of an angular velocity sensor[(deg/sec)^2] |
| var_imu_acc | double | 0.01 | variance of an accelerometer[(m/sec^2)^2] |
| use_gnss | bool | true | whether gnss is used or not |
| use_odom | bool | false | whether odom(lo/vo) is used or not |
demo
rviz2 -d src/kalman_filter_localization/rviz/ekfl_demo.rvizros2 launch kalman_filter_localization ekf.launch.pyros2 topic pub ekf_localization/initial_pose geometry_msgs/PoseStamped '{header: {stamp: {sec: 1532228824, nanosec: 55000000}, frame_id: "map"}, pose: {position: {x: 0, y: 0, z: 10}, orientation: {z: 1, w: 0}}}' --onceros2 bag play -s rosbag_v2 test.bag
blue:initial pose, red:gnss pose, green: fusion pose
references
- K Feng,"A New Quaternion-Based Kalman Filter",2017
- Joan Solà,"Quaternion kinematics for the error-state Kalman filter",2017
- Daniel Choukroun et al,"A Novel Quaternion Kalman Filter",2006
- An Improved EKF - The Error State Extended Kalman Filter
- Weikun Zhen, Sam Zeng, and Sebastian Scherer. "Robust Localization and Localizability Estimation with a Rotating Laser Scanner" , 2017.