From 44e3466e0aef9f7f3692f0361968c86456b5d904 Mon Sep 17 00:00:00 2001 From: jonathan Date: Mon, 21 Mar 2011 00:45:40 +0000 Subject: [PATCH] OP-191: Merge from full-calibration branch. Add an implementation of TWOSTEP, including bias-only, bias + scale factor, and bias + scale factor + orthogonality calibration methods. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3053 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../plugins/config/alignment-calibration.cpp | 51 ++ .../src/plugins/config/assertions.h | 78 +++ .../src/plugins/config/calibration.h | 54 ++ .../src/plugins/config/gyro-calibration.cpp | 87 +++ .../src/plugins/config/legacy-calibration.cpp | 114 ++++ .../src/plugins/config/twostep.cpp | 599 ++++++++++++++++++ 6 files changed, 983 insertions(+) create mode 100644 ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/assertions.h create mode 100644 ground/openpilotgcs/src/plugins/config/calibration.h create mode 100644 ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/twostep.cpp diff --git a/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp new file mode 100644 index 000000000..c6b880a77 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/alignment-calibration.cpp @@ -0,0 +1,51 @@ +#include "calibration.h" +#include +#include +#include + +using namespace Eigen; + +/** Calibrate the angular misalignment of one field sensor relative to another. + * + * @param rotationVector[out] The rotation vector that rotates sensor 1 such + * that its principle axes are colinear with the axes of sensor 0. + * @param samples0[in] A list of samples of the field observed by the reference + * sensor. + * @param reference0[in] The common value of the reference field in the inertial + * reference frame. + * @param samples1[in] The list of samples taken by the sensor to be aligned to + * the reference. The attitude of the sensor head as a whole must be identical + * between samples0[i] and samples1[i] for all i. + * @param reference1[in] The actual value of the second field in the inertial + * reference frame. + * @param n_samples The number of samples. + */ +void +calibration_misalignment(Vector3f& rotationVector, + const Vector3f samples0[], + const Vector3f& reference0, + const Vector3f samples1[], + const Vector3f& reference1, + size_t n_samples) +{ + // Note that this implementation makes the assumption that the angular + // misalignment is small. Something based on QUEST would be needed to + // account for errors larger than a few degrees. + Matrix X(n_samples, 3); + Matrix y(n_samples, 1); + + AngleAxisd reference(Quaterniond().setFromTwoVectors( + reference0.cast(), reference1.cast())); + for (size_t i = 0; i < n_samples; ++i) { + AngleAxisd observation(Quaterniond().setFromTwoVectors( + samples0[i].cast(), samples1[i].cast())); + + X.row(i) = observation.axis(); + y[i] = reference.angle() - observation.angle(); + } + + // Run linear least squares over the result. + Vector3d result; + (X.transpose() * X).ldlt().solve(X.transpose()*y, &result); + rotationVector = result.cast(); +} diff --git a/ground/openpilotgcs/src/plugins/config/assertions.h b/ground/openpilotgcs/src/plugins/config/assertions.h new file mode 100644 index 000000000..2a352d715 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/assertions.h @@ -0,0 +1,78 @@ +/* + * assertions.hpp + * + * Created on: Aug 6, 2009 + * Author: Jonathan Brandmeyer + * + * This file is part of libeknav, imported into the OpenPilot + * project by Jonathan Brandmeyer. + * + * Libeknav 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, version 3. + * + * Libeknav 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 libeknav. If not, see . + * + */ + +#ifndef AHRS_ASSERTIONS_HPP +#define AHRS_ASSERTIONS_HPP + +#include +#include + +template +bool hasNaN(const MatrixBase& expr); + +template +bool hasInf(const MatrixBase& expr); + +template +bool perpendicular(const MatrixBase& expl, const MatrixBase& expr); + +template +bool hasNaN(const MatrixBase& expr) +{ + for (int j = 0; j != expr.cols(); ++j) { + for (int i = 0; i != expr.rows(); ++i) { + if (std::isnan(expr.coeff(i, j))) + return true; + } + } + return false; +} + +template +bool hasInf(const MatrixBase& expr) +{ + for (int i = 0; i != expr.cols(); ++i) { + for (int j = 0; j != expr.rows(); ++j) { + if (std::isinf(expr.coeff(j, i))) + return true; + } + } + return false; +} + +template +bool isReal(const MatrixBase& exp) +{ + return !hasNaN(exp) && ! hasInf(exp); +} + +template +bool perpendicular(const MatrixBase& lhs, const MatrixBase& rhs) +{ + // A really weak check for "almost perpendicular". Use it for debugging + // purposes only. + return fabs(rhs.dot(lhs)) < 0.0001; +} + +#endif /* ASSERTIONS_HPP_ */ + diff --git a/ground/openpilotgcs/src/plugins/config/calibration.h b/ground/openpilotgcs/src/plugins/config/calibration.h new file mode 100644 index 000000000..8fa40167d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/calibration.h @@ -0,0 +1,54 @@ +#ifndef AHRS_CALIBRATION_HPP +#define AHRS_CALIBRATION_HPP + +#include +#include +using std::size_t; +using namespace Eigen; + +void +calibration_misalignment(Vector3f& rotationVector, + const Vector3f samples0[], + const Vector3f& reference0, + const Vector3f samples1[], + const Vector3f& reference1, + size_t n_samples); + +Vector3f +twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f& referenceField, + const float noise); + +void +twostep_bias_scale(Vector3f& bias, + Vector3f& scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f& referenceField, + const float noise); + +void +twostep_bias_scale(Vector3f& bias, + Matrix3f& scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f& referenceField, + const float noise); + +void +openpilot_bias_scale(Vector3f& bias, + Vector3f& scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f& referenceField); + +void +gyroscope_calibration(Vector3f& bias, + Matrix3f& accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples); + +#endif // !defined AHRS_CALIBRATION_HPP + diff --git a/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp new file mode 100644 index 000000000..20c12c252 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/gyro-calibration.cpp @@ -0,0 +1,87 @@ + +#include +#include +#include +#include + +/* + * Compute basic calibration parameters for a three axis gyroscope. + * The measurement equation is + * gyro_k = accelSensitivity * \tilde{accel}_k + bias + omega + * + * where, omega is the true angular rate (assumed to be zero) + * bias is the sensor bias + * accelSensitivity is the 3x3 matrix of sensitivity scale factors + * \tilde{accel}_k is the calibrated measurement of the accelerometer + * at time k + * + * After calibration, the optimized gyro measurement is given by + * \tilde{gyro}_k = gyro_k - bias - accelSensitivity * \tilde{accel}_k + */ +void +gyroscope_calibration(Vector3f& bias, + Matrix3f& accelSensitivity, + Vector3f gyroSamples[], + Vector3f accelSamples[], + size_t n_samples) +{ + // Assume the following measurement model: + // y = H*x + // where x is the vector of unknowns, and y is the measurement vector. + // the vector of unknowns is + // [a_x a_y a_z b_x] + // The measurement vector is + // [gyro_x] + // and the measurement matrix H is + // [accelSample_x accelSample_y accelSample_z 1] + + // Note that the individual solutions for x are given by + // (H^T \times H)^-1 \times H^T y + // Everything is identical except for y and x. So, transform it + // into block form X = (H^T \times H)^-1 \times H^T Y + // where each column of X contains the partial solution for each + // column of y. + + // Construct the matrix of accelerometer samples. Intermediate results + // are computed in "twice the precision that the source provides and the + // result deserves" by Kahan's thumbrule to prevent numerical problems. + Matrix H(n_samples, 4); + // And the matrix of gyro samples. + Matrix Y(n_samples, 3); + for (unsigned i = 0; i < n_samples; ++i) { + H.row(i) << accelSamples[i].transpose().cast(), 1.0; + Y.row(i) << gyroSamples[i].transpose().cast(); + } + +#if 1 + Matrix result; + // Use the cholesky-based Penrose pseudoinverse method. + (H.transpose() * H).ldlt().solve(H.transpose()*Y, &result); + + // Transpose the result and return it to the caller. Cast back to float + // since there really isn't that much accuracy in the result. + bias = result.row(3).transpose().cast(); + accelSensitivity = result.block<3, 3>(0, 0).transpose().cast(); +#else + // TODO: Compare this result with a total-least-squares model. + size_t n = 4; + Matrix C; + C << H, Y; + SVD > usv(C); + Matrix V_ab = usv.matrixV().block<4, 3>(0, n); + Matrix V_bb = usv.matrixV().corner(BottomRight, n_samples-4, 3); + // X = -V_ab/V_bb; + // X^T = (A * B^-1)^T + // X^T = (B^-1^T * A^T) + // X^T = (B^T^-1 * A^T) + // V_bb is orthgonal but not orthonormal. QR decomposition + // should be very fast in this case. + Matrix result; + V_bb.transpose().qr().solve(-V_ab.transpose(), &result); + + // Results only deserve single precision. + bias = result.col(3).cast(); + accelSensitivity = result.block<3, 3>(0, 0).cast(); + +#endif +} diff --git a/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp new file mode 100644 index 000000000..92e34efb2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/legacy-calibration.cpp @@ -0,0 +1,114 @@ +/** + ****************************************************************************** + * + * @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 +#include + +/** + * 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 A; + Matrix 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 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]; + } +} + + diff --git a/ground/openpilotgcs/src/plugins/config/twostep.cpp b/ground/openpilotgcs/src/plugins/config/twostep.cpp new file mode 100644 index 000000000..ec0d91fa9 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/twostep.cpp @@ -0,0 +1,599 @@ +/** + ****************************************************************************** + * + * @file twostep.cpp + * @author Jonathan Brandmeyer + * Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Config plugin + * @{ + * @brief Impliments low-level calibration algorithms + *****************************************************************************/ +/* + * 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; Version 3 of the License, and no other + * 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 +#include + +#include +#include +#include +#include + +#undef PRINTF_DEBUGGING + +#include +#include + +/* + * Alas, this implementation is a fairly direct copy of the contents of the + * following papers. You don't have a chance at understanding it without + * reading these first. Any bugs should be presumed to be JB's fault rather + * than Alonso and Shuster until proven otherwise. + * + * Reference [1]: "A New Algorithm for Attitude-independant magnetometer + * calibration", Robert Alonso and Malcolmn D. Shuster, Flight Mechanics/ + * Estimation Theory Symposium, NASA Goddard, 1994, pp 513-527 + * + * Reference [2]: Roberto Alonso and Malcolm D. Shuster, "Complete Linear + * Attitude-Independent Magnetometer Calibration", Journal of the Astronautical + * Sciences, Vol 50, No 4, 2002, pp 477-490 + * + */ + +namespace { +Vector3f center(const Vector3f samples[], + size_t n_samples) +{ + Vector3f summation(Vector3f::Zero()); + for (size_t i = 0; i < n_samples; ++i) { + summation += samples[i]; + } + return summation / float(n_samples); +} + +void inv_fisher_information_matrix(Matrix3f& fisherInv, + Matrix3f& fisher, + const Vector3f samples[], + size_t n_samples, + const float noise) +{ + MatrixXf column_samples(3, n_samples); + for (size_t i = 0; i < n_samples; ++i) { + column_samples.col(i) = samples[i]; + } + fisher = 4 / noise * + column_samples * column_samples.transpose(); + // Compute the inverse by taking advantage of the results symmetricness + fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv); + return; +} +// Eq 14 of the original reference. Computes the gradiant of the cost function +// with respect to the bias offset. +// @param samples: The vector of measurement samples +// @param mags: The vector of sample delta magnitudes +// @param b: The estimate of the bias +// @param mu: The trace of the noise matrix (3*noise) +// @return the negative gradiant of the cost function with respect to the +// current value of the estimate, b +Vector3f +neg_dJdb(const Vector3f samples[], + const float mags[], + size_t n_samples, + const Vector3f& b, + float mu, + float noise) +{ + Vector3f summation(Vector3f::Zero()); + float b_norm2 = b.squaredNorm(); + + for (size_t i = 0; i < n_samples; ++i) { + summation += (mags[i] - 2*samples[i].dot(b) + b_norm2 - mu) + *2* (samples[i] - b); + } + + return (1.0 / noise)*summation; +} +} // !namespace (anon) + +Vector3f twostep_bias_only(const Vector3f samples[], + size_t n_samples, + const Vector3f& referenceField, + const float noise) +{ + // \tilde{H} + Vector3f centeredSamples[n_samples]; + // z_k + float sampleDeltaMag[n_samples]; + // eq 7 and 8 applied to samples + Vector3f avg = center(samples, n_samples); + float refSquaredNorm = referenceField.squaredNorm(); + float sampleDeltaMagCenter = 0; + for (size_t i = 0; i < n_samples; ++i) + { + // eq 9 applied to samples + centeredSamples[i] = samples[i] - avg; + // eqn 2a + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + + Matrix3f P_bb; + Matrix3f P_bb_inv; + // Due to eq 12b + inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise); + // Compute centered magnitudes + float sampleDeltaMagCentered[n_samples]; + for (size_t i = 0; i < n_samples; ++i) { + sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + } + + // From eq 12a + Vector3f estimate( Vector3f::Zero()); + for (size_t i = 0; i < n_samples; ++i) { + estimate += sampleDeltaMagCentered[i] * centeredSamples[i]; + } + estimate = P_bb * ((2 / noise) * estimate); + + // Newton-Raphson gradient descent to the optimal solution + // eq 14a and 14b + float mu = -3*noise; + for (int i = 0; i < 6; ++i) { + Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples, + estimate, mu, noise); + Matrix3f scale = P_bb_inv + 4/noise*(avg - estimate)*(avg - estimate).transpose(); + Vector3f neg_increment; + scale.ldlt().solve(neg_gradiant, &neg_increment); + // Note that the negative has been done twice + estimate += neg_increment; + } + return estimate; +} + +namespace { +// Private utility functions for the version of TWOSTEP that computes bias and +// scale factor vectors alone. + +/** Compute the gradiant of the norm of b with respect to the estimate vector. + */ +Matrix +dnormb_dtheta(const Matrix& theta) +{ + return (Matrix() << 2*theta.coeff(0)/(1+theta.coeff(3)), + 2*theta.coeff(1)/(1+theta.coeff(4)), + 2*theta.coeff(2)/(1+theta.coeff(5)), + -pow(theta.coeff(0), 2)/pow(1+theta.coeff(3), 2), + -pow(theta.coeff(1), 2)/pow(1+theta.coeff(4), 2), + -pow(theta.coeff(2), 2)/pow(1+theta.coeff(5), 2)).finished(); +} + +/** + * Compute the gradiant of the cost function with respect to the optimal + * estimate. + */ +Matrix +dJdb(const Matrix& centerL, + double centerMag, + const Matrix& theta, + const Matrix& dbdtheta, + double mu, + double noise) +{ + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; + + // By equation 35 + double normbthetaSquared = 0; + for (unsigned i = 0; i < 3; ++i) { + normbthetaSquared += theta.coeff(i)*theta.coeff(i)/(1+theta.coeff(3+i)); + } + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu)/noise; + return scalarPart * vectorPart; +} + +/** Reconstruct the scale factor and bias offset vectors from the transformed + * estimate vector. + */ +Matrix +theta_to_sane(const Matrix& theta) +{ + return (Matrix() << + theta.coeff(0) / sqrt(1 + theta.coeff(3)), + theta.coeff(1) / sqrt(1 + theta.coeff(4)), + theta.coeff(2) / sqrt(1 + theta.coeff(5)), + -1 + sqrt(1 + theta.coeff(3)), + -1 + sqrt(1 + theta.coeff(4)), + -1 + sqrt(1 + theta.coeff(5))).finished(); +} + +} // !namespace (anon) + +/** + * Compute the scale factor and bias associated with a vector observer + * according to the equation: + * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k) + * where k is the sample index, + * B_k is the kth measurement + * I_{3x3} is a 3x3 identity matrix + * D is a 3x3 diagonal matrix of scale factors + * O is the orthogonal alignment matrix + * A_k is the attitude at the kth sample + * b is the bias in the global reference frame + * \epsilon_k is the noise at the kth sample + * This implementation makes the assumption that \epsilon is a constant white, + * guassian noise source that is common to all k. The misalignment matrix O + * is not computed, and the off-diagonal elements of D, corresponding to the + * misalignment scale factors are not either. + * + * @param bias[out] The computed bias, in the global frame + * @param scale[out] The computed scale factor, in the sensor frame + * @param samples[in] An array of measurement samples. + * @param n_samples The number of samples in the array. + * @param referenceField The magnetic field vector at this location. + * @param noise The one-sigma squared standard deviation of the observed noise + * in the sensor. + */ +void twostep_bias_scale(Vector3f& bias, + Vector3f& scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f& referenceField, + const float noise) +{ + // Initial estimate for gradiant descent starts at eq 37a of ref 2. + + // Define L_k by eq 30 and 28 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 6); + // \hbar{L} by eq 33, simplified by obesrving that the + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + double refSquaredNorm = referenceField.squaredNorm(); + + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2*samples[i].transpose().cast(), + -(samples[i][0]*samples[i][0]), + -(samples[i][1]*samples[i][1]), + -(samples[i][2]*samples[i][2]); + centerSample += fullSamples.row(i); + + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; + + // True for all k. + // double mu = -3*noise; + // The center value of mu, \bar{mu} + // double centerMu = mu; + // The center value of mu, \tilde{mu} + // double centeredMu = 0; + + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 6); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 37a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; // note: paper supplies 1/noise + + // By eq 37 b). Note, paper supplies 1/noise here + Matrix P_theta_theta_inv = (1.0f/noise)* + centeredSamples.transpose()*centeredSamples; +#ifdef PRINTF_DEBUGGING + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; +#endif + + // The Fisher information matrix has a small eigenvalue, with a + // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, + // 0.55]. This means that there is relatively little information + // about the common gain on the scale factor, which has a + // corresponding effect on the bias, too. The fixup is performed by + // the first iteration of the second stage of TWOSTEP, as documented in + // [1]. + Matrix estimate; + // By eq 37 a), \tilde{Fisher}^-1 + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + +#ifdef PRINTF_DEBUGGING + Matrix P_theta_theta; + P_theta_theta_inv.ldlt().solve(Matrix::Identity(), &P_theta_theta); + SelfAdjointEigenSolver > eig2(P_theta_theta); + std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; + std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; + std::cout << "estimate summation: \n" << estimateSummation.normalized() + << "\n"; +#endif + + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 3; + while (count --> 0) { + // eq 40 + Matrix db_dtheta = dnormb_dtheta(estimate); + + Matrix dJ_dtheta = dJdb(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3*noise, + noise/n_samples); + + + // Eq 39 (with double-negation on term inside parens) + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + } + + // Transform the estimated parameters from [c | e] back into [b | d]. + for (size_t i = 0; i < 3; ++i) { + scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3+i)); + bias.coeffRef(i) = estimate.coeff(i)/sqrt(1 + estimate.coeff(3+i)); + } +} + +namespace { +// Private functions specific to the scale factor and orthogonal calibration +// version of TWOSTEP + +/** + * Reconstruct the symmetric E matrix from the last 6 elements of the estimate + * vector + */ +Matrix3d +E_theta(const Matrix& theta) +{ + // By equation 49b + return (Matrix3d() << + theta.coeff(3), theta.coeff(6), theta.coeff(7), + theta.coeff(6), theta.coeff(4), theta.coeff(8), + theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished(); +} + +/** + * Compute the gradiant of the squared norm of b with respect to theta. Note + * that b itself is just a function of theta. Therefore, this function + * computes \frac{\delta,\delta\theta'}\left|b(\theta')\right| + * From eq 55 of [2]. + */ +Matrix +dnormb_dtheta(const Matrix& theta) +{ + // Re-form the matrix E from the vector of theta' + Matrix3d E = E_theta(theta); + // Compute (I + E)^-1 * c + Vector3d IE_inv_c; + E.ldlt().solve(theta.end<3>(), &IE_inv_c); + + return (Matrix() << 2*IE_inv_c, + -IE_inv_c.coeff(0)*IE_inv_c.coeff(0), + -IE_inv_c.coeff(1)*IE_inv_c.coeff(1), + -IE_inv_c.coeff(2)*IE_inv_c.coeff(2), + + -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(1), + -2*IE_inv_c.coeff(0)*IE_inv_c.coeff(2), + -2*IE_inv_c.coeff(1)*IE_inv_c.coeff(2)).finished(); +} + +// The gradiant of the cost function with respect to theta, at a particular +// value of theta. +// @param centerL: The center of the samples theta' +// @param centerMag: The center of the magnitude data +// @param theta: The estimate of the bias and scale factor +// @return the gradiant of the cost function with respect to the +// current value of the estimate, theta' +Matrix +dJ_dtheta(const Matrix& centerL, + double centerMag, + const Matrix& theta, + const Matrix& dbdtheta, + double mu, + double noise) +{ + // \frac{\delta}{\delta \theta'} |b(\theta')|^2 + Matrix vectorPart = dbdtheta - centerL; + + // |b(\theta')|^2 + Matrix3d E = E_theta(theta); + Vector3d rhs; + (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs); + double normbthetaSquared = theta.start<3>().dot(rhs); + + double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared + - mu)/noise; + return scalarPart * vectorPart; +} +} // !namespace (anonymous) + +/** + * Compute the scale factor and bias associated with a vector observer + * according to the equation: + * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k) + * where k is the sample index, + * B_k is the kth measurement + * I_{3x3} is a 3x3 identity matrix + * D is a 3x3 symmetric matrix of scale factors + * O is the orthogonal alignment matrix + * A_k is the attitude at the kth sample + * b is the bias in the global reference frame + * \epsilon_k is the noise at the kth sample + * + * After computing the scale factor and bias matricies, the optimal estimate is + * given by + * \tilde{B}_k = (I_{3x3} + D)B_k - b + * + * This implementation makes the assumption that \epsilon is a constant white, + * guassian noise source that is common to all k. The misalignment matrix O + * is not computed. + * + * @param bias[out] The computed bias, in the global frame + * @param scale[out] The computed scale factor matrix, in the sensor frame. + * @param samples[in] An array of measurement samples. + * @param n_samples The number of samples in the array. + * @param referenceField The magnetic field vector at this location. + * @param noise The one-sigma squared standard deviation of the observed noise + * in the sensor. + */ +void twostep_bias_scale(Vector3f& bias, + Matrix3f& scale, + const Vector3f samples[], + const size_t n_samples, + const Vector3f& referenceField, + const float noise) +{ + // Define L_k by eq 51 for k = 1 .. n_samples + Matrix fullSamples(n_samples, 9); + // \hbar{L} by eq 52, simplified by obesrving that the common noise term + // makes this a simple average. + Matrix centerSample = Matrix::Zero(); + // Define the sample differences z_k by eq 23 a) + double sampleDeltaMag[n_samples]; + // The center value \hbar{z} + double sampleDeltaMagCenter = 0; + // The squared norm of the reference vector + double refSquaredNorm = referenceField.squaredNorm(); + + for (size_t i = 0; i < n_samples; ++i) { + fullSamples.row(i) << 2*samples[i].transpose().cast(), + -(samples[i][0]*samples[i][0]), + -(samples[i][1]*samples[i][1]), + -(samples[i][2]*samples[i][2]), + -2*(samples[i][0]*samples[i][1]), + -2*(samples[i][0]*samples[i][2]), + -2*(samples[i][1]*samples[i][2]); + + centerSample += fullSamples.row(i); + + sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; + sampleDeltaMagCenter += sampleDeltaMag[i]; + } + sampleDeltaMagCenter /= n_samples; + centerSample /= n_samples; + + // Define \tilde{L}_k for k = 0 .. n_samples + Matrix centeredSamples(n_samples, 9); + // Define \tilde{z}_k for k = 0 .. n_samples + double centeredMags[n_samples]; + // Compute the term under the summation of eq 57a + Matrix estimateSummation = Matrix::Zero(); + for (size_t i = 0; i < n_samples; ++i) { + centeredSamples.row(i) = fullSamples.row(i) - centerSample; + centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; + estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); + } + estimateSummation /= noise; + + // By eq 57b + Matrix P_theta_theta_inv = (1.0f/noise)* + centeredSamples.transpose()*centeredSamples; + +#ifdef PRINTF_DEBUGGING + SelfAdjointEigenSolver > eig(P_theta_theta_inv); + std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; + std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() + << "\n"; + std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; +#endif + + // The current value of the estimate. Initialized to \tilde{\theta}^* + Matrix estimate; + // By eq 57a + P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); + + // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) + // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} + size_t count = 0; + double eta = 10000; + while (count++ < 2000 && eta > 1e-8) { + static bool warned = false; + if (hasNaN(estimate)) { + std::cout << "WARNING: found NaN at time " << count << "\n"; + warned = true; + } +#if 0 + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()) .cast(); + + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); + std::cout << "\n\nestimated bias: " << bias.transpose() + << "\nestimated scale:\n" << scale; +#endif + + Matrix db_dtheta = dnormb_dtheta(estimate); + + Matrix dJ_dtheta = ::dJ_dtheta(centerSample, + sampleDeltaMagCenter, + estimate, + db_dtheta, + -3*noise, + noise/n_samples); + + // Eq 59, with reused storage on db_dtheta + db_dtheta = centerSample - db_dtheta; + Matrix scale = P_theta_theta_inv + + (double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta; + + // eq 14b, mutatis mutandis (grumble, grumble) + Matrix increment; + scale.ldlt().solve(dJ_dtheta, &increment); + estimate -= increment; + eta = increment.dot(scale * increment); + std::cout << "eta: " << eta << "\n"; + } + std::cout << "terminated at eta = " << eta + << " after " << count << " iterations\n"; + + // Transform the estimated parameters from [c | E] back into [b | D]. + // See eq 63-65 + SelfAdjointEigenSolver eig_E(E_theta(estimate)); + Vector3d S = eig_E.eigenvalues(); + Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), + -1 + sqrt(1 + S.coeff(1)), + -1 + sqrt(1 + S.coeff(2)); + scale = (eig_E.eigenvectors() * W.asDiagonal() * + eig_E.eigenvectors().transpose()) .cast(); + + (Matrix3f::Identity() + scale).ldlt().solve( + estimate.start<3>().cast(), &bias); +} +