AP_NavEKF: fixed bug in variance constraint code