mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-06 17:46:07 +01:00
44e3466e0a
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3053 ebee16cc-31ac-478f-84a7-5cbb03baadba
52 lines
1.9 KiB
C++
52 lines
1.9 KiB
C++
#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>();
|
|
}
|