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

1. download the rosbag

   https://drive.google.com/folderview?id=0B4hFvojO5r3scWJRVWdhSmdLd0k&usp=sharing

2. replay the rosbag

   rosbag play 2016-03-09-22-11-07.bag

http://web.cs.iastate.edu/~cs577/handouts/quaternion.pdf