dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix gyro offset calculation

This commit is contained in:
Lorenz Meier 2015-03-15 10:39:21 +01:00
parent d28e4ed7a7
commit aceca6b2a9
2 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -784,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
/* lazily publish the attitude only once available */
if (_att_pub > 0) {
@ -1069,7 +1069,7 @@ void AttitudePositionEstimatorEKF::print_status()
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
(double)_baro_gps_offset);

20
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -210,7 +210,7 @@ void AttPosEKF::InitialiseParameters()
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dAngBiasSigma = 5.0e-7;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
@ -414,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt)
// calculate covariance prediction process noise
for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
// scale gyro bias noise when on ground to allow for faster bias estimation
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
float gyroBiasScale = (_onGround) ? 2.0f : 1.0f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale;
processNoise[13] = dVelBiasSigma;
if (!inhibitWindStates) {
for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
@ -2706,6 +2707,11 @@ void AttPosEKF::ConstrainStates()
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
// Constrain dtIMUfilt
if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
dtIMUfilt = dtIMU;
}
// Constrain quaternion
for (size_t i = 0; i <= 3; i++) {
states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
@ -2727,11 +2733,11 @@ void AttPosEKF::ConstrainStates()
// Angle bias limit - set to 8 degrees / sec
for (size_t i = 10; i <= 12; i++) {
states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt);
}
// Constrain delta velocity bias
states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt);
// Wind velocity limits - assume 120 m/s max velocity
for (size_t i = 14; i <= 15; i++) {
@ -2795,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged()
// Protect against division by zero
if (delta_len > 0.0f) {
float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f);
delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt;
}
bool diverged = (delta_len_scaled > 1.0f);