EKF filter fix

These changes address bad re-initialization after baro spikes. They should allow the filter to recover faster and consistently.