ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       legacy-calibration.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup ConfigPlugin Config Plugin
   9  * @{
  10  * @brief The Configuration Gadget used to update settings in the firmware
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27 #include <calibration.h>
  28 #include <Eigen/LU>
  29
  30 /**
  31  * The basic calibration algorithm initially used in OpenPilot. This is a basic
  32  * 6-point calibration that doesn't attempt to account for any of the white noise
  33  * in the sensors.
  34  * The measurement equation is
  35  * B_k = D^-1 * (A_k * H_k - b)
  36  * Where B_k is the measurement of the field at time k
  37  *      D is the diagonal matrix of scale factors
  38  *      A_k is the attitude direction cosine matrix at time k
  39  *  H_k is the reference field at the kth sample
  40  *  b is the vector of scale factors.
  41  *
  42  * After computing the scale factor and bias, the optimized measurement is
  43  * \tilde{B}_k = D * (B_k + b)
  44  *
  45  * @param bias[out] The computed bias of the sensor
  46  * @param scale[out] The computed scale factor of the sensor
  47  * @param samples An array of sample data points.  Only the first 6 are
  48  *      used.
  49  * @param n_samples The number of sample data points.  Must be at least 6.
  50  * @param referenceField The field being measured by the sensor.
  51  */
  52 void openpilot_bias_scale(Vector3f & bias,
  53                           Vector3f & scale,
  54                           const Vector3f samples[],
  55                           const size_t n_samples,
  56                           const Vector3f & referenceField)
  57 {
  58     // TODO: Add error handling, and return error codes through the return
  59     // value.
  60     if (n_samples < 6) {
  61         bias  = Vector3f::Zero();
  62         scale = Vector3f::Zero();
  63         return;
  64     }
  65     // mag = (S*x + b)**2
  66     // mag = (S**2 * x**2 + 2*S*b*x + b*b)
  67     // 0 = S**2 * (x1**2 - x2**2) + 2*S*B*(x1 - x2))
  68     // Fill in matrix A -
  69     // write six difference-in-magnitude equations of the form
  70     // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) +
  71     // 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
  72     //
  73     // or in other words
  74     // 2*Sx*bx*(x2-x1)/Sx^2  + Sy^2(y2^2-y1^2)/Sx^2  +
  75     // 2*Sy*by*(y2-y1)/Sx^2  + Sz^2(z2^2-z1^2)/Sx^2  + 2*Sz*bz*(z2-z1)/Sx^2  =
  76     // (x1^2-x2^2)
  77     Matrix<float, 5, 5> A;
  78     Matrix<float, 5, 1> f;
  79     for (unsigned i = 0; i < 5; i++) {
  80         A.row(i)[0] = 2.0 * (samples[i + 1].x() - samples[i].x());
  81         A.row(i)[1] = samples[i + 1].y() * samples[i + 1].y() - samples[i].y() * samples[i].y();
  82         A.row(i)[2] = 2.0 * (samples[i + 1].y() - samples[i].y());
  83         A.row(i)[3] = samples[i + 1].z() * samples[i + 1].z() - samples[i].z() * samples[i].z();
  84         A.row(i)[4] = 2.0 * (samples[i + 1].z() - samples[i].z());
  85         f[i] = samples[i].x() * samples[i].x() - samples[i + 1].x() * samples[i + 1].x();
  86     }
  87     Matrix<float, 5, 1> c;
  88     A.lu().solve(f, &c);
  89
  90
  91     // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
  92     float xp = samples[0].x();
  93     float yp = samples[0].y();
  94     float zp = samples[0].z();
  95
  96     float Sx = sqrt(referenceField.squaredNorm() /
  97                     (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]));
  98
  99     scale[0] = Sx;
 100     bias[0]  = Sx * c[0];
 101     scale[1] = sqrt(c[1] * Sx * Sx);
 102     bias[1]  = c[2] * Sx * Sx / scale[1];
 103     scale[2] = sqrt(c[3] * Sx * Sx);
 104     bias[2]  = c[4] * Sx * Sx / scale[2];
 105
 106     for (int i = 0; i < 3; ++i) {
 107         // Fixup difference between openpilot measurement model and twostep
 108         // version
 109         bias[i] = -bias[i] / scale[i];
 110     }
 111 }