Fused IMU and GPS

Added sampling time calculation in process.
Reduced number of tfs for efficiency.
Added trajectory generation command