diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp
index 259da2011c..64dd4ea84d 100644
--- a/libraries/AP_Compass/CompassCalibrator.cpp
+++ b/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 .
+*/
+
+/*
+ * 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
@@ -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;