From 08aaaa518b50d9822d961c3ddd11b34784416aec Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 14 Aug 2015 14:22:18 +0200 Subject: [PATCH] LP-93 - zero gyro bias after temp calibration --- .../thermal/thermalcalibration.cpp | 36 ++++++++++++------- .../calibration/thermal/thermalcalibration.h | 17 ++++++--- .../thermal/thermalcalibrationhelper.cpp | 14 +++++--- .../thermal/thermalcalibrationhelper.h | 1 + 4 files changed, 47 insertions(+), 21 deletions(-) diff --git a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp index c54db2bd5..4c9ba00bb 100644 --- a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp +++ b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp @@ -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]); } diff --git a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.h b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.h index 87dac1dce..1589226f3 100644 --- a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.h +++ b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibration.h @@ -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); diff --git a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp index d7997251e..d49b33d84 100644 --- a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp +++ b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp @@ -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) { diff --git a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h index 0e0136606..60a61d8bc 100644 --- a/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h +++ b/ground/gcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h @@ -73,6 +73,7 @@ typedef struct { float accel[3]; bool gyroCalibrated; float gyro[6]; + float gyroBias[3]; float baroInSigma; float baroOutSigma;