AP_NavEKF : Make initial height variance consistent with baro noise

This makes sense to do because we initialise the state to the instantaneous baro reading
This commit is contained in:
Paul Riseborough 2015-03-20 16:56:28 -07:00 committed by Andrew Tridgell
parent a976e9dad2
commit fe76cb4c0b

View File

@ -3831,7 +3831,7 @@ void NavEKF::CovarianceInit()
// positions
P[7][7] = sq(15.0f);
P[8][8] = P[7][7];
P[9][9] = sq(5.0f);
P[9][9] = sq(_baroAltNoise);
// delta angle biases
P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU));
P[11][11] = P[10][10];