mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
OP-1150 UI for thermal calibration: calculate variance before and after calibration as sanity check
This commit is contained in:
parent
9c1c556c9c
commit
afb7ba7ea4
@ -27,19 +27,27 @@
|
||||
*/
|
||||
|
||||
#include "calibrationutils.h"
|
||||
#include <math.h>
|
||||
using namespace Eigen;
|
||||
namespace OpenPilot {
|
||||
float CalibrationUtils::ComputeSigma(Eigen::VectorXf *samplesY)
|
||||
{
|
||||
Eigen::ArrayXd tmpd = samplesY->cast<double>().array();
|
||||
double mean = tmpd.mean();
|
||||
return (float) sqrt((tmpd - mean).square().mean());
|
||||
}
|
||||
|
||||
/*
|
||||
* The following ellipsoid calibration code is based on RazorImu calibration samples that can be found here:
|
||||
* https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration
|
||||
*/
|
||||
bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, float nominalRange, EllipsoidCalibrationResult *result)
|
||||
bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result)
|
||||
{
|
||||
Eigen::VectorXf radii;
|
||||
Eigen::Vector3f center;
|
||||
Eigen::MatrixXf evecs;
|
||||
|
||||
EllipsoidFit(&samplesX, &samplesY, &samplesZ, ¢er, &radii, &evecs);
|
||||
EllipsoidFit(samplesX, samplesY, samplesZ, ¢er, &radii, &evecs);
|
||||
|
||||
result->Scale.setZero();
|
||||
|
||||
@ -58,12 +66,12 @@ bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf samplesX, Eigen::Vec
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CalibrationUtils::PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError)
|
||||
bool CalibrationUtils::PolynomialCalibration(VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError)
|
||||
{
|
||||
int samples = samplesX.rows();
|
||||
int samples = samplesX->rows();
|
||||
// perform internal calculation using doubles
|
||||
VectorXd doubleX = samplesX.cast<double>();
|
||||
VectorXd doubleY = samplesY.cast<double>();
|
||||
VectorXd doubleX = samplesX->cast<double>();
|
||||
VectorXd doubleY = samplesY->cast<double>();
|
||||
Eigen::MatrixXd x(samples, degree + 1);
|
||||
|
||||
x.setOnes(samples, degree + 1);
|
||||
@ -86,6 +94,14 @@ bool CalibrationUtils::PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::Ve
|
||||
return relativeError < maxRelativeError;
|
||||
}
|
||||
|
||||
void CalibrationUtils::ComputePoly(VectorXf *samplesX, Eigen::VectorXf *polynomial, VectorXf *polyY)
|
||||
{
|
||||
polyY->array().fill(polynomial->coeff(0));
|
||||
for(int i = 1; i < polynomial->rows(); i++){
|
||||
polyY->array() += samplesX->array().pow(i) * polynomial->coeff(i);
|
||||
}
|
||||
}
|
||||
|
||||
/* C++ Implementation of Yury Petrov's ellipsoid fit algorithm
|
||||
* Following is the origial code and its license from which this implementation is derived
|
||||
*
|
||||
|
@ -42,9 +42,11 @@ public:
|
||||
Eigen::Vector3f Scale;
|
||||
Eigen::Vector3f Bias;
|
||||
};
|
||||
static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result);
|
||||
static bool PolynomialCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
|
||||
|
||||
static bool EllipsoidCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, float nominalRange, EllipsoidCalibrationResult *result);
|
||||
static bool PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
|
||||
static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY);
|
||||
static float ComputeSigma(Eigen::VectorXf *samplesY);
|
||||
|
||||
private:
|
||||
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||
|
@ -28,7 +28,18 @@
|
||||
#include "QDebug"
|
||||
#include "thermalcalibration.h"
|
||||
using namespace OpenPilot;
|
||||
bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result)
|
||||
|
||||
void ThermalCalibration::ComputeStats(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *correctionPoly, float *initialSigma, float *rebiasedSigma)
|
||||
{
|
||||
*initialSigma = CalibrationUtils::ComputeSigma(samplesY);
|
||||
Eigen::VectorXf bias(samplesX->rows());
|
||||
OpenPilot::CalibrationUtils::ComputePoly(samplesX, correctionPoly, &bias);
|
||||
Eigen::VectorXf rebiasedY(*samplesY);
|
||||
rebiasedY.array() -= bias.array();
|
||||
*rebiasedSigma = CalibrationUtils::ComputeSigma(&rebiasedY);
|
||||
}
|
||||
|
||||
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
|
||||
int index20deg = searchReferenceValue(20.0f, temperature);
|
||||
@ -39,64 +50,79 @@ bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::V
|
||||
qDebug() << "Rebiased zero is " << pressure[index20deg];
|
||||
|
||||
Eigen::VectorXf solution(BARO_PRESSURE_POLY_DEGREE + 1);
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, pressure, BARO_PRESSURE_POLY_DEGREE, solution, BARO_PRESSURE_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &pressure, BARO_PRESSURE_POLY_DEGREE, solution, BARO_PRESSURE_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
std::cout << "Baro calibration " << solution << std::endl;
|
||||
copyToArray(result, solution, BARO_PRESSURE_POLY_DEGREE + 1);
|
||||
return true;
|
||||
ComputeStats(&temperature, &pressure, &solution, inputSigma, calibratedSigma);
|
||||
return (*calibratedSigma) < (*inputSigma);
|
||||
}
|
||||
|
||||
bool ThermalCalibration::AccelerometerCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result)
|
||||
bool ThermalCalibration::AccelerometerCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
|
||||
{
|
||||
Eigen::VectorXf solution(ACCEL_X_POLY_DEGREE + 1);
|
||||
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesX, ACCEL_X_POLY_DEGREE, solution, ACCEL_X_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesX, ACCEL_X_POLY_DEGREE, solution, ACCEL_X_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[0] = solution[1];
|
||||
|
||||
solution[0] = 0;
|
||||
ComputeStats(&temperature, &samplesX, &solution, &inputSigma[0], &calibratedSigma[0]);
|
||||
|
||||
solution.resize(ACCEL_Y_POLY_DEGREE + 1);
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesY, ACCEL_Y_POLY_DEGREE, solution, ACCEL_Y_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesY, ACCEL_Y_POLY_DEGREE, solution, ACCEL_Y_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[1] = solution[1];
|
||||
|
||||
solution[0] = 0;
|
||||
ComputeStats(&temperature, &samplesY, &solution, &inputSigma[1], &calibratedSigma[1]);
|
||||
|
||||
solution.resize(ACCEL_Z_POLY_DEGREE + 1);
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, ACCEL_Z_POLY_DEGREE, solution, ACCEL_Z_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesZ, ACCEL_Z_POLY_DEGREE, solution, ACCEL_Z_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[2] = solution[1];
|
||||
|
||||
return true;
|
||||
solution[0] = 0;
|
||||
ComputeStats(&temperature, &samplesZ, &solution, &inputSigma[2], &calibratedSigma[2]);
|
||||
return (inputSigma[0] > calibratedSigma[0]) && (inputSigma[1] > calibratedSigma[1]) && (inputSigma[2] > calibratedSigma[2]);
|
||||
}
|
||||
|
||||
bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result)
|
||||
|
||||
|
||||
bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
|
||||
{
|
||||
Eigen::VectorXf solution(GYRO_X_POLY_DEGREE + 1);
|
||||
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesX, GYRO_X_POLY_DEGREE, solution, GYRO_X_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesX, GYRO_X_POLY_DEGREE, solution, GYRO_X_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[0] = solution[1];
|
||||
std::cout << solution << std::endl << std::endl;
|
||||
solution[0] = 0;
|
||||
ComputeStats(&temperature, &samplesX, &solution, &inputSigma[0], &calibratedSigma[0]);
|
||||
|
||||
|
||||
solution.resize(GYRO_Y_POLY_DEGREE + 1);
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesY, GYRO_Y_POLY_DEGREE, solution, GYRO_Y_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesY, GYRO_Y_POLY_DEGREE, solution, GYRO_Y_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[1] = solution[1];
|
||||
std::cout << solution << std::endl << std::endl;
|
||||
solution[0] = 0;
|
||||
ComputeStats(&temperature, &samplesY, &solution, &inputSigma[1], &calibratedSigma[1]);
|
||||
|
||||
solution.resize(GYRO_Z_POLY_DEGREE + 1);
|
||||
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, GYRO_Z_POLY_DEGREE, solution, GYRO_Z_MAX_REL_ERROR)) {
|
||||
if (!CalibrationUtils::PolynomialCalibration(&temperature, &samplesZ, GYRO_Z_POLY_DEGREE, solution, GYRO_Z_MAX_REL_ERROR)) {
|
||||
return false;
|
||||
}
|
||||
result[2] = solution[1];
|
||||
result[3] = solution[2];
|
||||
solution[0] = 0;
|
||||
std::cout << solution << std::endl;
|
||||
|
||||
return true;
|
||||
ComputeStats(&temperature, &samplesZ, &solution, &inputSigma[2], &calibratedSigma[2]);
|
||||
return (inputSigma[0] < calibratedSigma[0]) && (inputSigma[1] < calibratedSigma[1]) && (inputSigma[2] < calibratedSigma[2]);
|
||||
}
|
||||
|
||||
void ThermalCalibration::copyToArray(float *result, Eigen::VectorXf solution, int elements)
|
||||
|
@ -49,23 +49,40 @@ class ThermalCalibration {
|
||||
static const double GYRO_Y_MAX_REL_ERROR = 1E-6f;
|
||||
static const double GYRO_Z_MAX_REL_ERROR = 1E-6f;
|
||||
public:
|
||||
|
||||
/**
|
||||
* @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 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 produce the calibration polinomial coefficients from pressure and temperature samples
|
||||
* @param pressure Pressure samples
|
||||
* @param temperature Temperature samples
|
||||
* @param result Polinomial coefficients to be sent to board (x0, x1, x2, x3)
|
||||
* @param inputSigma a float populated with input sample variance
|
||||
* @param CalibratedSigma float populated with calibrated data variance
|
||||
* @return
|
||||
*/
|
||||
static bool BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result);
|
||||
static bool BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma);
|
||||
|
||||
/**
|
||||
* @brief AccelerometerCalibration produce the calibration polinomial coefficients from accelerometer axis and temperature samples
|
||||
* @param pressure
|
||||
* @param temperature
|
||||
* @param result a float[3] array containing value to populate calibration 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 AccelerometerCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result);
|
||||
static bool AccelerometerCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma);
|
||||
|
||||
/**
|
||||
* @brief GyroscopeCalibration produce the calibration polinomial coefficients from gyroscopes axis and temperature samples
|
||||
* @param samplesX
|
||||
@ -73,9 +90,11 @@ public:
|
||||
* @param samplesZ
|
||||
* @param temperature
|
||||
* @param result a float[4] array containing value to populate calibration settings (x,y,z1, z2)
|
||||
* @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);
|
||||
static bool GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, Eigen::VectorXf samplesZ, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma);
|
||||
|
||||
|
||||
private:
|
||||
|
@ -271,7 +271,7 @@ void ThermalCalibrationHelper::calculate()
|
||||
datat[x] = m_baroSamples[x].Temperature;
|
||||
}
|
||||
|
||||
m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro);
|
||||
m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro, &m_results.baroInSigma, &m_results.baroOutSigma);
|
||||
|
||||
setProcessPercentage(processPercentage() + 2);
|
||||
count = m_gyroSamples.count();
|
||||
@ -287,7 +287,7 @@ 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.gyroInSigma, m_results.gyroOutSigma);
|
||||
|
||||
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
|
||||
/*
|
||||
@ -308,6 +308,19 @@ void ThermalCalibrationHelper::calculate()
|
||||
m_results.accelCalibrated = ThermalCalibration::AccelerometerCalibration(datax, datay, dataz, datat, m_results.accel);
|
||||
*/
|
||||
m_results.accelCalibrated = false;
|
||||
|
||||
qDebug() << QStringLiteral("Calibration results");
|
||||
qDebug() << QStringLiteral("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);
|
||||
qDebug() << QStringLiteral("Gyro cal x{%1} y{%2} z{%3, %4}; initial variance: {%5, %6, %7}; Calibrated variance {%8, %9, %10}")
|
||||
.arg(m_results.gyro[0]).arg(m_results.gyro[1]).arg(m_results.gyro[2]).arg(m_results.baro[3])
|
||||
.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]);
|
||||
qDebug() << QStringLiteral("Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
|
||||
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
|
||||
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
|
||||
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]);
|
||||
copyResultToSettings();
|
||||
emit calculationCompleted();
|
||||
}
|
||||
|
@ -54,6 +54,7 @@ typedef struct {
|
||||
// this is not needed for revo, but should for CC/CC3D
|
||||
// AccelGyroSettings::DataFields accelGyroSettings;
|
||||
RevoSettings::DataFields revoSettings;
|
||||
AccelGyroSettings::DataFields accelGyroSettings;
|
||||
UAVObject::Metadata gyroSensorMeta;
|
||||
UAVObject::Metadata accelSensorMeta;
|
||||
UAVObject::Metadata baroensorMeta;
|
||||
@ -67,6 +68,15 @@ typedef struct {
|
||||
float accel[3];
|
||||
bool gyroCalibrated;
|
||||
float gyro[4];
|
||||
|
||||
float baroInSigma;
|
||||
float baroOutSigma;
|
||||
|
||||
float accelInSigma[3];
|
||||
float accelOutSigma[3];
|
||||
|
||||
float gyroInSigma[3];
|
||||
float gyroOutSigma[3];
|
||||
} thermalCalibrationResults;
|
||||
class ThermalCalibrationHelper : public QObject {
|
||||
Q_OBJECT
|
||||
|
Loading…
x
Reference in New Issue
Block a user