Extented Kalman Filter for 6D pose estimation using gps, imu, magnetometer and sonar sensor.
#state for kalman filter 0-3 quaternion
4-6 Px Py Pz
7-9 Vx Vy Vz
10-12 bwx bwy bwz
13-15 bax bay baz
#inertial frame: ENU
cd catkin_ws/src
git clone [email protected]:libing64/pose_ekf.git
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="pose_ekf"
roslaunch pose_ekf pose_ekf.launch
#rosbag for test
-
download the rosbag
https://drive.google.com/folderview?id=0B4hFvojO5r3scWJRVWdhSmdLd0k&usp=sharing
-
replay the rosbag
rosbag play 2016-03-09-22-11-07.bag
