Magnetometer Calibration
Seungmin Lee1, Gihun Bae2 and Katie Kortum3
Instructor: James Goppert4, Brandon Wampler5 and Inseok Hwang6
Purdue University, Department of Aeronautics and Astronautics
An inertial navigation system (INS) uses gyros, accelerometers and the magnetometer to come
up with its position and attitude in space. It uses both because each one is more accurate at
different frequencies. The magnetometer is more accurate in the lower frequency ranges
whereas the gyros/accelerometers are more accurate in the higher frequency ranges. Because
of this, the gyros and accelerometers are corrected using the readings from the magnetometer in
lower frequency ranges and vice versa in the higher frequency ranges.
When the
magnetometer is not calibrated the lower frequency readings are off which can cause the
navigation solution to diverge. Because of this, it is very important that an accurate method
for calibrating the magnetometer is devised. In this paper, several calibration methods for the
vector magnetometer are presented. The calibration methods can be divided into three different
categories. The initial method that was examined was a more physical approach to calibration.
The second category was calibration using the linear method. The third category was the
nonlinear method which was the most accurate and was decided upon and utilized for the
calibration of the Inertial Mass Unit (IMU) calibration. To evaluate the performance of the
calibration method, the simulation experiment was performed. The following results show how
accurate the method was.
I.
Introduction
The goal for this semester was to calibrate the magnetometer on the INS. There were several
different methods that were examined over the course of the semester. Eventually the downhill
simplex method was decided upon and used. C++ was used mainly in the programming for the
magnetometer calibration.
Magnetic fields are vectors at any point in space. A magnetic field has a magnitude and direction,
and can be separated into three component vectors (x, y, z) which are at right angles to one another.
These are the rectangular components of the vector. Many types of magnetometers have been used
to measure magnetic fields in many applications. The high accuracy measurement of the magnetic
field requires precisely calibrating the magnetometer. The biases and scaled factors are sought in
this work which minimize the sum of the distances, or the sum of the squared distances of each point
from an ellipsoid characterized by the bias and scale factors of the magnetic field sensors.
II. Development of Calibration Methods: Physical Approach
The first methods that were examined for potential use in calibration of the magnetometer were more
physical methods. These were methods where a computer was not used for the calibration itself but
merely just to update the magnetometer. Both physical methods that are discussed in this section
require the use of an already calibrated magnetometer in conjunction with the un-calibrated
magnetometer. Because of this, several departments within the university were contacted in the
1
2
3
4
5
6
Undergraduate Student, School of Aeronautics & Astronautics, Purdue University
Undergraduate Student, School of Aeronautics & Astronautics, Purdue University
Undergraduate Student, School of Aeronautics & Astronautics, Purdue University
Graduate Student, School of Aeronautics & Astronautics, Purdue University
Graduate Student, School of Aeronautics & Astronautics, Purdue University
Professor, School of Aeronautics & Astronautics, Purdue University
1
search for a working and calibrated magnetometer and nothing was found.
It was initially thought that the easiest way to calibrate the magnetometer in the IMU would be to use
the swinging method. This method would have been achieved by acquiring a working magnetometer
to measure the magnetic field with that and determine how far off the IMU magnetometer was and
adjust it from there. It was quickly determined that this method would not be accurate so it was
thrown out without much more investigation.
The next method that was examined was the 3-axis calibration method7. For this method a robotic
arm similar to that shown in Figure 1 is used.
Figure 1: 6 Degrees of Freedom Robotic Arm. 8
Next, the magnetometer is fixed to the end of the arm and each of the joints are incrementally moved
to change the position of the magnetometer. Since the movements of the robotic arm are very
accurate the inconsistencies between where the magnetometer is and where it thinks it is can be
determined. Once it is known how far off the readings are they can be decreased and eventually
eliminated. This method was an option for the calibration of the IMU because there is a 5-axis
machine at the Purdue airport that can be used to do what the robotic arm does. This method was not
investigated much further due to the fact that it was not as accurate as was needed.
7
Calibrating a Triaxial Accelerometer-Magnetometer: Erin L. Renk, Walter Collins, Matthew Rizzo, Fuju Lee,
Dennis Bernstein
8
Image from Calibrating a Triaxial Accelerometer-Magnetometer
2
III. Development of Calibration Methods: Linear Approach
Fitting an ellipsoid to set of 3D scattered points occurs in the areas of pattern recognition and 3D
graphics. Ellipsoids though a bit simple in representing 3D shapes, can provide information of center
and orientation of an object. In this paper, we aim to develop and effective and efficient ellipsoid
fitting algorithm and will consider those fitting methods based on the geometric distance or the
parametric representation of an ellipsoid as these will inevitably lead to a nonlinear optimization
procedure.
In order to make that the frequency of the Inertial Measurement Unit (IMU) must be faster than the
frequency of the calibration, Frequency ratio was calculated by using IMU frequency and
magnetometer frequency given. The code that used to calculate frequency ratio is shown below.
𝜔𝑟𝑎𝑡𝑖𝑜 =
𝜔𝐼𝑀𝑈
𝜔𝑚𝑎𝑔
Where, ωratio = frequency ratio, ωIMU = frequency of the IMU, ωmag = frequency of
magnetometer
// Calculate frequency ratio
const static int freqRatio = freqImu/freqMagCalib;
std::cout << "Frequency Ratio: " << freqRatio << std::endl;
if (freqRatio <=0)
{
std::cout << "The frequency of the imu: " << freqImu
<< " must be faster than that of the calibration: " <<
freqMagCalib << "." << std::endl;
exit(-1);
}
After calculating the frequency ratio, the data from the magnetometer was collected by updating IMU
and counter for IMU. The collected data was stored as variables in format of (x, y, z) for the
drawing of point cloud. Figure 1 shows that the red point cloud was scattered on all sides. The
point cloud was broken as IMU stopped collecting data. Also, the point cloud was the criterion for
calibrating linear and nonlinear solution in 3D ellipsoid fitting. The code that was used to make the
point cloud is shown below.
Figure 2 – Point cloud
// Store Data
x(n) = imu.mb[0];
y(n) = imu.mb[1];
z(n) = imu.mb[2];
// Store for drawing
pointCloud->addPoint(osg::Vec3(x(n),y(n),z(n)),red);
3
The design matrix was created in order to calibrate for linear solution in 3D ellipsoid fitting. The
size of the design matrix depends on the number of variable coefficients in the equation. We can
ignore the cross terms such as x*y since the ellipsoid is assumed to be oriented along the x, y, z axis
(no cross coupling in the magnetic sensors). The design matrix which has 6 × n dimension consists
of 6 elements that represent the quadratic ellipsoid equation. The equation and code for the
quadratic ellipsoid equation were followed below.
𝑥 2 + 𝑦 2 + 𝑧 2 + 2𝑥 + 2𝑦 + 2𝑧 = 1i
𝐷 = [𝑥 2 𝑦 2 𝑧 2 2𝑥 2𝑦 2𝑧]
Create Design Matrix for Linear Solution
std::cout << "Creating design matrix." << std::endl;
matrix<double> D(n,dim);
for (int i=0;i<n;i++)
{
D(i,0) = x(i)*x(i);
D(i,1) = y(i)*y(i);
D(i,2) = z(i)*z(i);
D(i,3) = 2*x(i);
D(i,4) = 2*y(i);
D(i,5) = 2*z(i);
}
It is important to define the ellipsoid and its properties before understanding the linear solution
completely. The equation of a standard axis-aligned ellipsoid body in a xyz-Cartesian coordinate
system is
𝑥2 𝑦2 𝑧2
+
+ =1
𝑎2 𝑏 2 𝑐 2
Where a and b are the equatorial radii along the x and y axes and c is the polar radius along the z-axis,
all of which are fixed positive real numbers that determine the shape of the ellipsoid. Therefore the
ellipse can be represented by the equation:
𝐷𝑉 = 1(𝑛 × 1)
Where V is the (6x1) design vector containing the coefficients of the ellipsoid that relate to the bias
and scale factors, and D is a vector by (n x 6). In this case and in the general case, the eigenvectors of
D define the principal directions of the ellipsoid and the inverse of the square root of the eigenvalues
are the corresponding equatorial radii.
The pseudo-inverse was used to obtain a matrix can serve as the inverse in some sense for non square
matrices. The pseudo-inverse can exist for an arbitrary matrix, and when a matrix has an inverse,
then its inverse and the pseudo-inverse are the same. The code used to calculate the matrix v is
shown below. The function prod is used for vector multiplication. The linear solution depends on
the center point, bias, radii and scale factor which were calculated by using the element of the matrix
V.
matrix<double> v(6,n);
v = prod(pinv(D),ones(n,1));
4
𝑣0
𝑣1
𝑣2
𝑉= 𝑣
3
𝑣4
[𝑣5 ]
𝑣3 𝑣3 𝑣4 𝑣4 𝑣5 𝑣5
𝛾 =1+(
+
+
)
𝑣0
𝑣1
𝑣2
𝑐𝑒𝑛𝑡𝑒𝑟 = (−
𝑣3 𝑣4 𝑣5
,− ,− )
𝑣0 𝑣1 𝑣2
𝑏𝑖𝑎𝑠 = (
𝑣3 𝑣4 𝑣5
, , )
𝑣0 𝑣1 𝑣2
𝛾
𝛾
𝛾
𝑟𝑎𝑑𝑖𝑖 = (√ , √ , √ )
𝑣0 𝑣1 𝑣2
𝑠𝑐𝑎𝑙𝑒 𝑓𝑎𝑐𝑡𝑜𝑟 =
1
1
1
,
,
𝛾
𝛾
𝛾
√𝑣 √𝑣 √𝑣
( 0
1
2)
The first three elements of the linear solution represent the center point that was computed above.
The last three elements indicate the radii of the linear ellipsoid. Figure 3 shows that the blue linear
ellipsoid is placed to the red point cloud closely. To draw the linear ellipsoid in Figure 2, the code
was created.
𝑙𝑖𝑛𝑒𝑎𝑟 𝑠𝑜𝑙𝑢𝑡𝑖𝑜𝑛 = (−
𝑣3 𝑣4
𝑣5
𝛾
𝛾
𝛾
,− ,− ,√ ,√ ,√ )
𝑣0 𝑣1 𝑣2 𝑣0 𝑣1 𝑣2
Figure 3 - Linear Ellipsoid
double gam = 1 + ( v(3,0)*v(3,0) / v(0,0) + v(4,0)*v(4,0) / v(1,0) + v(5,0)*v(5,0) / v(2,0) );
for (int i=0;i<3;i++)
{
linearSolution(i) = -v(i+3,0)/v(i,0);
linearSolution(i+3) = sqrt(gam/v(i,0));
5
}
// Draw linear ellipsoid
Ellipsoid * linearEllipsoid = new Ellipsoid(osg::Vec3(linearSolution(3),linearSolution(4),linearSolution(5)),
256osg::Vec3(linearSolution(0),linearSolution(1),linearSolution(2)),3,blue);
IV. Development of Calibration Methods: Non-linear Approach
The non-linear solution is more complicated to obtain than the linear solution, corresponding to 3D
ellipsoid fitting. The initial guess typically comes from the linear solution. There were a lot of
methods to calibrate non-linear solution. In terms of accuracy and simplicity of programming, we
selected the downhill simplex method for non-linear function minimization. Multidimensional
minimization consists of finding the minimum of a function of more than one independent variable.
The downhill simplex method which requires function evaluations is the best method to use if the
figure of merit is “get something working quickly” for problem whose computational burden is small
(Numerical Recipes in C: The Art of Scientific Computing). A simplex is the geometrical figure,
consisting in N dimensions, of N + 1 vertices and all their interconnecting line segments. The
algorithm is supposed to make its own way downhill through the unimaginable complexity of an 𝑁dimensional topography, until it encounters a minimum. If we think of one of these points as being the
initial starting point P0, then we can take the other 𝑁 points to be
𝑃𝑖 = 𝑃0 + ∆𝑒𝑖
Where the e𝑖 ’s are 𝑁 unit vectors, and where ∆ is a constant that is our guess of the problem’s
characteristic length scale.
The downhill simplex method takes a series of steps, most steps moving the point of the simplex
where the function is highest point through the opposite face of the simplex to a lower point. These
steps are called reflections, and they are constructed to conserve the volume of the simplex. The
method expands the simplex in one or another direction to take larger steps.
Figure 4 – (a) a reflection away, (b) a reflection and expansion away, (c) a contraction along one
dimension form high point or (d) a contraction along all dimensions toward the low point,
( (Numerical Recipes in C: The Art of Scientific Computing)
The equations which are given below help extrapolate by a factor through the face of the simplex
across from the high point, tries and replace the high point. Downhill simplex specified by the matrix
vertices (dim, dim+1). Its rows are n-dimensional vectors that are the vertices of the starting simplex.
In other words, this downhill simplex computed cost for all vertices.ii
𝑓𝑎𝑐𝑡𝑜𝑟1 =
(1 − 𝑓𝑎𝑐𝑡𝑜𝑟𝑠𝑡𝑟𝑒𝑡𝑐ℎ )
𝑑𝑖𝑚𝑒𝑛𝑠𝑖𝑜𝑛
𝑓𝑎𝑐𝑡𝑜𝑟2 = 𝑓𝑎𝑐𝑡𝑜𝑟1 − 𝑓𝑎𝑐𝑡𝑜𝑟𝑠𝑡𝑟𝑒𝑡𝑐ℎ
6
𝑁
𝑣𝑒𝑟𝑡𝑒𝑥𝑡𝑟𝑖𝑒𝑑 = ∑ 𝑒𝑙𝑒𝑚𝑒𝑛𝑡 ∙ 𝑓𝑎𝑐𝑡𝑜𝑟1 − 𝑣𝑒𝑟𝑡𝑖𝑐𝑒𝑠ℎ𝑖𝑔ℎ ∙ 𝑓𝑎𝑐𝑡𝑜𝑟2
𝑖=1
double DownhillSimplex::tryStretch(matrix<double> & vertices, vector<double> cost, vector<double> elementSum, int highIndex, double s
tretchFactor)
{
int dimension = vertices.size1();
double factor1 = (1.0 - stretchFactor)/dimension;
double factor2 = factor1 - stretchFactor;
vector<double> triedVertex(dimension);
for (int i = 0; i< dimension; i++)
{
triedVertex(i) = elementSum(i)*factor1 - vertices(i, highIndex)*factor2;
}
double tryCost = costFunction->eval(triedVertex);
if (tryCost < cost(highIndex))
{
//std::cout << "replacing vertex with tried vertex" << std::endl;
for (int i=0; i<dimension; i++) vertices(i,highIndex) = triedVertex(i);
}
return tryCost;
}
To begin with, it has to be determined which point is the highest, next-highest, and lowest,
by looping over the points in the simplex.
double tiny = 1e-10;
int dim = initialGuess.size();
vector<double> vertex(dim), cost(dim+1);
matrix<double> vertices(dim,dim+1);
solution = initialGuess;
for (int j=0;j<dim+1;j++)
{
for (int i=0; i<dim;i++) vertices(i,j) = initialGuess(i);
if (j>0) vertices(j-1,j) += delta;
}
Next, it helps compute the fractional range from highest to lowest and return if satisfactory. The
loop in the code below was stopped, if the tolerance is satisfied perfectly. It is important to return a
solution in this part. Based on the minimum and maximum distance cost, the core of algorithm in
Figure 3 was created by beginning a new iteration. Firstly, we extrapolated by a factor -1 through
the face of the simplex across from the high point, reflect the simplex from the high point. If it gives
a result better than the best point, an additional extrapolation by a factor 2 was tried. The reflected
point was worse than the second-highest, so we looked for an intermediate lower point and did a onedimensional contraction. If we couldn’t seem to get rid of that high point, the better contraction was
performed around the lowest point. This procedure kept going until the convergence value is less
than the tolerance range. The equation below was used to compute the convergence.
𝑐𝑜𝑛𝑣𝑒𝑟𝑔𝑒𝑛𝑐𝑒 =
2|𝑐𝑜𝑠𝑡𝑚𝑎𝑥 − 𝑐𝑜𝑠𝑡𝑚𝑖𝑛 |
|𝑐𝑜𝑠𝑡𝑚𝑎𝑥 | + |𝑐𝑜𝑠𝑡𝑚𝑖𝑛 | + 𝑡𝑖𝑛𝑦
for (int j=0;j<dim+1;j++)
{
//std::cout << "j: " << j << std::endl;
for (int i=0;i<dim;i++) vertex(i) = vertices(i,j);
cost(j) = costFunction->eval(vertex);
if ( j==0)
{
minCost = cost(j);
maxCost = cost(j);
nextMaxCost = cost(j);
minJ = j;
maxJ = j;
nextMaxJ = j;
}
7
else if ( cost(j) < minCost )
{
minCost = cost(j);
minJ = j;
}
else if ( cost(j) > maxCost )
{
nextMaxCost = maxCost;
maxCost = cost(j);
nextMaxJ = maxJ;
maxJ = j;
}
else if (cost(j) > nextMaxCost)
{
nextMaxCost = cost(j);
nextMaxJ = j;
}
// return if converged
convergence = 2*abs(maxCost-minCost)/(abs(maxCost)+abs(minCost)+tiny);
//std::cout << "convergence: " << convergence << std::endl;
if ( convergence < rtol)
{
std::cout << "Solution converged." << std::endl;
break;
}
else if (functionEvals > maxFunctionEvals)
{
std::cout << "Warning: Solution did not converge. ";
std::cout << "You exceeded the maximum # of function evaluations." << std::endl;
break;
}
// find element sum of vertices
vector<double> elementSum(dim+1);
double tryCostInvert, tryCostDoubleInvert, tryCostContraction;
for (int i=0;i<dim;i++)
{
elementSum(i) = 0;
for (int j=0;j<dim+1;j++) elementSum(i)+=vertices(i,j);
}
// try stretch by -1
tryCostInvert = tryStretch(vertices, cost, elementSum, maxJ, -1);
// if cost try(-1) < max vertex cost
if (tryCostInvert < maxCost)
{
// try stretch by -2
tryCostDoubleInvert = tryStretch(vertices, cost, elementSum, maxJ, 2);
// if cost try(-2) < cost try(-1)
//
return try(-2) vertex
// else
//
return try(-1) vertex
// end if
if (tryCostDoubleInvert < tryCostInvert)
{
//std::cout << "Double Invert" << std::endl;
}
else
{
//std::cout << "Invert" << std::endl;
}
}
// else
else
{
// try single dimension contraction
tryCostContraction = tryStretch(vertices, cost, elementSum, maxJ, -0.5);
// if single dim contraction cost lower than max cost return single
// dim contraction
if (tryCostContraction < maxCost)
{
//std::cout << "Contraction" << std::endl;
}
// else do a multi dimension contraction about the low point
else
{
8
//std::cout << "Multi Dimension Contraction" << std::endl;
for (int j= 0; j < dim+1; j++)
{
if (j != minJ)
{
for (int i=0; i<dim;i++)
{
vertices(i,j) = .5*(vertices(i,minJ) + vertices(i,j));
}
}
}
}
}
There are two ways to draw the non-linear ellipsoid fitting, the distance cost and squared cost of the
ellipsoid. In order to obtain these values, we had to figure out again what center point, radii, unit and
point were. We brought the vertex from the non-linear solution to set up center point and radii.
The unit, cost, squared cost were calculated by using these equations given by,
𝑢𝑛𝑖𝑡 =
(𝑝𝑜𝑖𝑛𝑡 − 𝑐𝑒𝑛𝑡𝑒𝑟)
𝑛𝑜𝑟𝑚_2(𝑝𝑜𝑖𝑛𝑡 − 𝑐𝑒𝑛𝑡𝑒𝑟)
1
𝑡=√
𝑢𝑛𝑖𝑡0 𝑢𝑛𝑖𝑡0
𝑢𝑛𝑖𝑡1 𝑢𝑛𝑖𝑡1
𝑢𝑛𝑖𝑡2 𝑢𝑛𝑖𝑡2
+
+
𝑟𝑎𝑑𝑖𝑖0 𝑟𝑎𝑑𝑖𝑖0 𝑟𝑎𝑑𝑖𝑖1 𝑟𝑎𝑑𝑖𝑖1 𝑟𝑎𝑑𝑖𝑖2 𝑟𝑎𝑑𝑖𝑖2
𝑝 = 𝑛𝑜𝑟𝑚_2(𝑝𝑜𝑖𝑛𝑡 − 𝑐𝑒𝑛𝑡𝑒𝑟)
𝑐𝑜𝑠𝑡 = 𝑐𝑜𝑠𝑡 + (𝑝 − 𝑡)
𝑐𝑜𝑠𝑡𝑠𝑞𝑢𝑎𝑟𝑒𝑑 = 𝑐𝑜𝑠𝑡 + (𝑝 − 𝑡)2
The sum of distance squared was used to draw non-linear ellipsoid cost. For sum of distance cost,
the non-linear ellipsoid cost colored of pink was plotted in Figure 4. Figure 5 shows that the green
ellipsoid present the sum of distance squared cost. The ellipsoid of the distance squared cost is
smaller than that of the distance cost. Also, other ellipsoids from true solution, linear solution were
shown as well in Figure 5 to compare how the ellipsoids were located according to the point cloud.
The code is attached below to ensure calculation.
double cost = 0, p, t;
vector<double> center(3), radii(3), unit(3), point(3);
center = subrange(vertex,0,3);
radii = subrange(vertex,3,6);
int n = x.size();
for (int i=0;i<n;i++)
{
point(0) = x(i);
point(1) = y(i);
point(2) = z(i);
unit = (point-center)/norm_2(point-center);
t = sqrt(1/(
unit(0)*unit(0)/(radii(0)*radii(0)) +
unit(1)*unit(1)/(radii(1)*radii(1)) +
unit(2)*unit(2)/(radii(2)*radii(2))
));
p = norm_2(point-center);
cost = cost + abs(p-t);
costSquare = cost + (p-t)*(p-t);
}
return cost;
}
9
Figure 5 - Non-linear ellipsoid (distance squared)
Figure 6 - Non-linear ellipsoid (distance cost)
V. Problem
From collecting all data from the magnetometer, we tested these linear and non-linear calibration
methods again and again. The linear solution performed poorly in that the solution’s center tended to
be close to the point cloud center than the generating ellipsoid. On the other hand, there were some
conflicts for non-linear ellipsoid. Because all of the calculations were based on the downhill simplex
method for non-linear ellipsoid, even though plotted the non-linear solution correctly, we got in
trouble the ellipsoid moving to the point cloud. In detail, when the ellipsoid moves to the point
cloud, the center point and radii should be changed continuously, according to the distance from the
point cloud. Sometimes, the ellipsoid went through the point cloud due to failure to change radii
correspondingly. However, we debugged the downhill simplex code and it now works correctly.
Figure 6 demonstrates the non-linear ellipsoid (squared cost) of green was moving steadily toward the
red point cloud.
10
Figure 7 – Non-linear ellipsoid movement
VI. Conclusion
From this project, we have learned the concept and methods of calibration by using 3D ellipsoid
fitting. When facing this project at the beginning of this semester, we really didn’t understand
exactly what calibration is and how it works. However, following the instructor’s comments and
reference books step by step, we began to comprehend how the ellipsoid was formed, how physical,
linear and non-linear solution method could work and how the methods calibrated their ellipsoid to
the point cloud. The downhill simplex method for non-linear solution was very efficient and easier
to develop than other non-linear methods. The sum of distance cost and distance cost squared helped
us to how the shape of ellipsoid changed to the point. It was good to understand the differences
among four ellipsoids in Figure 5, and the values from the linear solution and non-linear solution were
accurate and easy to fit 3D ellipsoid. The non-linear optimization gave results close to the true
solution with the sum of the distance squared cost. We hope this paper aids in understanding
magnetometer calibration.
VII. Appendix
Downhillsimplex.cc
/*
* DownhillSimplex.cc
* Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
*
* DownhillSimplex.cc 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.
*
* DownhillSimplex.cc 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, see <http://www.gnu.org/licenses/>.
*/
#include "DownhillSimplex.h"
using namespace boost::numeric::ublas;
using namespace uvsim;
using namespace boost::numeric::bindings::lapack;
namespace uvsim
{
DownhillSimplex::DownhillSimplex(DownhillSimplexCost * costFunction, DownhillSimplexCallback * callback,
int callbackPeriod) :
costFunction(costFunction), callback(callback), callbackPeriod(callbackPeriod)
{
callback->attach(this);
}
double DownhillSimplex::tryStretch(matrix<double> & vertices, vector<double> cost, vector<double>
elementSum, int highIndex, double stretchFactor)
{
int dimension = vertices.size1();
double factor1 = (1.0 - stretchFactor)/dimension;
double factor2 = factor1 - stretchFactor;
vector<double> triedVertex(dimension);
11
for (int i = 0; i< dimension; i++)
{
triedVertex(i) = elementSum(i)*factor1 - vertices(i, highIndex)*factor2;
}
double tryCost = costFunction->eval(triedVertex);
//std::cout << "stretch factor: " << stretchFactor << std::endl;
//std::cout << "tried vertex: " << triedVertex << std::endl;
//std::cout << "try cost: " << tryCost << std::endl;
if (tryCost < cost(highIndex))
{
//std::cout << "replacing vertex with tried vertex" << std::endl;
for (int i=0; i<dimension; i++) vertices(i,highIndex) = triedVertex(i);
}
return tryCost;
}
void DownhillSimplex::solve(vector<double> initialGuess, double delta, double rtol, int maxFunctionEvals)
{
double tiny = 1e-10;
double convergence;
int dim = initialGuess.size();
vector<double> vertex(dim), cost(dim+1);
double minCost, maxCost, nextMaxCost;
int minJ, maxJ, nextMaxJ;
matrix<double> vertices(dim,dim+1);
solution = initialGuess;
for (int j=0;j<dim+1;j++)
{
for (int i=0; i<dim;i++) vertices(i,j) = initialGuess(i);
if (j>0) vertices(j-1,j) += delta;
}
// while change < tolerance
for (int functionEvals = 0;; functionEvals++)
{
//std::cout << 100.0*functionEvals/maxFunctionEvals << "% Complete" << std::endl;
//std::cout << "center: " << subrange(solution,0,3) << std::endl;
//std::cout << "radii: " << subrange(solution,3,6) << std::endl;
//clear();
//std::cout << "vertices: " << vertices << std::endl;
//
find vertex with the min cost, max cost, and next max cost
for (int j=0;j<dim+1;j++)
{
//std::cout << "j: " << j << std::endl;
for (int i=0;i<dim;i++) vertex(i) = vertices(i,j);
cost(j) = costFunction->eval(vertex);
if ( j==0)
{
minCost = cost(j);
maxCost = cost(j);
nextMaxCost = cost(j);
minJ = j;
maxJ = j;
nextMaxJ = j;
}
else if ( cost(j) < minCost )
{
minCost = cost(j);
minJ = j;
}
else if ( cost(j) > maxCost )
{
nextMaxCost = maxCost;
maxCost = cost(j);
nextMaxJ = maxJ;
maxJ = j;
}
else if (cost(j) > nextMaxCost)
{
nextMaxCost = cost(j);
nextMaxJ = j;
}
for (int i=0;i<dim;i++) solution(i) = vertices(i,minJ);
if ((functionEvals % callbackPeriod) == 0) callback->update();
//std::cout << "minJ: " << minJ << std::endl;
//std::cout << "maxJ: " << maxJ << std::endl;
//std::cout << "nextMaxJ: " << nextMaxJ << std::endl;
}
//std::cout << "cost: " << cost << std::endl;
//std::cout << "minJ: " << minJ << std::endl;
//std::cout << "maxJ: " << maxJ << std::endl;
//std::cout << "nextMaxJ: " << nextMaxJ << std::endl;
//
return if converged
convergence = 2*abs(maxCost-minCost)/(abs(maxCost)+abs(minCost)+tiny);
//std::cout << "convergence: " << convergence << std::endl;
if ( convergence < rtol)
12
{
std::cout << "Solution converged." << std::endl;
break;
}
else if (functionEvals > maxFunctionEvals)
{
std::cout << "Warning: Solution did not converge. ";
std::cout << "You exceeded the maximum # of function evaluations." << std::endl;
break;
}
//
find element sum of vertices
vector<double> elementSum(dim+1);
double tryCostInvert, tryCostDoubleInvert, tryCostContraction;
for (int i=0;i<dim;i++)
{
elementSum(i) = 0;
for (int j=0;j<dim+1;j++) elementSum(i)+=vertices(i,j);
}
//
try stretch by -1
tryCostInvert = tryStretch(vertices, cost, elementSum, maxJ, -1);
//
if cost try(-1) < max vertex cost
if (tryCostInvert < maxCost)
{
//
try stretch by -2
tryCostDoubleInvert = tryStretch(vertices, cost, elementSum, maxJ, 2);
//
if cost try(-2) < cost try(-1)
//
return try(-2) vertex
//
else
//
return try(-1) vertex
//
end if
if (tryCostDoubleInvert < tryCostInvert)
{
//std::cout << "Double Invert" << std::endl;
}
else
{
//std::cout << "Invert" << std::endl;
}
}
//
else
else
{
// try single dimension contraction
tryCostContraction = tryStretch(vertices, cost, elementSum, maxJ, -0.5);
// if single dim contraction cost lower than max cost return single
// dim contraction
if (tryCostContraction < maxCost)
{
//std::cout << "Contraction" << std::endl;
}
// else do a multi dimension contraction about the low point
else
{
//std::cout << "Multi Dimension Contraction" << std::endl;
for (int j= 0; j < dim+1; j++)
{
if (j != minJ)
{
for (int i=0; i<dim;i++)
{
vertices(i,j) = .5*(vertices(i,minJ) + vertices(i,j));
}
}
}
}
}
//std::cin.ignore();
// end while
}
}
void DownhillSimplexCallback::attach(DownhillSimplex * solver)
{
this->solver = solver;
}
} // uvsim
// vim:ts=4:sw=4
13
magcalibNonLinSim.cc
/*
* magCalibNonLinSim.cc
* Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
*
* magCalibNonLinSim.cc 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.
*
* magCalib.cc 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, see <http://www.gnu.org/licenses/>.
*/
#include
#include
#include
#include
#include
#include
"uvsim/utilities/DownhillSimplex.h"
"uvsim/visualization/SimpleViewer.h"
<boost/random/normal_distribution.hpp>
<boost/random/random_number_generator.hpp>
<boost/random/mersenne_twister.hpp>
<boost/random/variate_generator.hpp>
using namespace uvsim;
using namespace boost;
class EllipsoidDistanceCost : public DownhillSimplexCost
{
public:
vector<double> x, y, z;
EllipsoidDistanceCost(vector<double> x, vector<double> y, vector<double> z) : x(x), y(y), z(z) {};
double eval(vector<double> vertex)
{
double cost = 0, p, t;
vector<double> center(3), radii(3), unit(3), point(3);
center = subrange(vertex,0,3);
radii = subrange(vertex,3,6);
int n = x.size();
for (int i=0;i<n;i++)
{
point(0) = x(i);
point(1) = y(i);
point(2) = z(i);
unit = (point-center)/norm_2(point-center);
t = sqrt(1/(
unit(0)*unit(0)/(radii(0)*radii(0)) +
unit(1)*unit(1)/(radii(1)*radii(1)) +
unit(2)*unit(2)/(radii(2)*radii(2))
));
p = norm_2(point-center);
cost = cost + abs(p-t);
}
return cost;
}
virtual ~EllipsoidDistanceCost() {};
};
class EllipsoidDistanceSquaredCost : public DownhillSimplexCost
{
public:
vector<double> x, y, z;
EllipsoidDistanceSquaredCost(vector<double> x, vector<double> y, vector<double> z) : x(x), y(y), z(z)
{};
double eval(vector<double> vertex)
{
double cost = 0, p, t;
vector<double> center(3), radii(3), unit(3), point(3);
center = subrange(vertex,0,3);
radii = subrange(vertex,3,6);
int n = x.size();
for (int i=0;i<n;i++)
{
point(0) = x(i);
point(1) = y(i);
point(2) = z(i);
unit = (point-center)/norm_2(point-center);
t = sqrt(1/(
unit(0)*unit(0)/(radii(0)*radii(0)) +
unit(1)*unit(1)/(radii(1)*radii(1)) +
unit(2)*unit(2)/(radii(2)*radii(2))
));
p = norm_2(point-center);
14
cost = cost + (p-t)*(p-t);
}
return cost;
}
virtual ~EllipsoidDistanceSquaredCost() {};
};
class EllipsoidCallback : public DownhillSimplexCallback
{
public:
EllipsoidCallback(Ellipsoid * ellipsoid) : ellipsoid(ellipsoid) {};
virtual ~EllipsoidCallback() {};
virtual void update()
{
vector<double> v = getSolver()->getSolution();
ellipsoid->setParam(osg::Vec3(v(3),v(4),v(5)),osg::Vec3(v(0),v(1),v(2)));
}
private:
Ellipsoid * ellipsoid;
};
int main (int argc, char const* argv[])
{
// Ellipse Test Case
// radii
const static double a = 105;
const static double b = 110;
const static double c = 135;
// center
const static double cx = 525;
const static double cy = 490;
const static double cz = 540;
// Constants
// Non-linear Solver
int callbackPeriod = 10;
double delta = 10;
double rtol = 1e-14;
int maxFunctionEvals = 5000;
// Viewer Related
float fps = 20;
osg::Vec3d manipCenter(512,512,512);
double manipDistance = 1000;
float frameSize = 5;
double fogStart=1000000;
double fogEnd=10000000;
// Data Related
const static int maxN=10000;
const static int dim = 6;
// Frequencies
const static int freqImu = 520, freqMagCalib = 100;
// Colors
const static osg::Vec4d white(1,1,1,1), red(1,0,0,1), green(0,1,0,1), blue(0,0,1,1), pink(1,0,1,1);
// Variables
// Data
vector<double> x(maxN), y(maxN), z(maxN);
// Declare the root node
osg::Group *root = new osg::Group;
// Solutions
vector<double> trueSolution(dim);
trueSolution(0) = cx;
trueSolution(1) = cy;
trueSolution(2) = cz;
trueSolution(3) = a;
trueSolution(4) = b;
trueSolution(5) = c;
vector<double> linearSolution(dim);
vector<double> initialGuess(dim);
// Visualization
// Point Cloud
osg::ref_ptr<uvsim::PointCloud> pointCloud = new PointCloud(5);
root->addChild((pointCloud->root).get());
15
// Input
// ratios of ellipse
double r1, r2;
std::cout << "ratio of ellipse theta (0-1): ";
std::cin >> r1;
std::cout << "ratio of ellipse phi (0-1): ";
std::cin >> r2;
// noise standard deviation
double noiseStdDev;
std::cout << "noise standard deviation: ";
std::cin >> noiseStdDev;
// Setup
// Create random number generator
mt19937 randgen(static_cast<unsigned int>(std::time(0)));
normal_distribution<double> noise(0, noiseStdDev);
variate_generator<mt19937, normal_distribution<double> > noiseGen(randgen, noise);
// Calculate frequency ratio
const static int freqRatio = freqImu/freqMagCalib;
std::cout << "Frequency Ratio: " << freqRatio << std::endl;
if (freqRatio <=0)
{
std::cout << "The frequency of the imu: " << freqImu
<< " must be faster than that of the calibration: " <<
freqMagCalib << "." << std::endl;
exit(-1);
}
// Launch the viewer thread
SimpleViewer simpleViewer(0,fps,root,manipDistance,manipCenter,frameSize,fogStart,fogEnd);
simpleViewer.start();
// Collect data
int n = 0; // set counter to zero
for (double theta=0;theta<r1*2*M_PI;theta+=M_PI/40)
{
for (double phi=0;phi<r2*M_PI;phi+=M_PI/20)
{
x(n) = a*sin(phi)*cos(theta) + cx + noiseGen();
y(n) = b*sin(phi)*sin(theta) + cy + noiseGen();
z(n) = c*cos(phi) + cz + noiseGen();
//std::cout << "n: " << n;
//std::cout << "\tx,y,z: " << x(n) << ", " << y(n) << ", " << z(n) << std::endl;
// Store for drawing
pointCloud->addPoint(osg::Vec3(x(n),y(n),z(n)),osg::Vec4(1,0,0,0));
// Check number of points
if (n++ > maxN)
{
std::cout << "Too many points." << std::endl;
exit(1);
};
}
}
// Create Design Matrix for Linear Solution
std::cout << "Creating design matrix." << std::endl;
matrix<double> D(n,dim);
for (int i=0;i<n;i++)
{
D(i,0) = x(i)*x(i);
D(i,1) = y(i)*y(i);
D(i,2) = z(i)*z(i);
D(i,3) = 2*x(i);
D(i,4) = 2*y(i);
D(i,5) = 2*z(i);
}
// Linear Solution
if (n > maxN)
{
std::cout << "N too large to compute svd" << std::endl;
return 1;
}
std::cout << "Finding linear solution for bias and scale factors. " << std::endl;
matrix<double> v(6,1);
v = prod(pinv(D),ones(n,1));
16
double gam = 1 + ( v(3,0)*v(3,0) / v(0,0) + v(4,0)*v(4,0) / v(1,0) +
v(5,0)*v(5,0) / v(2,0) );
for (int i=0;i<3;i++)
{
linearSolution(i) = -v(i+3,0)/v(i,0);
linearSolution(i+3) = sqrt(gam/v(i,0));
}
// Draw true ellipsoid
Ellipsoid * trueEllipsoid = new Ellipsoid(osg::Vec3(trueSolution(3),trueSolution(4),trueSolution(5)),
osg::Vec3(trueSolution(0),trueSolution(1),trueSolution(2)),3,white);
root->addChild(trueEllipsoid->root.get());
// Draw linear ellipsoid
Ellipsoid * linearEllipsoid = new
Ellipsoid(osg::Vec3(linearSolution(3),linearSolution(4),linearSolution(5)),
osg::Vec3(linearSolution(0),linearSolution(1),linearSolution(2)),3,blue);
root->addChild(linearEllipsoid->root.get());
// Non-Linear Solution
initialGuess = linearSolution;
// Draw nonlinear ellipsoid cost: sum of distance
Ellipsoid * nonlinearEllipsoid = new
Ellipsoid(osg::Vec3(initialGuess(3),initialGuess(4),initialGuess(5)),
osg::Vec3(initialGuess(0),initialGuess(1),initialGuess(2)),3,green);
root->addChild(nonlinearEllipsoid->root.get());
// Draw nonlinear ellipsoid cost: sum of distance squred
Ellipsoid * nonlinearEllipsoidSq = new
Ellipsoid(osg::Vec3(initialGuess(3),initialGuess(4),initialGuess(5)),
osg::Vec3(initialGuess(0),initialGuess(1),initialGuess(2)),3,pink);
root->addChild(nonlinearEllipsoidSq->root.get());
// Wait for user to being non-linear portion
std::cout << "Press <return> to begin solving non-linear problem: " << std::endl;
std::cin.ignore();
// Setup Non-Linear Optimization Cost: Distance
EllipsoidDistanceCost costFunction(subrange(x,0,n),subrange(y,0,n),subrange(z,0,n));
EllipsoidCallback callback(nonlinearEllipsoid);
DownhillSimplex solver(&costFunction, &callback,callbackPeriod);
// Setup Non-Linear Optimization Cost: Distance Squared
EllipsoidDistanceSquaredCost costFunctionSq(subrange(x,0,n),subrange(y,0,n),subrange(z,0,n));
EllipsoidCallback callbackSq(nonlinearEllipsoidSq);
DownhillSimplex solverSq(&costFunctionSq, &callbackSq,callbackPeriod);
// Solve
std::cout << "Solving Non-linear problem with Cost: Sum of Distances" << std::endl;
solver.solve(initialGuess,delta,rtol,maxFunctionEvals);
std::cout << "Solving Non-linear problem with Cost: Sum of Distances Squared" << std::endl;
solverSq.solve(initialGuess,delta,rtol,maxFunctionEvals);
// Output
std::cout
std::cout
std::endl;
std::cout
std::cout
<< "\nTrue Solution (white) " << std::endl;
<< "------------------------------------------------------------------------------------" <<
<< "\tsolution (center, radii): " << trueSolution << std::endl;
<< "\tcost: " << costFunction.eval(trueSolution) << std::endl;
std::cout
std::cout
std::endl;
std::cout
std::cout
std::cout
<< "\nLinear Solution (blue)" << std::endl;
<< "------------------------------------------------------------------------------------" <<
std::cout
std::cout
std::endl;
std::cout
std::cout
std::cout
std::cout
std::cout
<< "\nNon-Linear Solution, Cost: Sum of Distances from Ellipsoid (green)" << std::endl;
<< "------------------------------------------------------------------------------------" <<
std::cout
std::cout
std::endl;
std::cout
std::cout
std::cout
std::cout
std::cout
<< "\nNon-Linear Solution, Cost: Sum of Distances Squared from Ellipsoid (pink)" << std::endl;
<< "------------------------------------------------------------------------------------" <<
<< "\tsolution (center, radii): " << linearSolution << std::endl;
<< "\tcost: " << costFunction.eval(linearSolution) << std::endl;
<< "\terror (center, radii): " << linearSolution - trueSolution << std::endl;
<<
<<
<<
<<
<<
<<
<<
<<
<<
<<
"\tinitialGuess (center, radii): " << initialGuess << std::endl;
"\tcost: " << costFunction.eval(initialGuess) << std::endl;
"\tsolution (center, radii): " << solver.getSolution() << std::endl;
"\tcost: " << costFunction.eval(solver.getSolution()) << std::endl;
"\terror (center, radii): " << solver.getSolution() - trueSolution << std::endl;
"\tinitialGuess (center, radii): " << initialGuess << std::endl;
"\tcost: " << costFunctionSq.eval(initialGuess) << std::endl;
"\tsolution (center, radii): " << solverSq.getSolution() << std::endl;
"\tcost: " << costFunction.eval(solverSq.getSolution()) << std::endl;
"\terror (center, radii): " << solverSq.getSolution() - trueSolution << std::endl;
17
// Wait for user to press enter then quit
std::cout << "\nPress enter to quit." << std::endl;
std::cin.ignore();
simpleViewer.stop();
return 0;
}
// vim:ts=4:sw=4
18
References
i
Ellipsoid, “http://en.wikipedia.org/wiki/Ellipsoid”
ii
NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING
Numerical Recipes in C: The Art of Scientific Computing.
Calibrating a Triaxial Accelerometer-Magnetometer
Least Squares Ellipsoid Specific Fitting
Calibration of Strapdown Magnetometers in Magnetic Field Domain
A new Magnetic Compass Calibration Algorithm Using Neutral Networks
19