pose_ekf
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
How to run the code
cd catkin_ws/src
git clone git@github.com: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
reference:
http://web.cs.iastate.edu/~cs577/handouts/quaternion.pdf