rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

LP-93 - zero gyro bias after temp calibration

Browse Source
This commit is contained in:
Alessio Morale 2015-08-14 14:22:18 +02:00
parent faa5d0fb4a
commit 08aaaa518b
4 changed files with 47 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp
View File

@ -39,6 +39,16 @@ void ThermalCalibration::ComputeStats(Eigen::VectorXf *samplesX, Eigen::VectorXf
*rebiasedSigma = CalibrationUtils::ComputeSigma(&rebiasedY);
}
void ThermalCalibration::ComputeBias(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *correctionPoly, float *bias)
{
Eigen::VectorXf tempBias(samplesX->rows());
OpenPilot::CalibrationUtils::ComputePoly(samplesX, correctionPoly, &tempBias);
Eigen::VectorXf rebiasedY(*samplesY);
rebiasedY.array() -= tempBias.array();
*bias = rebiasedY.array().mean();
}
bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
{
// assume the nearest reading to 20°C as the "zero bias" point
@ -91,7 +101,7 @@ bool ThermalCalibration::AccelerometerCalibration(Eigen::VectorXf samplesX, Eige
}
bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *resultPoly, float *resultBias, float *inputSigma, float *calibratedSigma)
{
Eigen::VectorXf solution(GYRO_X_POLY_DEGREE + 1);
@ -99,29 +109,31 @@ bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::V
return false;
}
result[0] = solution[1];
result[1] = solution[2];
solution[0] = 0;
resultPoly[0] = solution[1];
resultPoly[1] = solution[2];
solution[0] = 0;
ComputeStats(&temperature, &samplesX, &solution, &inputSigma[0], &calibratedSigma[0]);
ComputeBias(&temperature, &samplesX, &solution, &resultBias[0]);
solution.resize(GYRO_Y_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesY, GYRO_Y_POLY_DEGREE, solution, GYRO_Y_MAX_REL_ERROR)) {
return false;
}
result[2] = solution[1];
result[3] = solution[2];
solution[0] = 0;
resultPoly[2] = solution[1];
resultPoly[3] = solution[2];
solution[0] = 0;
ComputeStats(&temperature, &samplesY, &solution, &inputSigma[1], &calibratedSigma[1]);
ComputeBias(&temperature, &samplesY, &solution, &resultBias[1]);
solution.resize(GYRO_Z_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesZ, GYRO_Z_POLY_DEGREE, solution, GYRO_Z_MAX_REL_ERROR)) {
return false;
}
result[4] = solution[1];
result[5] = solution[2];
solution[0] = 0;
resultPoly[4] = solution[1];
resultPoly[5] = solution[2];
solution[0] = 0;
ComputeStats(&temperature, &samplesZ, &solution, &inputSigma[2], &calibratedSigma[2]);
ComputeBias(&temperature, &samplesZ, &solution, &resultBias[2]);
return (inputSigma[0] > calibratedSigma[0]) && (inputSigma[1] > calibratedSigma[1]) && (inputSigma[2] > calibratedSigma[2]);
}

17
ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.h
View File

@ -55,12 +55,21 @@ public:
* @param samplesX X values for input samples
* @param samplesY Y values for input samples
* @param correctionPoly coefficients for the correction polynomial
* @param degrees Degree of the correction polynomial
* @param initialSigma Standard deviation calculated over input samples
* @param rebiasedSigma Standard deviation calculated over calibrated samples
*/
static void ComputeStats(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *correctionPoly, float *initialSigma, float *rebiasedSigma);
/**
* @brief ComputeStats
* @param samplesX X values for input samples
* @param samplesY Y values for input samples
* @param correctionPoly coefficients for the correction polynomial
* @param degrees Degree of the correction polynomial
* @param bias Calculated mean bias over the temp-compensated samples
*/
static void ComputeBias(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *correctionPoly, float *bias);
/**
* @brief produce the calibration polinomial coefficients from pressure and temperature samples
* @param pressure Pressure samples
@ -89,13 +98,13 @@ public:
* @param samplesY
* @param samplesZ
* @param temperature
* @param result a float[4] array containing value to populate calibration settings (x,y,z1, z2)
* @param resultPoly a float[4] array containing value to populate calibration settings (x,y,z1, z2)
* @param resultBias a float[3] array containing value to populate bias settings (x,y,z)
* @param inputSigma a float[3] array populated with input sample variance
* @param CalibratedSigma float[3] array populated with calibrated data variance
* @return
*/
static bool GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma);
static bool GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *resultPoly, float *resultBias, float *inputSigma, float *calibratedSigma);
private:
static void copyToArray(float *result, Eigen::VectorXf solution, int elements);

14
ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp
View File

@ -338,7 +338,8 @@ void ThermalCalibrationHelper::calculate()
datat[x] = m_gyroSamples[x].temperature;
}
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro,
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat,
m_results.gyro, m_results.gyroBias,
m_results.gyroInSigma, m_results.gyroOutSigma);
if (m_results.gyroCalibrated) {
addInstructions(tr("Gyro is calibrated."));
@ -372,10 +373,10 @@ void ThermalCalibrationHelper::calculate()
str += QStringLiteral("INFO::Baro cal {%1, %2, %3, %4}; initial variance: %5; Calibrated variance %6")
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + "\n";
str += QStringLiteral("INFO::Gyro cal x{%1, %2} y{%3, %4} z{%5, %6}; initial variance: {%7, %8, %9}; Calibrated variance {%10, %11, %12}")
.arg(m_results.gyro[0]).arg(m_results.gyro[1])
.arg(m_results.gyro[2]).arg(m_results.gyro[3])
.arg(m_results.gyro[4]).arg(m_results.gyro[5])
str += QStringLiteral("INFO::Gyro cal x{%1, %2, %3} y{%4, %5, %6} z{%7, %8, %9}; initial variance: {%10, %11, %12}; Calibrated variance {%13, %14, %15}")
.arg(m_results.gyroBias[0]).arg(m_results.gyro[0]).arg(m_results.gyro[1])
.arg(m_results.gyroBias[1]).arg(m_results.gyro[2]).arg(m_results.gyro[3])
.arg(m_results.gyroBias[2]).arg(m_results.gyro[4]).arg(m_results.gyro[5])
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + "\n";
str += QStringLiteral("INFO::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
@ -538,6 +539,9 @@ void ThermalCalibrationHelper::copyResultToSettings()
data.gyro_temp_coeff[3] = m_results.gyro[3];
data.gyro_temp_coeff[4] = m_results.gyro[4];
data.gyro_temp_coeff[5] = m_results.gyro[5];
data.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += m_results.gyroBias[0];
data.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += m_results.gyroBias[1];
data.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += m_results.gyroBias[2];
}
if (m_results.accelCalibrated) {

1
ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h
View File

@ -73,6 +73,7 @@ typedef struct {
float accel[3];
bool gyroCalibrated;
float gyro[6];
float gyroBias[3];
float baroInSigma;
float baroOutSigma;