AP_NavEKF: Scale divergence check with covariance

This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias