From 1e1a04bc459ea2ee30b4751b7dadc92bb76957b4 Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Sun, 20 Mar 2016 03:13:43 -0700 Subject: [PATCH] Add wobble error estimate --- imuread.h | 2 +- quality.c | 130 +++++++++++++++++++++++++++++++++++++++++++++------- visualize.c | 5 +- 3 files changed, 118 insertions(+), 19 deletions(-) diff --git a/imuread.h b/imuread.h index fe4793f..2ab7fd5 100644 --- a/imuread.h +++ b/imuread.h @@ -78,7 +78,7 @@ void quality_reset(void); void quality_update(const Point_t *point); float quality_surface_gap_error(void); float quality_magnitude_variance_error(void); -float quality_centroid_placement_error(void); +float quality_wobble_error(void); float quality_spherical_fit_error(void); // magnetic calibration & buffer structure diff --git a/quality.c b/quality.c index a6b9d4d..ce7253f 100644 --- a/quality.c +++ b/quality.c @@ -1,13 +1,25 @@ #include "imuread.h" -// longitude = 0 to 2pi (meaning 0 to 360 degrees) -// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees) +//static int countdown=1000; +//static int pr=0; + // return 0 to 99 - which region on the sphere (100 of equal surface area) -static int sphere_region(float longitude, float latitude) +static int sphere_region(float x, float y, float z) { + float latitude, longitude; int region; + //if (pr) printf(" region %.1f,%.1f,%.1f ", x, y, z); + + // longitude = 0 to 2pi (meaning 0 to 360 degrees) + longitude = atan2f(y, x) + (float)M_PI; + // latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees) + latitude = (float)(M_PI / 2.0) - atan2f(sqrtf(x * x + y * y), z); + + //if (pr) printf(" lat=%.1f", latitude * (float)(180.0 / M_PI)); + //if (pr) printf(",lon=%.1f ", longitude * (float)(180.0 / M_PI)); + // https://etna.mcs.kent.edu/vol.25.2006/pp309-327.dir/pp309-327.html // sphere equations.... // area of unit sphere = 4*pi @@ -15,19 +27,19 @@ static int sphere_region(float longitude, float latitude) // lattitude of unit sphere cap = arcsin(1 - h) if (latitude > 1.37046f /* 78.52 deg */) { // arctic cap, 1 region - return 0; + region = 0; } else if (latitude < -1.37046f /* -78.52 deg */) { // antarctic cap, 1 region - return 99; + region = 99; } else if (latitude > 0.74776f /* 42.84 deg */ || latitude < -0.74776f ) { // temperate zones, 15 regions each region = floorf(longitude * (float)(15.0 / (M_PI * 2.0))); if (region < 0) region = 0; else if (region > 14) region = 14; if (latitude > 0.0) { - return region + 1; // 1 to 15 + region += 1; // 1 to 15 } else { - return region + 84; // 84 to 98 + region += 84; // 84 to 98 } } else { // tropic zones, 34 regions each @@ -35,36 +47,88 @@ static int sphere_region(float longitude, float latitude) if (region < 0) region = 0; else if (region > 33) region = 33; if (latitude >= 0.0) { - return region + 16; // 16 to 49 + region += 16; // 16 to 49 } else { - return region + 50; // 50 to 83 + region += 50; // 50 to 83 } } + //if (pr) printf(" %d\n", region); + return region; } static int count=0; static int spheredist[100]; +static Point_t spheredata[100]; +static Point_t sphereideal[100]; +static int sphereideal_initialized=0; static float magnitude[MAGBUFFSIZE]; void quality_reset(void) { + float longitude; + int i; +/* + countdown--; + if (countdown == 0) { + countdown = 1000; + pr = 1; + } else { + pr = 0; + } +*/ count=0; memset(spheredist, 0, sizeof(spheredist)); + memset(spheredata, 0, sizeof(spheredata)); + if (!sphereideal_initialized) { + sphereideal[0].x = 0.0f; + sphereideal[0].y = 0.0f; + sphereideal[0].z = 1.0f; + for (i=1; i <= 15; i++) { + longitude = ((float)(i - 1) + 0.5f) * (M_PI * 2.0 / 15.0); + sphereideal[i].x = cosf(longitude) * cosf(1.05911) * -1.0f; + sphereideal[i].y = sinf(longitude) * cosf(1.05911) * -1.0f; + sphereideal[i].z = sinf(1.05911); + } + for (i=16; i <= 49; i++) { + longitude = ((float)(i - 16) + 0.5f) * (M_PI * 2.0 / 34.0); + sphereideal[i].x = cosf(longitude) * cos(0.37388) * -1.0f; + sphereideal[i].y = sinf(longitude) * cos(0.37388) * -1.0f; + sphereideal[i].z = sinf(0.37388); + } + for (i=50; i <= 83; i++) { + longitude = ((float)(i - 50) + 0.5f) * (M_PI * 2.0 / 34.0); + sphereideal[i].x = cosf(longitude) * cos(0.37388) * -1.0f; + sphereideal[i].y = sinf(longitude) * cos(0.37388) * -1.0f; + sphereideal[i].z = sinf(-0.37388); + } + for (i=84; i <= 98; i++) { + longitude = ((float)(i - 1) + 0.5f) * (M_PI * 2.0 / 15.0); + sphereideal[i].x = cosf(longitude) * cosf(1.05911) * -1.0f; + sphereideal[i].y = sinf(longitude) * cosf(1.05911) * -1.0f; + sphereideal[i].z = sinf(-1.05911); + } + sphereideal[99].x = 0.0f; + sphereideal[99].y = 0.0f; + sphereideal[99].z = -1.0f; + sphereideal_initialized = 1; + } } void quality_update(const Point_t *point) { float x, y, z; - float latitude, longitude; + int region; x = point->x; y = point->y; z = point->z; magnitude[count] = sqrtf(x * x + y * y + z * z); - longitude = atan2f(y, x) + (float)M_PI; - latitude = atan2f(sqrtf(x * x + y * y), z) - (float)(M_PI / 2.0); - spheredist[sphere_region(longitude, latitude)]++; + region = sphere_region(x, y, z); + spheredist[region]++; + spheredata[region].x += x; + spheredata[region].y += y; + spheredata[region].z += z; count++; } @@ -107,10 +171,44 @@ float quality_magnitude_variance_error(void) return sqrtf(variance) / mean * 100.0f; } -// Offset of centroid -float quality_centroid_placement_error(void) +// Offset of piecewise average data from ideal sphere surface +float quality_wobble_error(void) { - return 0.0f; + float sum, radius, x, y, z, xi, yi, zi; + float xoff=0.0f, yoff=0.0f, zoff=0.0f; + int i, n=0; + + sum = 0.0f; + for (i=0; i < count; i++) { + sum += magnitude[i]; + } + radius = sum / (float)count; + //if (pr) printf(" radius = %.2f\n", radius); + for (i=0; i < 100; i++) { + if (spheredist[i] > 0) { + //if (pr) printf(" i=%3d", i); + x = spheredata[i].x / (float)spheredist[i]; + y = spheredata[i].y / (float)spheredist[i]; + z = spheredata[i].z / (float)spheredist[i]; + //if (pr) printf(" at: %5.1f %5.1f %5.1f :", x, y, z); + xi = sphereideal[i].x * radius; + yi = sphereideal[i].y * radius; + zi = sphereideal[i].z * radius; + //if (pr) printf(" ideal: %5.1f %5.1f %5.1f :", xi, yi, zi); + xoff += x - xi; + yoff += y - yi; + zoff += z - zi; + //if (pr) printf("\n"); + n++; + } + } + if (n == 0) return 100.0f; + //if (pr) printf(" off = %.2f, %.2f, %.2f\n", xoff, yoff, zoff); + xoff /= (float)n; + 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; } // Freescale's algorithm fit error diff --git a/visualize.c b/visualize.c index bc58408..b442ae9 100644 --- a/visualize.c +++ b/visualize.c @@ -105,10 +105,11 @@ void display_callback(void) } } } -#if 0 - printf(" quality: %5.1f %5.1f %5.1f\n", +#if 1 + printf(" quality: %5.1f %5.1f %5.1f %5.1f\n", quality_surface_gap_error(), quality_magnitude_variance_error(), + quality_wobble_error(), quality_spherical_fit_error()); #endif }