Introduce Updates to Compass Calibration Algorithm and UX

- Updated compass calibration algorithm, based on iterative approximation
  - Updated calibration UX
This commit is contained in:
Joe Finney 2018-07-05 23:48:41 +01:00
parent e79284d797
commit bbf56210ed
5 changed files with 361 additions and 91 deletions

View File

@ -47,6 +47,18 @@ DEALINGS IN THE SOFTWARE.
#define MICROBIT_COMPASS_EVT_CALIBRATE 3
#define MICROBIT_COMPASS_EVT_CALIBRATION_NEEDED 4
struct CompassCalibration
{
Sample3D centre; // Zero offset of the compass.
Sample3D scale; // Scale factor to apply in each axis to accomodate 1st order directional fields.
int radius; // Indication of field strength - the "distance" from the centre to outmost sample.
CompassCalibration() : centre(), scale(1024, 1024, 1024)
{
radius = 0;
}
};
/**
* Class definition for a general e-compass.
*/
@ -55,7 +67,7 @@ class MicroBitCompass : public MicroBitComponent
protected:
uint16_t samplePeriod; // The time between samples, in milliseconds.
Sample3D average; // The zero offset of this compass (generated by calibration)
CompassCalibration calibration; // The calibration data of this compass
Sample3D sample; // The last sample read, in the coordinate system specified by the coordinateSpace variable.
Sample3D sampleENU; // The last sample read, in raw ENU format (stored in case requests are made for data in other coordinate spaces)
CoordinateSpace &coordinateSpace; // The coordinate space transform (if any) to apply to the raw data from the hardware.
@ -149,7 +161,7 @@ class MicroBitCompass : public MicroBitComponent
*
* @param calibration A Sample3D containing the offsets for the x, y and z axis.
*/
void setCalibration(Sample3D calibration);
void setCalibration(CompassCalibration calibration);
/**
* Provides the calibration data currently in use by the compass.
@ -158,7 +170,7 @@ class MicroBitCompass : public MicroBitComponent
*
* @return A Sample3D containing the offsets for the x, y and z axis.
*/
Sample3D getCalibration();
CompassCalibration getCalibration();
/**
* Returns 0 or 1. 1 indicates that the compass is calibrated, zero means the compass requires calibration.

View File

@ -78,7 +78,59 @@ class MicroBitCompassCalibrator
*
* This function is, by design, synchronous and only returns once calibration is complete.
*/
void calibrate(MicroBitEvent);
void calibrateUX(MicroBitEvent);
/**
* Calculates an independent X, Y, Z scale factor and centre for a given set of data points,
* assumed to be on a bounding sphere
*
* @param data An array of all data points
* @param samples The number of samples in the 'data' array.
*
* This algorithm should be called with no fewer than 12 points, but testing has indicated >21
* points provides a more robust calculation.
*
* @return A calibration structure containing the a calculated centre point, the radius of the
* minimum enclosing spere of points and a scaling factor for each axis that places those
* points as close as possible to the surface of the containing sphere.
*/
static CompassCalibration calibrate(Sample3D *data, int samples);
private:
/**
* Scoring function for a hill climb algorithm.
*
* @param c An approximated centre point
* @param data a collection of data points
* @param samples the number of samples in the 'data' array
*
* @return The deviation between the closest and further point in the data array from the point given.
*/
static int measureScore(Sample3D &c, Sample3D *data, int samples);
/*
* Performs an interative approximation (hill descent) algorithm to determine an
* estimated centre point of a sphere upon which the given data points reside.
*
* @param data an array containing sample points
* @param samples the number of sample points in the 'data' array.
*
* @return the approximated centre point of the points in the 'data' array.
*/
static Sample3D approximateCentre(Sample3D *data, int samples);
/**
* Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere
*
* @param centre A proviously calculated centre point of all data.
* @param data An array of all data points
* @param samples The number of samples in the 'data' array.
*
* @return A calibration structure containing the centre point provided, the radius of the minimum
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
* to the surface of the containing sphere.
*/
static CompassCalibration spherify(Sample3D centre, Sample3D *data, int samples);
};
#endif

View File

@ -121,6 +121,12 @@ struct Sample3D
{
return !(x == other.x && y == other.y && z == other.z);
}
int dSquared(Sample3D &s)
{
return (x - s.x)*(x - s.x) + (y - s.y)*(y - s.y) + (z - s.z)*(z - s.z);
}
};

View File

