AP_Compass: add info about compass calibrator procedure

libraries/AP_Compass/CompassCalibrator.cpp

@@ -1,3 +1,60 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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, see <http://www.gnu.org/licenses/>.
+*/
+
+/*
+ *    AP_Compass_Callib.cpp 
+ *
+ *     1.The following code uses an implementation of a Levenberg-Marquardt non-linear
+ *       least square regression technique to fit the result over a sphere.
+ *       http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
+ *
+ *     2.Fitness Matrix is generated by placing the sample points into a general sphere equation.
+ *  
+ *     3.Jacobian matrix is calculated using partial derivative equation of each parameters
+ *       wrt fitness function.
+ *
+ *
+ *     Sampling-Rules
+ *     ==============
+ *
+ *     1.Every point should be unique, no repeated samples
+ *
+ *     2.Every consecutive 4 samples should not be coplanar, as for every 4 non-coplanar point
+ *      in space there exists a distinct sphere. Therefore using this method we will be getting
+ *      set of atleast NUM_SAMPLES quadruples of coplanar point.
+ *
+ *     3.Every point should be atleast separated by D distance:
+ *
+ *              where:
+ *                  D = distance between any two sample points
+ *                  (Surface Area of Sphere)/(2 * (Area of equilateral triangle)) = NUM_SAMPLES
+ *              =>  D >= 5.5 * Radius / 10
+ *              but for the sake of leniency to the user let's halve this distance. This will ensure
+ *              atleast 50% coverage of sphere. The rest will be taken care of by Gauss-Newton.
+ *                  D >= 5.5 * Radius / 20
+ *
+ *      Explaination: If we are to consider a sphere and place discrete points which are uniformly
+ *                    spread. The simplest possible polygon that can be created using distinct closest
+ *                    points is an equilateral triangle. The number of such triangles will be NUM_SAMPLES
+ *                    and will all be totally distinct. The side of such triangles also represent the 
+ *                    minimum distance between any two samples for 100% coverage. But since this would 
+ *                    be very-difficult/impossible for user to achieve, we reduce it to minimum 50% coverage.
+ *
+ */
+
 #include "CompassCalibrator.h"
 #include <AP_HAL.h>
 
@@ -352,9 +409,9 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t&
     float C =  (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z    * (sample.z + offset.z));
     float length = (softiron*(sample+offset)).length();
 
-    // 0: radius
+    // 0: partial derivative (radius wrt fitness fn) fn operated on sample
     ret[0] = 1.0f;
-    // 1-3: offsets
+    // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
     ret[1] = -1.0f * (((diag.x    * A) + (offdiag.x * B) + (offdiag.y * C))/length);
     ret[2] = -1.0f * (((offdiag.x * A) + (diag.y    * B) + (offdiag.z * C))/length);
     ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z    * C))/length);
@@ -401,6 +458,7 @@ void CompassCalibrator::run_sphere_fit()
 
 
     //------------------------Levenberg-part-starts-here---------------------------------//
+    //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
     for(uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
         JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda;
         JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping;
@@ -458,15 +516,15 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param
     float C =  (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z    * (sample.z + offset.z));
     float length = (softiron*(sample+offset)).length();
 
-    // 0-2: offsets
+    // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
     ret[0] = -1.0f * (((diag.x    * A) + (offdiag.x * B) + (offdiag.y * C))/length);
     ret[1] = -1.0f * (((offdiag.x * A) + (diag.y    * B) + (offdiag.z * C))/length);
     ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z    * C))/length);
-    // 3-5: diagonals
+    // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
     ret[3] = -1.0f * ((sample.x + offset.x) * A)/length;
     ret[4] = -1.0f * ((sample.y + offset.y) * B)/length;
     ret[5] = -1.0f * ((sample.z + offset.z) * C)/length;
-    // 6-8: off-diagonals
+    // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
     ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
     ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length;
     ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length;
@@ -516,6 +574,7 @@ void CompassCalibrator::run_ellipsoid_fit()
 
 
     //------------------------Levenberg-part-starts-here---------------------------------//
+    //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
     for(uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
         JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda;
         JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda/lma_damping;