Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Compass: implement 9 parameter ellipsoid fit

This commit is contained in:
bugobliterator 2015-03-09 20:07:12 +05:30 committed by Andrew Tridgell
parent 35555c7b21
commit 7711dde2ad
2 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -97,6 +97,7 @@ void CompassCalibrator::run_fit_chunk() {
if(_status == COMPASS_CAL_SAMPLING_STEP_ONE) {
if(_fit_step < 21) {
_running_ellipsoid_fit = false;
run_sphere_fit(1);
_fit_step++;
} else {
@ -105,16 +106,19 @@ void CompassCalibrator::run_fit_chunk() {
}
set_status(COMPASS_CAL_SAMPLING_STEP_TWO);
_lambda = 1.0f; //reset lambda for ellipsoid fitness
_ellipsoid_param.named.offset = _sphere_param.named.offset;
}
} else if(_status == COMPASS_CAL_SAMPLING_STEP_TWO) {
if(_fit_step < 3) {
run_sphere_fit(3);
} else if(_fit_step < 6) {
run_ellipsoid_fit(3);
} else if(_fit_step%2 == 0 && _fit_step < 35) {
run_sphere_fit(3);
} else if(_fit_step%2 == 1 && _fit_step < 35) {
run_ellipsoid_fit(3);
if(_fit_step < 21) {
_running_ellipsoid_fit = true;
run_ellipsoid_fit(1);
} else if(_fit_step == 21){
_lambda = 1.0f; //reset lambda for sphere fitness
_sphere_param.named.offset = _ellipsoid_param.named.offset;
} else if(_fit_step < 42){
_running_ellipsoid_fit = false;
run_sphere_fit(1);
} else {
if(fit_acceptable()) {
set_status(COMPASS_CAL_SUCCESS);
@ -299,8 +303,11 @@ float CompassCalibrator::calc_residual(const Vector3f& sample, const sphere_para
ep.named.offdiag.x , ep.named.diag.y , ep.named.offdiag.z,
ep.named.offdiag.y , ep.named.offdiag.z , ep.named.diag.z
);
return sp.named.radius - (softiron*(sample+sp.named.offset)).length();
if(_running_ellipsoid_fit){
return sp.named.radius - (softiron*(sample+ep.named.offset)).length();
} else{
return sp.named.radius - (softiron*(sample+sp.named.offset)).length();
}
}
float CompassCalibrator::calc_mean_squared_residuals() const
@ -435,7 +442,7 @@ void CompassCalibrator::run_sphere_fit(uint8_t max_iterations)
void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& ep, ellipsoid_param_t &ret) const{
const Vector3f &offset = _sphere_param.named.offset;
const Vector3f &offset = ep.named.offset;
const Vector3f &diag = ep.named.diag;
const Vector3f &offdiag = ep.named.offdiag;
Matrix3f softiron(
@ -449,6 +456,9 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellip
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();
ret.named.offset.x = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
ret.named.offset.y = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
ret.named.offset.z = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
ret.named.diag.x = -1.0f * ((sample.x + offset.x) * A)/length;
ret.named.diag.y = -1.0f * ((sample.y + offset.y) * B)/length;
ret.named.diag.z = -1.0f * ((sample.z + offset.z) * C)/length;

3
libraries/AP_Compass/CompassCalibrator.h
View File

@ -1,7 +1,7 @@
#include <AP_Math.h>
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 6
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
#define COMPASS_CAL_NUM_SAMPLES 300
//RMS tolerance
@ -53,6 +53,7 @@ private:
union ellipsoid_param_t {
ellipsoid_param_t(){};
struct {
Vector3f offset;
Vector3f diag;
Vector3f offdiag;
} named;