@ -33,6 +33,10 @@ DEALINGS IN THE SOFTWARE.
#include "LSM303Magnetometer.h"
#include "FXOS8700.h"
//
// Internal convenience macro to apply calibration to a given sample.
//
#define CALIBRATED_SAMPLE(sample, axis) (((sample.axis - calibration.centre.axis) * calibration.scale.axis) >> 10)
/**
* Constructor.
@ -42,7 +46,7 @@ DEALINGS IN THE SOFTWARE.
* @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN
*
*/
MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace)
MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace)
{
accelerometer = NULL;
init(id);
@ -57,7 +61,7 @@ MicroBitCompass::MicroBitCompass(CoordinateSpace &cspace, uint16_t id) : sample(
* @param coordinateSpace the orientation of the sensor. Defaults to: SIMPLE_CARTESIAN
*
*/
MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : sample(), sampleENU(), coordinateSpace(cspace)
MicroBitCompass::MicroBitCompass(MicroBitAccelerometer &accel, CoordinateSpace &cspace, uint16_t id) : calibration(), sample(), sampleENU(), coordinateSpace(cspace)
{
accelerometer = &accel;
init(id);
@ -236,9 +240,9 @@ int MicroBitCompass::calibrate()
*
* @param calibration A Sample3D containing the offsets for the x, y and z axis.
*/
void MicroBitCompass::setCalibration(Sample3D calibration)
void MicroBitCompass::setCalibration(CompassCalibration calibration)
{
average = calibration;
this->calibration = calibration;
status |= MICROBIT_COMPASS_STATUS_CALIBRATED;
}
@ -249,9 +253,9 @@ void MicroBitCompass::setCalibration(Sample3D calibration)
*
* @return A Sample3D containing the offsets for the x, y and z axis.
*/
Sample3D MicroBitCompass::getCalibration()
CompassCalibration MicroBitCompass::getCalibration()
{
return average;
return calibration;
}
/**
@ -275,7 +279,7 @@ int MicroBitCompass::isCalibrating()
*/
void MicroBitCompass::clearCalibration()
{
average = Sample3D();
calibration = CompassCalibration();
status &= ~MICROBIT_COMPASS_STATUS_CALIBRATED;
}
@ -356,7 +360,10 @@ int MicroBitCompass::requestUpdate()
int MicroBitCompass::update()
{
// Store the new data, after performing any necessary coordinate transformations.
sample = coordinateSpace.transform(sampleENU - average);
sampleENU.x = CALIBRATED_SAMPLE(sampleENU, x);
sampleENU.y = CALIBRATED_SAMPLE(sampleENU, y);
sampleENU.z = CALIBRATED_SAMPLE(sampleENU, z);
sample = coordinateSpace.transform(sampleENU);
// Indicate that a new sample is available
MicroBitEvent e(id, MICROBIT_COMPASS_EVT_DATA_UPDATE);
@ -373,7 +380,7 @@ int MicroBitCompass::update()
Sample3D MicroBitCompass::getSample(CoordinateSystem coordinateSystem)
{
requestUpdate();
return coordinateSpace.transform(sampleENU - average, coordinateSystem);
return coordinateSpace.transform(sampleENU, coordinateSystem);
}
/**
@ -427,28 +434,36 @@ int MicroBitCompass::getZ()
*/
int MicroBitCompass::tiltCompensatedBearing()
{
// Precompute the tilt compensation parameters to improve readability.
float phi = accelerometer->getRollRadians();
float theta = accelerometer->getPitchRadians();
Sample3D cs = this->getSample(NORTH_EAST_DOWN);
Sample3D as = accelerometer->getSample(NORTH_EAST_DOWN);
Sample3D s = getSample(NORTH_EAST_DOWN);
// Convert to floating point to reduce rounding errors
float x = (float) cs.x;
float y = (float) cs.y;
float z = (float) cs.z;
float x = (float) s.x;
float y = (float) s.y;
float z = (float) s.z;
float ax = (float) as.x;
float ay = (float) as.y;
float az = (float) as.z;
// Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient.
float sinPhi = sin(phi);
float cosPhi = cos(phi);
float sinTheta = sin(theta);
float cosTheta = cos(theta);
// normalize the readings
float amag = sqrt(ax*ax + ay*ay + az*az);
ax = ax/amag;
ay = ay/amag;
az = az/amag;
float bearing = (360*atan2(z*sinPhi - y*cosPhi, x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi)) / (2*PI);
float ax2 = ax*ax;
float ay2 = ay*ay;
float resultx = x*(1.0f - ax2) - y*ax*ay - z*ax*sqrt(1.0f-ax2-ay2);
float resulty = y*sqrt(1.0f-ax2-ay2) - z*ay;
float bearing = (360*atan2(resulty,resultx)) / (2*PI);
if (bearing < 0)
bearing += 360.0;
return (int) bearing;
return (int) (360.0 - bearing);
}
/**
@ -456,7 +471,7 @@ int MicroBitCompass::tiltCompensatedBearing()
*/
int MicroBitCompass::basicBearing()
{
float bearing = (atan2((double)(sample.y - average.y),(double)(sample.x - average.x)))*180/PI;
float bearing = (atan2((double)getY(),(double)getX()))*180/PI;
if (bearing < 0)
bearing += 360.0;

View File

@ -28,6 +28,8 @@ DEALINGS IN THE SOFTWARE.
#include "EventModel.h"
#include "Matrix4.h"
#define CALIBRATION_INCREMENT 10
/**
* Constructor.
*
@ -48,61 +50,269 @@ DEALINGS IN THE SOFTWARE.
MicroBitCompassCalibrator::MicroBitCompassCalibrator(MicroBitCompass& _compass, MicroBitAccelerometer& _accelerometer, MicroBitDisplay& _display) : compass(_compass), accelerometer(_accelerometer), display(_display)
{
if (EventModel::defaultEventBus)
EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrate, MESSAGE_BUS_LISTENER_IMMEDIATE);
EventModel::defaultEventBus->listen(MICROBIT_ID_COMPASS, MICROBIT_COMPASS_EVT_CALIBRATE, this, &MicroBitCompassCalibrator::calibrateUX, MESSAGE_BUS_LISTENER_IMMEDIATE);
}
/**
* Performs a simple game that in parallel, calibrates the compass.
*
* This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
*
* This function is, by design, synchronous and only returns once calibration is complete.
*/
void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
* Scoring function for a hill climb algorithm.
*
* @param c An approximated centre point
* @param data a collection of data points
* @param samples the number of samples in the 'data' array
*
* @return The deviation between the closest and further point in the data array from the point given.
*/
int MicroBitCompassCalibrator::measureScore(Sample3D &c, Sample3D *data, int samples)
{
int minD;
int maxD;
minD = maxD = c.dSquared(data[0]);
for (int i = 1; i < samples; i++)
{
int d = c.dSquared(data[i]);
if (d < minD)
minD = d;
if (d > maxD)
maxD = d;
}
return (maxD - minD);
}
/**
* Calculates an independent X, Y, Z scale factor and centre for a given set of data points, assumed to be on
* a bounding sphere
*
* @param data An array of all data points
* @param samples The number of samples in the 'data' array.
*
* This algorithm should be called with no fewer than 12 points, but testing has indicated >21 points provides
* a more robust calculation.
*
* @return A calibration structure containing the a calculated centre point, the radius of the minimum
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
* to the surface of the containing sphere.
*/
CompassCalibration MicroBitCompassCalibrator::calibrate(Sample3D *data, int samples)
{
Sample3D centre = approximateCentre(data, samples);
return spherify(centre, data, samples);
}
/**
* Calculates an independent scale factor for X,Y and Z axes that places the given data points on a bounding sphere
*
* @param centre A proviously calculated centre point of all data.
* @param data An array of all data points
* @param samples The number of samples in the 'data' array.
*
* @return A calibration structure containing the centre point provided, the radius of the minimum
* enclosing spere of points and a scaling factor for each axis that places those points as close as possible
* to the surface of the containing sphere.
*/
CompassCalibration MicroBitCompassCalibrator::spherify(Sample3D centre, Sample3D *data, int samples)
{
// First, determine the radius of the enclosing sphere from the given centre.
// n.b. this will likely be different to the radius from the centre of mass previously calculated.
// We use the same algorithm though.
CompassCalibration result;
float radius = 0;
float scaleX = 0.0;
float scaleY = 0.0;
float scaleZ = 0.0;
float scale = 0.0;
float weightX = 0.0;
float weightY = 0.0;
float weightZ = 0.0;
for (int i = 0; i < samples; i++)
{
int d = sqrt((float)centre.dSquared(data[i]));
if (d > radius)
radius = d;
}
// Now, for each data point, determine a scalar multiplier for the vector between the centre and that point that
// takes the point onto the surface of the enclosing sphere.
for (int i = 0; i < samples; i++)
{
// Calculate the distance from this point to the centre of the sphere
float d = sqrt(centre.dSquared(data[i]));
// Now determine a scalar multiplier that, when applied to the vector to the centre,
// will place this point on the surface of the sphere.
float s = (radius / d) - 1;
scale = max(scale, s);
// next, determine the scale effect this has on each of our components.
float dx = (data[i].x - centre.x);
float dy = (data[i].y - centre.y);
float dz = (data[i].z - centre.z);
weightX += s * fabsf(dx / d);
weightY += s * fabsf(dy / d);
weightZ += s * fabsf(dz / d);
}
float wmag = sqrt((weightX * weightX) + (weightY * weightY) + (weightZ * weightZ));
scaleX = 1.0 + scale * (weightX / wmag);
scaleY = 1.0 + scale * (weightY / wmag);
scaleZ = 1.0 + scale * (weightZ / wmag);
result.scale.x = (int)(1024 * scaleX);
result.scale.y = (int)(1024 * scaleY);
result.scale.z = (int)(1024 * scaleZ);
result.centre.x = centre.x;
result.centre.y = centre.y;
result.centre.z = centre.z;
result.radius = radius;
return result;
}
/*
* Performs an interative approximation (hill descent) algorithm to determine an
* estimated centre point of a sphere upon which the given data points reside.
*
* @param data an array containing sample points
* @param samples the number of sample points in the 'data' array.
*
* @return the approximated centre point of the points in the 'data' array.
*/
Sample3D MicroBitCompassCalibrator::approximateCentre(Sample3D *data, int samples)
{
Sample3D c,t;
Sample3D centre = { 0,0,0 };
Sample3D best = { 0,0,0 };
int score;
for (int i = 0; i < samples; i++)
{
centre.x += data[i].x;
centre.y += data[i].y;
centre.z += data[i].z;
}
// Calclulate a centre of mass for our input samples. We only use this for validation purposes.
centre.x = centre.x / samples;
centre.y = centre.y / samples;
centre.z = centre.z / samples;
// Start hill climb in the centre of mass.
c = centre;
// calculate the nearest and furthest point to us.
score = measureScore(c, data, samples);
// iteratively attempt to improve position...
while (1)
{
for (int x = -CALIBRATION_INCREMENT; x <= CALIBRATION_INCREMENT; x=x+CALIBRATION_INCREMENT)
{
for (int y = -CALIBRATION_INCREMENT; y <= CALIBRATION_INCREMENT; y=y+CALIBRATION_INCREMENT)
{
for (int z = -CALIBRATION_INCREMENT; z <= CALIBRATION_INCREMENT; z=z+CALIBRATION_INCREMENT)
{
t = c;
t.x += x;
t.y += y;
t.z += z;
int s = measureScore(t, data, samples);
if (s < score)
{
score = s;
best = t;
}
}
}
}
if (best.x == c.x && best.y == c.y && best.z == c.z)
break;
c = best;
}
return c;
}
/**
* Performs a simple game that in parallel, calibrates the compass.
*
* This function is executed automatically when the user requests a compass bearing, and compass calibration is required.
*
* This function is, by design, synchronous and only returns once calibration is complete.
*/
void MicroBitCompassCalibrator::calibrateUX(MicroBitEvent)
{
struct Point
{
uint8_t x;
uint8_t y;
uint8_t on;
};
const int PERIMETER_POINTS = 12;
const int PERIMETER_POINTS = 25;
const int PIXEL1_THRESHOLD = 200;
const int PIXEL2_THRESHOLD = 800;
const int PIXEL2_THRESHOLD = 680;
const int REDISPLAY_MSG_TIMEOUT_MS = 30000;
const int SAMPLES_END_MSG_COUNT = 15;
const int TIME_STEP = 100;
const int MSG_TIME = 155 * TIME_STEP; //We require MSG_TIME % TIME_STEP == 0
wait_ms(100);
Matrix4 X(PERIMETER_POINTS, 4);
Point perimeter[PERIMETER_POINTS] = {{1,0,0}, {2,0,0}, {3,0,0}, {4,1,0}, {4,2,0}, {4,3,0}, {3,4,0}, {2,4,0}, {1,4,0}, {0,3,0}, {0,2,0}, {0,1,0}};
Point cursor = {2,2,0};
static const Point perimeter[PERIMETER_POINTS] = {{0,0}, {1,0}, {2,0}, {3,0}, {4,0}, {0,1}, {1,1}, {2,1}, {3,1}, {4,1}, {0,2}, {1,2}, {2,2}, {3,2}, {4,2}, {0,3}, {1,3}, {2,3}, {3,3}, {4,3}, {0,4}, {1,4}, {2,4}, {3,4}, {4,4}};
Point cursor = {2,2};
MicroBitImage img(5,5);
MicroBitImage smiley("0,255,0,255,0\n0,255,0,255,0\n0,0,0,0,0\n255,0,0,0,255\n0,255,255,255,0\n");
int samples = 0;
Sample3D data[PERIMETER_POINTS];
uint8_t visited[PERIMETER_POINTS] = { 0 };
uint8_t cursor_on = 0;
uint8_t samples = 0;
uint8_t samples_this_period = 0;
int16_t remaining_scroll_time = MSG_TIME; // 32s maximum in uint16_t
// Firstly, we need to take over the display. Ensure all active animations are paused.
display.stopAnimation();
display.scrollAsync("DRAW A CIRCLE");
for (int i=0; i<110; i++)
wait_ms(100);
display.stopAnimation();
display.clear();
while(samples < PERIMETER_POINTS)
{
// Scroll a message the first time we enter this loop and every REDISPLAY_MSG_TIMEOUT_MS
if (remaining_scroll_time == MSG_TIME || remaining_scroll_time <= -REDISPLAY_MSG_TIMEOUT_MS) {
display.clear();
display.scrollAsync("TILT TO FILL SCREEN "); // Takes about 14s
remaining_scroll_time = MSG_TIME;
samples_this_period = 0;
}
else if (remaining_scroll_time == 0 || samples_this_period == SAMPLES_END_MSG_COUNT)
{
// This stops the scrolling at the end of the message.
// ...and it is the source of the ((MSG_TIME % TIME_STEP) == 0) requirement
display.stopAnimation();
}
// update our model of the flash status of the user controlled pixel.
cursor.on = (cursor.on + 1) % 4;
cursor_on = (cursor_on + 1) % 4;
// take a snapshot of the current accelerometer data.
int x = accelerometer.getX();
int y = accelerometer.getY();
// Wait a little whie for the button state to stabilise (one scheduler tick).
wait_ms(10);
// Deterine the position of the user controlled pixel on the screen.
if (x < -PIXEL2_THRESHOLD)
cursor.x = 0;
@ -130,60 +340,35 @@ void MicroBitCompassCalibrator::calibrate(MicroBitEvent)
// Turn on any pixels that have been visited.
for (int i=0; i<PERIMETER_POINTS; i++)
if (perimeter[i].on)
if (visited[i] == 1)
img.setPixelValue(perimeter[i].x, perimeter[i].y, 255);
// Update the pixel at the users position.
img.setPixelValue(cursor.x, cursor.y, 255);
img.setPixelValue(cursor.x, cursor.y, cursor_on);
// Update the buffer to the screen.
display.image.paste(img,0,0,0);
// Update the buffer to the screen ONLY if we've finished scrolling the message
if (remaining_scroll_time < 0 || samples_this_period > SAMPLES_END_MSG_COUNT)
display.image.paste(img,0,0,0);
// test if we need to update the state at the users position.
for (int i=0; i<PERIMETER_POINTS; i++)
{
if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !perimeter[i].on)
if (cursor.x == perimeter[i].x && cursor.y == perimeter[i].y && !(visited[i] == 1))
{
// Record the sample data for later processing...
Sample3D s = compass.getSample(RAW);
X.set(samples, 0, s.x);
X.set(samples, 1, s.y);
X.set(samples, 2, s.z);
X.set(samples, 3, 1);
//X.set(samples, 0, compass.getX(RAW));
//X.set(samples, 1, compass.getY(RAW));
//X.set(samples, 2, compass.getZ(RAW));
data[samples] = compass.getSample();
// Record that this pixel has been visited.
perimeter[i].on = 1;
visited[i] = 1;
samples++;
samples_this_period++;
}
}
wait_ms(100);
wait_ms(TIME_STEP);
remaining_scroll_time-=TIME_STEP;
}
// We have enough sample data to make a fairly accurate calibration.
// We use a Least Mean Squares approximation, as detailed in Freescale application note AN2426.
// Firstly, calculate the square of each sample.
Matrix4 Y(X.height(), 1);
for (int i = 0; i < X.height(); i++)
{
float v = X.get(i, 0)*X.get(i, 0) + X.get(i, 1)*X.get(i, 1) + X.get(i, 2)*X.get(i, 2);
Y.set(i, 0, v);
}
// Now perform a Least Squares Approximation.
Matrix4 Alpha = X.multiplyT(X).invert();
Matrix4 Gamma = X.multiplyT(Y);
Matrix4 Beta = Alpha.multiply(Gamma);
// The result contains the approximate zero point of each axis, but doubled.
// Halve each sample, and record this as the compass calibration data.
Sample3D cal ((int)(Beta.get(0,0) / 2), (int)(Beta.get(1,0) / 2), (int)(Beta.get(2,0) / 2));
compass.setCalibration(cal);
compass.setCalibration(calibrate(data, samples));
// Show a smiley to indicate that we're done, and continue on with the user program.
display.clear();