EKF3 large position offset after startup using UWB beacons

@priseborough can you take a look at this?