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

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

@@ -0,0 +1,51 @@
+#include "calibration.h"
+#include <Eigen/Core>
+#include <Eigen/Cholesky>
+#include <Eigen/Geometry>
+
+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<double, Dynamic, 3> X(n_samples, 3);
+	Matrix<double, Dynamic, 1> y(n_samples, 1);
+
+	AngleAxisd reference(Quaterniond().setFromTwoVectors(
+			reference0.cast<double>(), reference1.cast<double>()));
+	for (size_t i = 0; i < n_samples; ++i) {
+		AngleAxisd observation(Quaterniond().setFromTwoVectors(
+			samples0[i].cast<double>(), samples1[i].cast<double>()));
+
+		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<float>();
+}

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef AHRS_ASSERTIONS_HPP
+#define AHRS_ASSERTIONS_HPP
+
+#include <Eigen/Core>
+#include <cmath>
+
+template<typename MatrixBase>
+bool hasNaN(const MatrixBase& expr);
+
+template<typename MatrixBase>
+bool hasInf(const MatrixBase& expr);
+
+template<typename MatrixBase>
+bool perpendicular(const MatrixBase& expl, const MatrixBase& expr);
+
+template<typename MatrixBase>
+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<typename MatrixBase>
+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<typename MatrixBase>
+bool isReal(const MatrixBase& exp)
+{
+	return !hasNaN(exp) && ! hasInf(exp);
+}
+
+template<typename MatrixBase>
+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_ */
+

ground/openpilotgcs/src/plugins/config/calibration.h

@@ -0,0 +1,54 @@
+#ifndef AHRS_CALIBRATION_HPP
+#define AHRS_CALIBRATION_HPP
+
+#include <Eigen/Core>
+#include <cstdlib>
+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
+

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

