Extented Kalman Filter for 6D pose estimation using gps, imu, magnetometer and sonar sensor. - View it on GitHub