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

OP-1150 Make calibrationutil class static and include the Polynomial fit algorithm

Browse Source
This commit is contained in:
Alessio Morale 2014-01-07 19:50:53 +01:00
parent 46ff4c8bf9
commit 78f77e8d3f
2 changed files with 53 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,7 +4,7 @@
* @file calibrationutils.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
*
* @brief Utilities for calibration using ellipsoid fit algorithm
* @brief Utilities for calibration. Ellipsoid and polynomial fit algorithms
* @see The GNU Public License (GPL) Version 3
* @defgroup
* @{
@ -29,40 +29,57 @@
#include "calibrationutils.h"
using namespace Eigen;
namespace OpenPilot{
CalibrationUtils::CalibrationUtils()
{
NominalRange = 1.0f;
}
/*
* The following code is based on RazorImu calibration samples that can be found here:
* 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::Calibrate(){
Eigen::VectorXf samplesX = Eigen::VectorXf(samples_x);
Eigen::VectorXf samplesY = Eigen::VectorXf(samples_y);
Eigen::VectorXf samplesZ = Eigen::VectorXf(samples_z);
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;
this->EllipsoidFit(&samplesX, &samplesY, &samplesZ, &center, &radii, &evecs);
EllipsoidFit(&samplesX, &samplesY, &samplesZ, &center, &radii, &evecs);
Scale.setZero();
result->Scale.setZero();
Scale << NominalRange / radii.coeff(0),
NominalRange / radii.coeff(1),
NominalRange / radii.coeff(2);
result->Scale << nominalRange / radii.coeff(0),
nominalRange / radii.coeff(1),
nominalRange / radii.coeff(2);
Eigen::Matrix3f tmp;
tmp << Scale.coeff(0), 0, 0,
0, Scale.coeff(1), 0,
0, 0, Scale.coeff(2);
tmp << result->Scale.coeff(0), 0, 0,
0, result->Scale.coeff(1), 0,
0, 0, result->Scale.coeff(2);
CalibrationMatrix = evecs * tmp * evecs.transpose();
Bias.setZero();
Bias << center.coeff(0), center.coeff(1), center.coeff(2);
result->CalibrationMatrix = evecs * tmp * evecs.transpose();
result->Bias.setZero();
result->Bias << center.coeff(0), center.coeff(1), center.coeff(2);
return true;
}
bool CalibrationUtils::PolynomialCalibration(Eigen::VectorXf samplesX, Eigen::VectorXf samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result){
int samples = samplesX.rows();
// perform internal calculation using doubles
VectorXd doubleX = samplesX.cast<double>();
VectorXd doubleY = samplesY.cast<double>();
Eigen::MatrixXd x(samples, degree + 1);
x.setOnes(samples, degree + 1);
for(int i = 1; i < degree + 1; i++){
Eigen::MatrixXd tmp = Eigen::MatrixXd(x.col(i-1));
Eigen::MatrixXd tmp2 = tmp.cwiseProduct(doubleX);
x.col(i) = tmp2;
}
Eigen::MatrixXd xt = x.transpose();
Eigen::MatrixXd xtx = xt * x;
Eigen::VectorXd xty = xt * doubleY;
std::cout << xty << std::endl;
result = xtx.fullPivHouseholderQr().solve(xty).cast<float>();
return true;
}

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

@ -4,7 +4,7 @@
* @file calibrationutils.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
*
* @brief Utilities for calibration using ellipsoid fit algorithm
* @brief Utilities for calibration. Ellipsoid and polynomial fit algorithms
* @see The GNU Public License (GPL) Version 3
* @defgroup
* @{
@ -31,6 +31,7 @@
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <iostream>
namespace OpenPilot{
@ -38,20 +39,21 @@ namespace OpenPilot{
class CalibrationUtils
{
public:
CalibrationUtils();
bool Calibrate();
Eigen::VectorXf samples_x;
Eigen::VectorXf samples_y;
Eigen::VectorXf samples_z;
Eigen::Matrix3f CalibrationMatrix;
Eigen::Vector3f Scale;
Eigen::Vector3f Bias;
float NominalRange;
struct EllipsoidCalibrationResult{
Eigen::Matrix3f CalibrationMatrix;
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);
private:
void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
Eigen::Vector3f *center,
Eigen::VectorXf *radii,
Eigen::MatrixXf *evecs);
};
}