aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Added tuning parameter for onGround yaw gyro variance

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-31 11:37:23 +11:00 committed by Andrew Tridgell
parent 9515f6c745
commit 2c567cd721
2 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -24,6 +24,7 @@ NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
yawVarScale(100.0f), // scale factor applied to yaw gyro errors when on ground
TASmsecMax(333), // maximum allowed interal between airspeed measurement updates TASmsecMax(333), // maximum allowed interal between airspeed measurement updates
MAGmsecMax(333), // maximum allowed interval between magnetometer measurement updates MAGmsecMax(333), // maximum allowed interval between magnetometer measurement updates
HGTmsecMax(250), // maximum interval between height measurement updates HGTmsecMax(250), // maximum interval between height measurement updates
@ -398,14 +399,8 @@ void NavEKF::CovariancePrediction()
dvz_b = states[15]; dvz_b = states[15];
daxCov = sq(dt*1.4544411e-2f); daxCov = sq(dt*1.4544411e-2f);
dayCov = sq(dt*1.4544411e-2f); dayCov = sq(dt*1.4544411e-2f);
if (!onGround) dazCov = sq(dt*1.4544411e-2f);
{ if (onGround) dazCov = dazCov * yawVarScale;
dazCov = sq(dt*1.4544411e-2f);
}
else
{
dazCov = sq(dt*1.4544411e-1f); //reduce heading drift when on ground
}
dvxCov = sq(dt*0.5f); dvxCov = sq(dt*0.5f);
dvyCov = sq(dt*0.5f); dvyCov = sq(dt*0.5f);
dvzCov = sq(dt*0.5f); dvzCov = sq(dt*0.5f);

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -220,6 +220,7 @@ private:
const float covTimeStepMax; // maximum time allowed between covariance predictions const float covTimeStepMax; // maximum time allowed between covariance predictions
const float covDelAngMax; // maximum delta angle between covariance predictions const float covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool covPredStep; // boolean set to true when a covariance prediction step has been performed
const float yawVarScale; // scale factor applied to yaw gyro errors when on ground
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed