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