WIP: Lower RAM footprint compass calibration

- moved to single precision floats
- optimised transpose/multiply operation
- optimisation of algorithm

inc/Matrix4.h

@@ -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

source/Matrix4.cpp

@@ -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);

source/MicroBit.cpp

@@ -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.