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

Add functionality for UAV

Closes #12 and #22

See merge request !29

This helps handling dynamic sensor params because it creates the dynamic state blocks automatically if required.