ground/gcs/src/plugins/config/gyro-calibration.cpp

   1 #include <calibration.h>
   2 #include <Eigen/Cholesky>
   3 #include <Eigen/SVD>
   4 #include <Eigen/QR>
   5
   6 /*
   7  * Compute basic calibration parameters for a three axis gyroscope.
   8  * The measurement equation is
   9  * gyro_k = accelSensitivity * \tilde{accel}_k + bias + omega
  10  *
  11  *      where, omega is the true angular rate (assumed to be zero)
  12  *              bias is the sensor bias
  13  *              accelSensitivity is the 3x3 matrix of sensitivity scale factors
  14  *              \tilde{accel}_k is the calibrated measurement of the accelerometer
  15  *                      at time k
  16  *
  17  * After calibration, the optimized gyro measurement is given by
  18  * \tilde{gyro}_k = gyro_k - bias - accelSensitivity * \tilde{accel}_k
  19  */
  20 void gyroscope_calibration(Vector3f & bias,
  21                            Matrix3f & accelSensitivity,
  22                            Vector3f gyroSamples[],
  23                            Vector3f accelSamples[],
  24                            size_t n_samples)
  25 {
  26     // Assume the following measurement model:
  27     // y = H*x
  28     // where x is the vector of unknowns, and y is the measurement vector.
  29     // the vector of unknowns is
  30     // [a_x a_y a_z b_x]
  31     // The measurement vector is
  32     // [gyro_x]
  33     // and the measurement matrix H is
  34     // [accelSample_x accelSample_y accelSample_z 1]
  35
  36     // Note that the individual solutions for x are given by
  37     // (H^T \times H)^-1 \times H^T y
  38     // Everything is identical except for y and x.  So, transform it
  39     // into block form X = (H^T \times H)^-1 \times H^T Y
  40     // where each column of X contains the partial solution for each
  41     // column of y.
  42
  43     // Construct the matrix of accelerometer samples.  Intermediate results
  44     // are computed in "twice the precision that the source provides and the
  45     // result deserves" by Kahan's thumbrule to prevent numerical problems.
  46     Matrix<double, Dynamic, 4> H(n_samples, 4);
  47     // And the matrix of gyro samples.
  48     Matrix<double, Dynamic, 3> Y(n_samples, 3);
  49     for (unsigned i = 0; i < n_samples; ++i) {
  50         H.row(i) << accelSamples[i].transpose().cast<double>(), 1.0;
  51         Y.row(i) << gyroSamples[i].transpose().cast<double>();
  52     }
  53
  54 #if 1
  55     Matrix<double, 4, 3> result;
  56     // Use the cholesky-based Penrose pseudoinverse method.
  57     (H.transpose() * H).ldlt().solve(H.transpose() * Y, &result);
  58
  59     // Transpose the result and return it to the caller.  Cast back to float
  60     // since there really isn't that much accuracy in the result.
  61     bias = result.row(3).transpose().cast<float>();
  62     accelSensitivity = result.block<3, 3>(0, 0).transpose().cast<float>();
  63 #else
  64     // TODO: Compare this result with a total-least-squares model.
  65     size_t n = 4;
  66     Matrix<double, Dynamic, 7> C;
  67     C << H, Y;
  68     SVD<Matrix<double, Dynamic, 7> > usv(C);
  69     Matrix<double, 4, 3> V_ab = usv.matrixV().block<4, 3>(0, n);
  70     Matrix<double, Dynamic, 3> V_bb = usv.matrixV().corner(BottomRight, n_samples - 4, 3);
  71     // X = -V_ab/V_bb;
  72     // X^T = (A * B^-1)^T
  73     // X^T = (B^-1^T * A^T)
  74     // X^T = (B^T^-1 * A^T)
  75     // V_bb is orthgonal but not orthonormal.  QR decomposition
  76     // should be very fast in this case.
  77     Matrix<double, 3, 4> result;
  78     V_bb.transpose().qr().solve(-V_ab.transpose(), &result);
  79
  80     // Results only deserve single precision.
  81     bias = result.col(3).cast<float>();
  82     accelSensitivity = result.block<3, 3>(0, 0).cast<float>();
  83
  84 #endif // if 1
  85 }