forked from Archive/PX4-Autopilot
EKF: initialize covariances before we reset the heading in order to preserve the yaw uncertainty
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
parent
490888285d
commit
472f2286b7
|
@ -232,6 +232,9 @@ bool Ekf::initialiseFilter()
|
|||
_state.mag_B.setZero();
|
||||
_state.wind_vel.setZero();
|
||||
|
||||
// initialise the state covariance matrix
|
||||
initialiseCovariance();
|
||||
|
||||
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
|
||||
float pitch = 0.0f;
|
||||
float roll = 0.0f;
|
||||
|
@ -276,9 +279,6 @@ bool Ekf::initialiseFilter()
|
|||
|
||||
}
|
||||
|
||||
// initialise the state covariance matrix
|
||||
initialiseCovariance();
|
||||
|
||||
// try to initialise the terrain estimator
|
||||
_terrain_initialised = initHagl();
|
||||
|
||||
|
|
Loading…
Reference in New Issue