mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-07 18:46:06 +01:00
44e3466e0a
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3053 ebee16cc-31ac-478f-84a7-5cbb03baadba
115 lines
4.1 KiB
C++
115 lines
4.1 KiB
C++
/**
|
|
******************************************************************************
|
|
*
|
|
* @file legacy-calibration.cpp
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @addtogroup GCSPlugins GCS Plugins
|
|
* @{
|
|
* @addtogroup ConfigPlugin Config Plugin
|
|
* @{
|
|
* @brief The Configuration Gadget used to update settings in the firmware
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
#include <calibration.h>
|
|
#include <Eigen/LU>
|
|
|
|
/**
|
|
* The basic calibration algorithm initially used in OpenPilot. This is a basic
|
|
* 6-point calibration that doesn't attempt to account for any of the white noise
|
|
* in the sensors.
|
|
* The measurement equation is
|
|
* B_k = D^-1 * (A_k * H_k - b)
|
|
* Where B_k is the measurement of the field at time k
|
|
* D is the diagonal matrix of scale factors
|
|
* A_k is the attitude direction cosine matrix at time k
|
|
* H_k is the reference field at the kth sample
|
|
* b is the vector of scale factors.
|
|
*
|
|
* After computing the scale factor and bias, the optimized measurement is
|
|
* \tilde{B}_k = D * (B_k + b)
|
|
*
|
|
* @param bias[out] The computed bias of the sensor
|
|
* @param scale[out] The computed scale factor of the sensor
|
|
* @param samples An array of sample data points. Only the first 6 are
|
|
* used.
|
|
* @param n_samples The number of sample data points. Must be at least 6.
|
|
* @param referenceField The field being measured by the sensor.
|
|
*/
|
|
void
|
|
openpilot_bias_scale(Vector3f& bias,
|
|
Vector3f& scale,
|
|
const Vector3f samples[],
|
|
const size_t n_samples,
|
|
const Vector3f& referenceField)
|
|
{
|
|
// TODO: Add error handling, and return error codes through the return
|
|
// value.
|
|
if (n_samples < 6) {
|
|
bias = Vector3f::Zero();
|
|
scale = Vector3f::Zero();
|
|
return;
|
|
}
|
|
// mag = (S*x + b)**2
|
|
// mag = (S**2 * x**2 + 2*S*b*x + b*b)
|
|
// 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2))
|
|
// Fill in matrix A -
|
|
// write six difference-in-magnitude equations of the form
|
|
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) +
|
|
// 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
|
|
//
|
|
// or in other words
|
|
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 +
|
|
// 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 =
|
|
// (x1^2-x2^2)
|
|
Matrix<float, 5, 5> A;
|
|
Matrix<float, 5, 1> f;
|
|
for (unsigned i=0; i<5; i++){
|
|
A.row(i)[0] = 2.0 * (samples[i+1].x() - samples[i].x());
|
|
A.row(i)[1] = samples[i+1].y()* samples[i+1].y() - samples[i].y()*samples[i].y();
|
|
A.row(i)[2] = 2.0 * (samples[i+1].y() - samples[i].y());
|
|
A.row(i)[3] = samples[i+1].z()*samples[i+1].z() - samples[i].z()*samples[i].z();
|
|
A.row(i)[4] = 2.0 * (samples[i+1].z() - samples[i].z());
|
|
f[i] = samples[i].x()*samples[i].x() - samples[i+1].x()*samples[i+1].x();
|
|
}
|
|
Matrix<float, 5, 1> c;
|
|
A.lu().solve(f, &c);
|
|
|
|
|
|
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
|
|
float xp = samples[0].x();
|
|
float yp = samples[0].y();
|
|
float zp = samples[0].z();
|
|
|
|
float Sx = sqrt(referenceField.squaredNorm() /
|
|
(xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
|
|
|
|
scale[0] = Sx;
|
|
bias[0] = Sx*c[0];
|
|
scale[1] = sqrt(c[1]*Sx*Sx);
|
|
bias[1] = c[2]*Sx*Sx/scale[1];
|
|
scale[2] = sqrt(c[3]*Sx*Sx);
|
|
bias[2] = c[4]*Sx*Sx/scale[2];
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
// Fixup difference between openpilot measurement model and twostep
|
|
// version
|
|
bias[i] = -bias[i] / scale[i];
|
|
}
|
|
}
|
|
|
|
|