Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Add public method to reset gyro bias states

Browse Source
This commit is contained in:
priseborough 2014-10-29 12:21:42 +11:00 committed by Randy Mackay
parent 2c07299c04
commit fb14da7a4b
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2813,6 +2813,18 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
gyroBias = state.gyro_bias / dtIMU;
}
// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
void NavEKF::resetGyroBias(void)
{
state.gyro_bias.zero();
zeroRows(P,10,12);
zeroCols(P,10,12);
P[10][10] = sq(radians(0.1f * dtIMU));
P[11][11] = P[10][10];
P[12][12] = P[10][10];
}
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2
void NavEKF::getAccelBias(Vector3f &accelBias) const
{

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -105,6 +105,9 @@ public:
// return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const;
// reset body axis gyro bias estimates
void resetGyroBias(void);
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2
void getAccelBias(Vector3f &accelBias) const;