Add gimbal controller and update navigation filter