Complete UAV identification setup

See merge request !30

Check for keyframes
Remove 0.5*ang_acc*dt^2
Put orientation at the end