AP_Compass: CompassCalibrator comment update

This commit is contained in:
Jonathan Challinger 2015-07-06 13:12:45 -07:00 committed by Andrew Tridgell
parent 1c100498d4
commit bff9b9065e
1 changed files with 38 additions and 31 deletions

View File

@ -15,43 +15,50 @@
*/
/*
* AP_Compass_Callib.cpp
* The intention of a magnetometer in a compass application is to measure
* Earth's magnetic field. Measurements other than those of Earth's magnetic
* field are considered errors. This algorithm computes a set of correction
* parameters that null out errors from various sources:
*
* 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
* - Sensor bias error
* - "Hard iron" error caused by materials fixed to the vehicle body that
* produce static magnetic fields.
* - Sensor scale-factor error
* - Sensor cross-axis sensitivity
* - "Soft iron" error caused by materials fixed to the vehicle body that
* distort magnetic fields.
*
* 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.
* This is done by taking a set of samples that are assumed to be the product
* of rotation in earth's magnetic field and fitting an offset ellipsoid to
* them, determining the correction to be applied to adjust the samples into an
* origin-centered sphere.
*
* The state machine of this library is described entirely by the
* compass_cal_status_t enum, and all state transitions are managed by the
* set_status function. Normally, the library is in the NOT_STARTED state. When
* the start function is called, the state transitions to WAITING_TO_START,
* until two conditions are met: the delay as elapsed, and the memory for the
* sample buffer has been successfully allocated.
* Once these conditions are met, the state transitions to RUNNING_STEP_ONE, and
* samples are collected via calls to the new_sample function. These samples are
* accepted or rejected based on distance to the nearest sample. The samples are
* assumed to cover the surface of a sphere, and the radius of that sphere is
* initialized to a conservative value. Based on a circle-packing pattern, the
* minimum distance is set such that some percentage of the surface of that
* sphere must be covered by samples.
*
* Sampling-Rules
* ==============
* Once the sample buffer is full, a sphere fitting algorithm is run, which
* computes a new sphere radius. The sample buffer is thinned of samples which
* no longer meet the acceptance criteria, and the state transitions to
* RUNNING_STEP_TWO. Samples continue to be collected until the buffer is full
* again, the full ellipsoid fit is run, and the state transitions to either
* SUCCESS or FAILED.
*
* 1.Every point should be unique, no repeated samples
* The fitting algorithm used is Levenberg-Marquardt. See also:
* http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
*
* 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.
* The sample acceptance distance is determined as follows:
* < EXPLANATION OF SAMPLE ACCEPTANCE TO BE FILLED IN BY SID >
*
*/