rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1150 UI for thermal calibration: Calculation relative error checking

Browse Source
This commit is contained in:
Alessio Morale 2014-01-18 13:46:05 +01:00
parent a82873f7ba
commit 3a5213ac9f
4 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,7 +58,7 @@ 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)
bool CalibrationUtils::PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError)
{
int samples = samplesX.rows();
// perform internal calculation using doubles
@ -79,9 +79,11 @@ bool CalibrationUtils::PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::Ve
Eigen::MatrixXd xtx = xt * x;
Eigen::VectorXd xty = xt * doubleY;
std::cout << xty << std::endl;
result = xtx.fullPivHouseholderQr().solve(xty).cast<float>();
return true;
Eigen::VectorXd tmpx = xtx.fullPivHouseholderQr().solve(xty);
result = tmpx.cast<float>();
double relativeError = (xtx*tmpx - xty).norm() / xty.norm();
std::cout << "Estimated relative error " << relativeError << "; Max allowed error " << maxRelativeError << std::endl;
return relativeError < maxRelativeError;
}
/* C++ Implementation of Yury Petrov's ellipsoid fit algorithm

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

@ -44,7 +44,7 @@ public:
};
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);
static bool PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
private:
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,

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

@ -36,14 +36,13 @@ bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::V
qDebug() << "Ref zero is " << index20deg << " T: " << temperature[index20deg] << " P:" << pressure[index20deg];
float refZero = pressure[index20deg];
pressure.array() -= refZero;
qDebug() << "Rebased zero is " << pressure[index20deg];
qDebug() << "Rebiased zero is " << pressure[index20deg];
Eigen::VectorXf solution(BARO_PRESSURE_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, pressure, BARO_PRESSURE_POLY_DEGREE, solution)) {
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;
}
@ -52,19 +51,19 @@ bool ThermalCalibration::AccelerometerCalibration(Eigen::VectorXf samplesX, Eige
{
Eigen::VectorXf solution(ACCEL_X_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesX, ACCEL_X_POLY_DEGREE, solution)) {
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesX, ACCEL_X_POLY_DEGREE, solution, ACCEL_X_MAX_REL_ERROR)) {
return false;
}
result[0] = solution[1];
solution.resize(ACCEL_Y_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesY, ACCEL_Y_POLY_DEGREE, solution)) {
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesY, ACCEL_Y_POLY_DEGREE, solution, ACCEL_Y_MAX_REL_ERROR)) {
return false;
}
result[1] = solution[1];
solution.resize(ACCEL_Z_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, ACCEL_Z_POLY_DEGREE, solution)) {
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, ACCEL_Z_POLY_DEGREE, solution, ACCEL_Z_MAX_REL_ERROR)) {
return false;
}
result[2] = solution[1];
@ -76,21 +75,21 @@ bool ThermalCalibration::GyroscopeCalibration(Eigen::VectorXf samplesX, Eigen::V
{
Eigen::VectorXf solution(GYRO_X_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesX, GYRO_X_POLY_DEGREE, solution)) {
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.resize(GYRO_Y_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesY, GYRO_Y_POLY_DEGREE, solution)) {
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.resize(GYRO_Z_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, GYRO_Z_POLY_DEGREE, solution)) {
if (!CalibrationUtils::PolynomialCalibration(temperature, samplesZ, GYRO_Z_POLY_DEGREE, solution, GYRO_Z_MAX_REL_ERROR)) {
return false;
}
result[2] = solution[1];

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

@ -40,7 +40,14 @@ class ThermalCalibration {
static const int ACCEL_Z_POLY_DEGREE = 1;
static const int BARO_PRESSURE_POLY_DEGREE = 3;
//TODO: determine max allowable relative error
static const double BARO_PRESSURE_MAX_REL_ERROR = 1E-6f;
static const double ACCEL_X_MAX_REL_ERROR = 1E-6f;
static const double ACCEL_Y_MAX_REL_ERROR = 1E-6f;
static const double ACCEL_Z_MAX_REL_ERROR = 1E-6f;
static const double GYRO_X_MAX_REL_ERROR = 1E-6f;
static const double GYRO_Y_MAX_REL_ERROR = 1E-6f;
static const double GYRO_Z_MAX_REL_ERROR = 1E-6f;
public:
/**
* @brief produce the calibration polinomial coefficients from pressure and temperature samples