Add wobble error estimate
This commit is contained in:
PaulStoffregen
parent
0aff5bef36
commit
1e1a04bc45
3 changed files with 118 additions and 19 deletions
|
|
@ -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
|
||||
|
|
|
|||
130
quality.c
130
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue