WIP: Lower RAM footprint compass calibration
- moved to single precision floats - optimised transpose/multiply operation - optimisation of algorithmmaster
parent
844a77972f
commit
8756bbd103
|
@ -15,7 +15,7 @@
|
|||
*/
|
||||
class Matrix4
|
||||
{
|
||||
double *data; // Linear buffer representing the matrix.
|
||||
float *data; // Linear buffer representing the matrix.
|
||||
int rows; // The number of rows in the matrix.
|
||||
int cols; // The number of columns in the matrix.
|
||||
|
||||
|
@ -80,10 +80,10 @@ public:
|
|||
*
|
||||
* Example:
|
||||
* @code
|
||||
* double v = matrix.get(1,2);
|
||||
* float v = matrix.get(1,2);
|
||||
* @endcode
|
||||
*/
|
||||
double get(int row, int col);
|
||||
float get(int row, int col);
|
||||
|
||||
/**
|
||||
* Writes the matrix element at the given position.
|
||||
|
@ -97,7 +97,7 @@ public:
|
|||
* matrix.set(1,2,42.0);
|
||||
* @endcode
|
||||
*/
|
||||
void set(int row, int col, double v);
|
||||
void set(int row, int col, float v);
|
||||
|
||||
/**
|
||||
* Transposes this matrix.
|
||||
|
@ -119,7 +119,18 @@ public:
|
|||
* Matrix result = matrixA.multiply(matrixB);
|
||||
* @endcode
|
||||
*/
|
||||
Matrix4 multiply(Matrix4 &matrix);
|
||||
Matrix4 multiply(Matrix4 &matrix, bool transpose = false);
|
||||
|
||||
/**
|
||||
* Multiplies the transpose of this matrix with the given matrix (if possible).
|
||||
* @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
|
||||
*
|
||||
* Example:
|
||||
* @code
|
||||
* Matrix result = matrixA.multiplyT(matrixB);
|
||||
* @endcode
|
||||
*/
|
||||
Matrix4 multiplyT(Matrix4 &matrix);
|
||||
|
||||
/**
|
||||
* Performs an optimisaed inversion of a 4x4 matrix.
|
||||
|
@ -140,4 +151,9 @@ public:
|
|||
~Matrix4();
|
||||
};
|
||||
|
||||
inline Matrix4 Matrix4::multiplyT(Matrix4 &matrix)
|
||||
{
|
||||
return multiply(matrix, true);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -30,7 +30,7 @@ Matrix4::Matrix4(int rows, int cols)
|
|||
int size = rows * cols;
|
||||
|
||||
if (size > 0)
|
||||
data = new double[size];
|
||||
data = new float[size];
|
||||
else
|
||||
data = NULL;
|
||||
}
|
||||
|
@ -55,7 +55,7 @@ Matrix4::Matrix4(const Matrix4 &matrix)
|
|||
|
||||
if (size > 0)
|
||||
{
|
||||
data = new double[size];
|
||||
data = new float[size];
|
||||
for (int i = 0; i < size; i++)
|
||||
data[i] = matrix.data[i];
|
||||
}
|
||||
|
@ -105,10 +105,10 @@ int Matrix4::height()
|
|||
*
|
||||
* Example:
|
||||
* @code
|
||||
* double v = matrix.get(1,2);
|
||||
* float v = matrix.get(1,2);
|
||||
* @endcode
|
||||
*/
|
||||
double Matrix4::get(int row, int col)
|
||||
float Matrix4::get(int row, int col)
|
||||
{
|
||||
if (row < 0 || col < 0 || row >= rows || col >= cols)
|
||||
return 0;
|
||||
|
@ -128,7 +128,7 @@ double Matrix4::get(int row, int col)
|
|||
* matrix.set(1,2,42.0);
|
||||
* @endcode
|
||||
*/
|
||||
void Matrix4::set(int row, int col, double v)
|
||||
void Matrix4::set(int row, int col, float v)
|
||||
{
|
||||
if (row < 0 || col < 0 || row >= rows || col >= cols)
|
||||
return;
|
||||
|
@ -165,21 +165,24 @@ Matrix4 Matrix4::transpose()
|
|||
* Matrix result = matrixA.multiply(matrixB);
|
||||
* @endcode
|
||||
*/
|
||||
Matrix4 Matrix4::multiply(Matrix4 &matrix)
|
||||
Matrix4 Matrix4::multiply(Matrix4 &matrix, bool transpose)
|
||||
{
|
||||
if (width() != matrix.height())
|
||||
int w = transpose ? height() : width();
|
||||
int h = transpose ? width() : height();
|
||||
|
||||
if (w != matrix.height())
|
||||
return Matrix4(0, 0);
|
||||
|
||||
Matrix4 result(height(), matrix.width());
|
||||
Matrix4 result(h, matrix.width());
|
||||
|
||||
for (int r = 0; r < result.height(); r++)
|
||||
{
|
||||
for (int c = 0; c < result.width(); c++)
|
||||
{
|
||||
double v = 0.0;
|
||||
float v = 0.0;
|
||||
|
||||
for (int i = 0; i < width(); i++)
|
||||
v += get(r, i) * matrix.get(i, c);
|
||||
for (int i = 0; i < w; i++)
|
||||
v += (transpose ? get(i, r) : get(r, i)) * matrix.get(i, c);
|
||||
|
||||
result.set(r, c, v);
|
||||
}
|
||||
|
@ -224,7 +227,7 @@ Matrix4 Matrix4::invert()
|
|||
result.data[14] = -data[0] * data[5] * data[14] + data[0] * data[6] * data[13] + data[4] * data[1] * data[14] - data[4] * data[2] * data[13] - data[12] * data[1] * data[6] + data[12] * data[2] * data[5];
|
||||
result.data[15] = data[0] * data[5] * data[10] - data[0] * data[6] * data[9] - data[4] * data[1] * data[10] + data[4] * data[2] * data[9] + data[8] * data[1] * data[6] - data[8] * data[2] * data[5];
|
||||
|
||||
double det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12];
|
||||
float det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12];
|
||||
|
||||
if (det == 0)
|
||||
return Matrix4(0, 0);
|
||||
|
|
|
@ -248,18 +248,40 @@ void MicroBit::compassCalibrator(MicroBitEvent)
|
|||
|
||||
// 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.
|
||||
uBit.serial.printf("Input Data\n");
|
||||
for (int i=0; i<PERIMETER_POINTS; i++)
|
||||
uBit.serial.printf("%d : %d : %d : %d\n", (int)X.get(i,0), (int)X.get(i,1), (int)X.get(i,2), (int)X.get(i, 3));
|
||||
|
||||
|
||||
// Firstly, calculate the square of each sample.
|
||||
Matrix4 Y(X.height(), 1);
|
||||
for (int i = 0; i < X.height(); i++)
|
||||
{
|
||||
double 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);
|
||||
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 XT = X.transpose();
|
||||
Matrix4 Beta = XT.multiply(X).invert().multiply(XT).multiply(Y);
|
||||
// Method1
|
||||
//Matrix4 XT = X.transpose();
|
||||
//Matrix4 Beta = XT.multiply(X).invert().multiply(XT).multiply(Y);
|
||||
|
||||
//uBit.serial.printf("Output Data1\n");
|
||||
//uBit.serial.printf("%d : %d : %d [%d]\n", (int)(Beta.get(0,0)/2), (int)(Beta.get(1,0)/2), (int)(Beta.get(2,0)/2), (int)Beta.get(3, 0));
|
||||
|
||||
// Method2
|
||||
//Matrix4 Alpha2 = XT.multiply(X).invert();
|
||||
//Matrix4 Gamma2 = XT.multiply(Y);
|
||||
//Matrix4 Beta2 = Alpha2.multiply(Gamma2);
|
||||
//uBit.serial.printf("Output Data2\n");
|
||||
//uBit.serial.printf("%d : %d : %d [%d]\n", (int)(Beta2.get(0,0)/2), (int)(Beta2.get(1,0)/2), (int)(Beta2.get(2,0)/2), (int)Beta2.get(3, 0));
|
||||
|
||||
// Method3
|
||||
Matrix4 Alpha = X.multiplyT(X).invert();
|
||||
Matrix4 Gamma = X.multiplyT(Y);
|
||||
Matrix4 Beta = Alpha.multiply(Gamma);
|
||||
uBit.serial.printf("Output Data\n");
|
||||
uBit.serial.printf("%d : %d : %d [%d]\n", (int)(Beta.get(0,0)/2), (int)(Beta.get(1,0)/2), (int)(Beta.get(2,0)/2), (int)Beta.get(3, 0));
|
||||
|
||||
// The result contains the approximate zero point of each axis, but doubled.
|
||||
// Halve each sample, and record this as the compass calibration data.
|
||||
|
|
Loading…
Reference in New Issue