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
1 Undergraduate Student, School of Aeronautics & Astronautics, Purdue University 2 Undergraduate Student, School of Aeronautics & Astronautics, Purdue University 3 Undergraduate Student, School of Aeronautics & Astronautics, Purdue University 4 Graduate Student, School of Aeronautics & Astronautics, Purdue University 5 Graduate Student, School of Aeronautics & Astronautics, Purdue University 6 Professor, School of Aeronautics & Astronautics, Purdue University
1 magnetometer. Because of this, several departments within the university were contacted in the 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, = frequency ratio, = frequency of the IMU, = 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 dimension consists of 6 elements that represent the quadratic ellipsoid equation. The equation and code for the quadratic ellipsoid equation were followed below.
Create Design Matrix for Linear Solution std::cout << "Creating design matrix." << std::endl; matrix 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 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: 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 4 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. 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)); } // 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 dimensions, of 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 5 Where the ’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.i double DownhillSimplex::tryStretch(matrix 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(); 6 vector 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 one- dimensional 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. for (int j=0;j 7 { 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 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 8 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, 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 Figure 5 - Non-linear ellipsoid (distance squared) 9 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. 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 10 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 #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 11 int dim = initialGuess.size(); vector for (int j=0;j 12 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 13 void DownhillSimplexCallback::attach(DownhillSimplex * solver) { this->solver = solver; } } // uvsim // vim:ts=4:sw=4 14 magcalibNonLinSim.cc /* * magCalibNonLinSim.cc * Copyright (C) James Goppert 2009 #include "uvsim/utilities/DownhillSimplex.h" #include "uvsim/visualization/SimpleViewer.h" #include 15 vector // 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; 16 // 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 // Declare the root node osg::Group *root = new osg::Group; // Solutions vector // Visualization // Point Cloud osg::ref_ptr // 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 // 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); } 17 // 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 // 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 // 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 // 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)), 18 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 // 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 << "\nTrue Solution (white) " << std::endl; std::cout << "------" << std::endl; std::cout << "\tsolution (center, radii): " << trueSolution << std::endl; std::cout << "\tcost: " << costFunction.eval(trueSolution) << std::endl; std::cout << "\nLinear Solution (blue)" << std::endl; std::cout << "------" << std::endl; std::cout << "\tsolution (center, radii): " << linearSolution << std::endl; std::cout << "\tcost: " << costFunction.eval(linearSolution) << std::endl; std::cout << "\terror (center, radii): " << linearSolution - trueSolution << std::endl; std::cout << "\nNon-Linear Solution, Cost: Sum of Distances from Ellipsoid (green)" << std::endl; std::cout << "------" << std::endl; std::cout << "\tinitialGuess (center, radii): " << initialGuess << std::endl; std::cout << "\tcost: " << costFunction.eval(initialGuess) << std::endl; std::cout << "\tsolution (center, radii): " << solver.getSolution() << std::endl; std::cout << "\tcost: " << costFunction.eval(solver.getSolution()) << std::endl; std::cout << "\terror (center, radii): " << solver.getSolution() - trueSolution << std::endl; std::cout << "\nNon-Linear Solution, Cost: Sum of Distances Squared from Ellipsoid (pink)" << std::endl; std::cout << "------" << std::endl; std::cout << "\tinitialGuess (center, radii): " << initialGuess << std::endl; std::cout << "\tcost: " << costFunctionSq.eval(initialGuess) << std::endl; std::cout << "\tsolution (center, radii): " << solverSq.getSolution() << std::endl; 19 std::cout << "\tcost: " << costFunction.eval(solverSq.getSolution()) << std::endl; std::cout << "\terror (center, radii): " << solverSq.getSolution() - trueSolution << std::endl; // 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 20 References 21 i 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