rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

OP-1150 UI for thermal calibration: calculate variance before and after calibration as sanity check

Browse Source
This commit is contained in:
Alessio Morale 2014-01-27 00:24:53 +01:00
parent 9c1c556c9c
commit afb7ba7ea4
6 changed files with 115 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp
View File

@ -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, &center, &radii, &evecs);
EllipsoidFit(samplesX, samplesY, samplesZ, &center, &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
*

6
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h
View File

@ -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,

58
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibration.cpp
View File

@ -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)

25
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibration.h
View File

@ -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:

17
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp
View File

@ -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();
}

10
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h
View File

@ -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