angle/rate loop test OK
INS updating