From 9d28351f4604aaa8bfbd512a5db71e2adcaa9223 Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Fri, 1 Apr 2016 04:57:08 -0700 Subject: [PATCH] Reduce variance when gaps error < 25% --- quality.c | 27 ++++++++++++-- rawdata.c | 103 +++++++++++++++++++++++++++++++++++++++++++----------- 2 files changed, 107 insertions(+), 23 deletions(-) diff --git a/quality.c b/quality.c index ce7253f..3a3ee85 100644 --- a/quality.c +++ b/quality.c @@ -63,6 +63,12 @@ static Point_t spheredata[100]; static Point_t sphereideal[100]; static int sphereideal_initialized=0; static float magnitude[MAGBUFFSIZE]; +static float quality_gaps_buffer; +static float quality_variance_buffer; +static float quality_wobble_buffer; +static int quality_gaps_computed=0; +static int quality_variance_computed=0; +static int quality_wobble_computed=0; void quality_reset(void) { @@ -113,6 +119,9 @@ void quality_reset(void) sphereideal[99].z = -1.0f; sphereideal_initialized = 1; } + quality_gaps_computed = 0; + quality_variance_computed = 0; + quality_wobble_computed = 0; } void quality_update(const Point_t *point) @@ -130,6 +139,9 @@ void quality_update(const Point_t *point) spheredata[region].y += y; spheredata[region].z += z; count++; + quality_gaps_computed = 0; + quality_variance_computed = 0; + quality_wobble_computed = 0; } // How many surface gaps @@ -138,6 +150,7 @@ float quality_surface_gap_error(void) float error=0.0f; int i, num; + if (quality_gaps_computed) return quality_gaps_buffer; for (i=0; i < 100; i++) { num = spheredist[i]; if (num == 0) { @@ -148,7 +161,9 @@ float quality_surface_gap_error(void) error += 0.01f; } } - return error; + quality_gaps_buffer = error; + quality_gaps_computed = 1; + return quality_gaps_buffer; } // Variance in magnitude @@ -157,6 +172,7 @@ float quality_magnitude_variance_error(void) float sum, mean, diff, variance; int i; + if (quality_variance_computed) return quality_variance_buffer; sum = 0.0f; for (i=0; i < count; i++) { sum += magnitude[i]; @@ -168,7 +184,9 @@ float quality_magnitude_variance_error(void) variance += diff * diff; } variance /= (float)count; - return sqrtf(variance) / mean * 100.0f; + quality_variance_buffer = sqrtf(variance) / mean * 100.0f; + quality_variance_computed = 1; + return quality_variance_buffer; } // Offset of piecewise average data from ideal sphere surface @@ -178,6 +196,7 @@ float quality_wobble_error(void) float xoff=0.0f, yoff=0.0f, zoff=0.0f; int i, n=0; + if (quality_wobble_computed) return quality_wobble_buffer; sum = 0.0f; for (i=0; i < count; i++) { sum += magnitude[i]; @@ -208,7 +227,9 @@ float quality_wobble_error(void) yoff /= (float)n; zoff /= (float)n; //if (pr) printf(" off = %.2f, %.2f, %.2f\n", xoff, yoff, zoff); - return sqrtf(xoff * xoff + yoff * yoff + zoff * zoff) / radius * 100.0f; + quality_wobble_buffer = sqrtf(xoff * xoff + yoff * yoff + zoff * zoff) / radius * 100.0f; + quality_wobble_computed = 1; + return quality_wobble_buffer; } // Freescale's algorithm fit error diff --git a/rawdata.c b/rawdata.c index 894d64a..0547248 100644 --- a/rawdata.c +++ b/rawdata.c @@ -20,35 +20,98 @@ void raw_data_reset(void) magcal.B = 50.0f; } +static int choose_discard_magcal(void) +{ + int32_t rawx, rawy, rawz; + int32_t dx, dy, dz; + float x, y, z; + uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFF; + static int runcount=0; + int i, j, minindex=0; + Point_t point; + float gaps, field, error, errormax; + + // When enough data is collected (gaps error is low), assume we + // have a pretty good coverage and the field stregth is known. + gaps = quality_surface_gap_error(); + if (gaps < 25.0f) { + // occasionally look for points farthest from average field strength + // always rate limit assumption-based data purging, but allow the + // rate to increase as the angular coverage improves. + if (gaps < 1.0f) gaps = 1.0f; + if (++runcount > (int)(gaps * 10.0f)) { + j = MAGBUFFSIZE; + errormax = 0.0f; + for (i=0; i < MAGBUFFSIZE; i++) { + rawx = magcal.BpFast[0][i]; + rawy = magcal.BpFast[1][i]; + rawz = magcal.BpFast[2][i]; + apply_calibration(rawx, rawy, rawz, &point); + x = point.x; + y = point.y; + z = point.z; + field = sqrtf(x * x + y * y + z * z); + // if magcal.B is bad, things could go horribly wrong + error = fabsf(field - magcal.B); + if (error > errormax) { + errormax = error; + j = i; + } + } + runcount = 0; + if (j < MAGBUFFSIZE) { + //printf("worst error at %d\n", j); + return j; + } + } + } else { + runcount = 0; + } + // When solid info isn't availabe, find 2 points closest to each other, + // and randomly discard one. When we don't have good coverage, this + // approach tends to add points into previously unmeasured areas while + // discarding info from areas with highly redundant info. + for (i=0; i < MAGBUFFSIZE; i++) { + for (j=i+1; j < MAGBUFFSIZE; j++) { + dx = magcal.BpFast[0][i] - magcal.BpFast[0][j]; + dy = magcal.BpFast[1][i] - magcal.BpFast[1][j]; + dz = magcal.BpFast[2][i] - magcal.BpFast[2][j]; + distsq = (int64_t)dx * (int64_t)dx; + distsq += (int64_t)dy * (int64_t)dy; + distsq += (int64_t)dz * (int64_t)dz; + if (distsq < minsum) { + minsum = distsq; + minindex = (random() & 1) ? i : j; + } + } + } + return minindex; +} + + static void add_magcal_data(const int16_t *data) { - int32_t dx, dy, dz; - uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFF; - int i, j, minindex=0; + int i; // first look for an unused caldata slot for (i=0; i < MAGBUFFSIZE; i++) { if (!magcal.valid[i]) break; } - // if no unused, find the ones closest to each other - // TODO: after reasonable sphere fit, we should retire older data - // and choose the ones farthest from the sphere's radius + // If the buffer is full, we must choose which old data to discard. + // We must choose wisely! Throwing away the wrong data could prevent + // collecting enough data distributed across the entire 3D angular + // range, preventing a decent cal from ever happening at all. Making + // any assumption about good vs bad data is particularly risky, + // because being wrong could cause an unstable feedback loop where + // bad data leads to wrong decisions which leads to even worse data. + // But if done well, purging bad data has massive potential to + // improve results. The trick is telling the good from the bad while + // still in the process of learning what's good... if (i >= MAGBUFFSIZE) { - for (i=0; i < MAGBUFFSIZE; i++) { - for (j=i+1; j < MAGBUFFSIZE; j++) { - dx = magcal.BpFast[0][i] - magcal.BpFast[0][j]; - dy = magcal.BpFast[1][i] - magcal.BpFast[1][j]; - dz = magcal.BpFast[2][i] - magcal.BpFast[2][j]; - distsq = (int64_t)dx * (int64_t)dx; - distsq += (int64_t)dy * (int64_t)dy; - distsq += (int64_t)dz * (int64_t)dz; - if (distsq < minsum) { - minsum = distsq; - minindex = (random() & 1) ? i : j; - } - } + i = choose_discard_magcal(); + if (i < 0 || i >= MAGBUFFSIZE) { + i = random() % MAGBUFFSIZE; } - i = minindex; } // add it to the cal buffer magcal.BpFast[0][i] = data[6];