Skip to content

Latest commit

 

History

History
59 lines (45 loc) · 1.99 KB

README.md

File metadata and controls

59 lines (45 loc) · 1.99 KB
Raw

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

rosbag demo data(ROS1)

rviz2 -d src/kalman_filter_localization/rviz/ekfl_demo.rviz

ros2 launch kalman_filter_localization ekf.launch.py

ros2 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}}}' --once

ros2 bag play -s rosbag_v2 test.bag

demo
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.