@@ -0,0 +1,87 @@
+
+#include <calibration.h>
+#include <Eigen/Cholesky>
+#include <Eigen/SVD>
+#include <Eigen/QR>
+
+/*
+ * 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<double, Dynamic, 4> H(n_samples, 4);
+	// And the matrix of gyro samples.
+	Matrix<double, Dynamic, 3> Y(n_samples, 3);
+	for (unsigned i = 0; i < n_samples; ++i) {
+		H.row(i) << accelSamples[i].transpose().cast<double>(), 1.0;
+		Y.row(i) << gyroSamples[i].transpose().cast<double>();
+	}
+
+#if 1
+	Matrix<double, 4, 3> 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<float>();
+	accelSensitivity = result.block<3, 3>(0, 0).transpose().cast<float>();
+#else
+	// TODO: Compare this result with a total-least-squares model.
+	size_t n = 4;
+	Matrix<double, Dynamic, 7> C;
+	C << H, Y;
+	SVD<Matrix<double, Dynamic, 7> > usv(C);
+	Matrix<double, 4, 3> V_ab = usv.matrixV().block<4, 3>(0, n);
+	Matrix<double, Dynamic, 3> 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<double, 3, 4> result;
+	V_bb.transpose().qr().solve(-V_ab.transpose(), &result);
+
+	// Results only deserve single precision.
+	bias = result.col(3).cast<float>();
+	accelSensitivity = result.block<3, 3>(0, 0).cast<float>();
+
+#endif
+}

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 <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];
+	}
+}
+
+

ground/openpilotgcs/src/plugins/config/twostep.cpp

@@ -0,0 +1,599 @@
+/**
+ ******************************************************************************
+ *
+ * @file       twostep.cpp
+ * @author     Jonathan Brandmeyer <jonathan.brandmeyer@gmail.com>
+ * 	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 <calibration.h>
+#include <assertions.h>
+
+#include <Eigen/Cholesky>
+#include <Eigen/QR>
+#include <Eigen/LU>
+#include <cstdlib>
+
+#undef PRINTF_DEBUGGING
+
+#include <iostream>
+#include <iomanip>
+
+/*
+ * 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<double, 6, 1>
+dnormb_dtheta(const Matrix<double, 6, 1>& theta)
+{
+	return (Matrix<double, 6, 1>() << 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<double, 6, 1>
+dJdb(const Matrix<double, 6, 1>& centerL, 
+		double centerMag,
+		const Matrix<double, 6, 1>& theta, 
+		const Matrix<double, 6, 1>& dbdtheta,
+		double mu,
+		double noise)
+{
+	// \frac{\delta}{\delta \theta'} |b(\theta')|^2
+	Matrix<double, 6, 1> 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<double, 1, 6>
+theta_to_sane(const Matrix<double, 6, 1>& theta)
+{
+	return (Matrix<double, 6, 1>() << 
+		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<double, Dynamic, 6> fullSamples(n_samples, 6);
+	// \hbar{L} by eq 33, simplified by obesrving that the
+	Matrix<double, 1, 6> centerSample = Matrix<double, 1, 6>::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<double>(), 
+			-(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<double, Dynamic, 6> 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<double, 6, 1> estimateSummation = Matrix<double, 6, 1>::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<double, 6, 6> P_theta_theta_inv = (1.0f/noise)*
+			centeredSamples.transpose()*centeredSamples;
+#ifdef PRINTF_DEBUGGING
+	SelfAdjointEigenSolver<Matrix<double, 6, 6> > 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<double, 6, 1> estimate;
+	// By eq 37 a), \tilde{Fisher}^-1
+	P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);
+
+#ifdef PRINTF_DEBUGGING
+	Matrix<double, 6, 6> P_theta_theta;
+	P_theta_theta_inv.ldlt().solve(Matrix<double, 6, 6>::Identity(), &P_theta_theta);
+	SelfAdjointEigenSolver<Matrix<double, 6, 6> > 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<double, 1, 6> db_dtheta = dnormb_dtheta(estimate);
+
+		Matrix<double, 6, 1> 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<double, 6, 6> scale = P_theta_theta_inv + 
+			(double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta;
+
+		// eq 14b, mutatis mutandis (grumble, grumble)
+		Matrix<double, 6, 1> 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<double, 9, 1>& 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<double, 9, 1>
+dnormb_dtheta(const Matrix<double, 9, 1>& 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<double, 9, 1>() << 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<double, 9, 1>
+dJ_dtheta(const Matrix<double, 9, 1>& centerL, 
+		double centerMag,
+		const Matrix<double, 9, 1>& theta, 
+		const Matrix<double, 9, 1>& dbdtheta,
+		double mu,
+		double noise)
+{
+	// \frac{\delta}{\delta \theta'} |b(\theta')|^2
+	Matrix<double, 9, 1> 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<double, Dynamic, 9> fullSamples(n_samples, 9);
+	// \hbar{L} by eq 52, simplified by obesrving that the common noise term
+	// makes this a simple average.
+	Matrix<double, 1, 9> centerSample = Matrix<double, 1, 9>::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<double>(), 
+			-(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<double, Dynamic, 9> 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<double, 9, 1> estimateSummation = Matrix<double, 9, 1>::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<double, 9, 9> P_theta_theta_inv = (1.0f/noise)*
+		centeredSamples.transpose()*centeredSamples;
+
+#ifdef PRINTF_DEBUGGING
+	SelfAdjointEigenSolver<Matrix<double, 9, 9> > 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<double, 9, 1> 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<Matrix3d> 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<float>();
+
+		(Matrix3f::Identity() + scale).ldlt().solve(
+				estimate.start<3>().cast<float>(), &bias);
+		std::cout << "\n\nestimated bias: " << bias.transpose()
+			<< "\nestimated scale:\n" << scale;
+#endif
+		
+		Matrix<double, 1, 9> db_dtheta = dnormb_dtheta(estimate);
+
+		Matrix<double, 9, 1> 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<double, 9, 9> scale = P_theta_theta_inv + 
+			(double(n_samples)/noise)*db_dtheta.transpose() * db_dtheta;
+
+		// eq 14b, mutatis mutandis (grumble, grumble)
+		Matrix<double, 9, 1> 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<Matrix3d> 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<float>();
+
+	(Matrix3f::Identity() + scale).ldlt().solve(
+			estimate.start<3>().cast<float>(), &bias);
+}
+