I am developing code for fitting a least-squares quadratic to data from sensors. The method involves creating an appropriate matrix, finding its inverse, and multiplying a data vector appropriate for the sensor data by that inverse matrix. Everything works OK until I perform that last multiplication. There, for example, multiplying the term 19.00 from the inverse matrix by the corresponding term 2100.00 from the data vector yields 39895.88 instead of the correct value 39900.00. However, performing the matrix multiplication using data copied from the calculated matrices yields correct results; 19.00 * 2100.00 = 39900.00.
I used excerpts from the Arduino matrix library as well as code of my own. The code includes lots of print statements which I hope display the problem clearly.
I hope someone can see and let me know where where things are going wrong .
Thank you.
Ted
// Example showing errors multiplying matrices
const int nScanAngles = 7;
int scanAngles[nScanAngles] = {-45, -30, -15, 0, 15, 30, 45}; // Scanner angles
int integerArguments[nScanAngles] = {-3, -2, -1, 0, 1, 2, 3};
int leastSqInputs[nScanAngles];
int inputs[nScanAngles]; // scan A/D readings
float m[3][3];
float a[3] = {100, 0, 50}; // Coefficients for least-squares quadratic fit
float newA[3];
float data[3]; // Right-hand side
float mInverse[3][3];
float c[3][3];
float mTest[3][3] = {
{19.00, 21.00, 5.00},
{21.00, 24.50, 6.00},
{5.00, 6.00, 1.50}};
float xTest[3] = {2100.00, 0.00, 12600.00};
float yTest[3];
/*
* Excerpts from MatrixMath.cpp Library for Matrix Math
*
* Created by Charlie Matlack on 12/18/10.
* Modified from code by RobH45345 on Arduino Forums, taken from unknown source.
*
*/
// Matrix Printing Routine
// Uses tabs to separate numbers under assumption printed float width won't cause problems
void matrixPrint(float* A, int m, int n, String label){
// A = input matrix (m x n)
int i,j;
Serial.println();
Serial.println(label);
for (i=0; i<m; i++){
for (j=0;j<n;j++){
Serial.print(A[n*i+j]);
Serial.print("\t");
}
Serial.println();
}
}
void matrixCopy(float* A, int n, int m, float* B)
{
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
B[n*i+j] = A[n*i+j];
}
}
//Matrix Multiplication Routine
// C = A*B
void matrixMultiply(float* A, float* B, int m, int p, int n, float* C)
{
// A = input matrix (m x p)
// B = input matrix (p x n)
// m = number of rows in A
// p = number of columns in A = number of rows in B
// n = number of columns in B
// C = output matrix = A*B (m x n)
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
C[n*i+j]=0;
for (k=0;k<p;k++) C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
}
}
//Matrix Inversion Routine
// * This function inverts a matrix based on the Gauss Jordan method.
// * Specifically, it uses partial pivoting to improve numeric stability.
// * The algorithm is drawn from those presented in
// NUMERICAL RECIPES: The Art of Scientific Computing.
// * The function returns 1 on success, 0 on failure.
// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
int matrixInvert(float* A, int n)
{
// A = input matrix AND result matrix
// n = number of rows = number of columns in A (n x n)
int pivrow; // keeps track of current pivot row
int k,i,j; // k: overall index along diagonal; i: row index; j: col index
int pivrows[n]; // keeps track of rows swaps to undo at end
float tmp; // used for finding max value and making column swaps
for (k = 0; k < n; k++)
{
// find pivot row, the row with biggest entry in current column
tmp = 0;
for (i = k; i < n; i++)
{
if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
{
tmp = abs(A[i*n+k]);
pivrow = i;
}
}
// check for singular matrix
if (A[pivrow*n+k] == 0.0f)
{
Serial.println("Inversion failed due to singular matrix");
return 0;
}
// Execute pivot (row swap) if needed
if (pivrow != k)
{
// swap row k with pivrow
for (j = 0; j < n; j++)
{
tmp = A[k*n+j];
A[k*n+j] = A[pivrow*n+j];
A[pivrow*n+j] = tmp;
}
}
pivrows[k] = pivrow; // record row swap (even if no swap happened)
tmp = 1.0f/A[k*n+k]; // invert pivot element
A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
// Perform row reduction (divide every element by pivot)
for (j = 0; j < n; j++)
{
A[k*n+j] = A[k*n+j]*tmp;
}
// Now eliminate all other entries in this column
for (i = 0; i < n; i++)
{
if (i != k)
{
tmp = A[i*n+k];
A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
for (j = 0; j < n; j++)
{
A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
}
}
}
}
// Done, now need to undo pivot row swaps by doing column swaps in reverse order
for (k = n-1; k >= 0; k--)
{
if (pivrows[k] != k)
{
for (i = 0; i < n; i++)
{
tmp = A[i*n+k];
A[i*n+k] = A[i*n+pivrows[k]];
A[i*n+pivrows[k]] = tmp;
}
}
}
return 1;
}
//########################################################################################
// My code starts here
float SQUARE(float x){ return x*x;}
float aij(int I, int J, int N, int* t){ // t is an array of integers
float term = 0;
for(int i = 0; i < N; i++){
term = term + (float)pow((float)t[i], (float)(I + J));
}
return term;
}
float formMatrix(int N, int* t){ // matrix is an N by N array of floats, t an array of ints
for( int i = 0; i < N; i++){
for(int j = 0; j < N; j++){
m[i][j] = aij(i, j, N, t);
}
}
}
float vectorTerm(int I, int N, int* scannerInputs){
float term = 0;
for(int i = 0; i < N; i++){ // Sum over all the scannerInputs
term = term + pow((float)integerArguments[i], (float)I)*scannerInputs[i];
}
return term;
}
void matMult(float* m, float* b, float* c){ // {c} = [m]{b} where [m] is 3 by 3 and {b} is 1 by 3
for(int i = 0; i < 3; i++){ // For each row
c[i] = 0;
for(int j = 0; j < 3; j++){
c[i] = c[i] + m[3*i+j]*b[j];
Serial.print("In matMult, m["); Serial.print(i); Serial.print("]["); Serial.print(j);Serial.print("] = ");
Serial.print(m[3*i+j]); Serial.print(", data["); Serial.print(j); Serial.print("] = "); Serial.print(b[j]);
Serial.print(", c["); Serial.print(i); Serial.print("] = "); Serial.println(c[i]);
}
Serial.println();
}
}
void newMatMult(){ // {newA} = [m]{data} where [m] is 3 by 3 and {data} is 1 by 3
for(int i = 0; i < 3; i++){ // For each row
newA[i] = 0;
for(int j = 0; j < 3; j++){
newA[i] = newA[i] + mInverse[i][j]*data[j];
Serial.print("In newMatMult, m["); Serial.print(i); Serial.print("]["); Serial.print(j);Serial.print("] = ");
Serial.print(mInverse[i][j]); Serial.print(", data["); Serial.print(j); Serial.print("] = "); Serial.print(data[j]);
Serial.print(", newA["); Serial.print(i); Serial.print("] = "); Serial.println(newA[i]);
}
Serial.println();
}
}
void setup(){
Serial.begin(57600); // Enable Serial for testing
// Generate the arrays
formMatrix(3, integerArguments); // [m] of [m]{a} = {d} for least-squares approximation
float matrix[3][3];
Serial.println("Find vector {a} such that [m]{a} = {data} where [m] and {data} are known.");
Serial.println("The solution is {a} = [b]{data} where [b] is the inverse of [m].");
for(int i = 0; i < nScanAngles; i++){ // Calculate inputs for the assumed coefficients
inputs[i] = (a[2]*SQUARE((float)integerArguments[i]) + a[1]*(float)integerArguments[i] + a[0])/(float)1;}
for(int i = 0; i < 3; i++){data[i] = vectorTerm(i, nScanAngles, inputs);}
// Form the inverse
matrixCopy((float*)m, 3, 3, (float*)mInverse); // Save the origional [m] in [mInverse]
matrixPrint((float*)m, 3, 3, "The matrix m[]:");
matrixPrint((float*)data, 3, 1, "The data:");
matrixInvert((float*)mInverse, 3); // [mInverse] is now the inverse of [m]
matrixCopy((float*)mInverse, 3, 3, (float*)matrix); // Save the origional [m] in [mInverse]
matrixPrint((float*)mInverse, 3, 3, "The inverted matrix mInverse[]:");
matrixMultiply((float*)mInverse, (float*)m, 3, 3, 3, (float*) c); // Check that the [m][b] product is the identity matrix
matrixPrint((float*)c, 3, 3, "The product of m[] and its inverse to verify inverse:");
Serial.println();
// Trouble occurs here with the calculated arraya
Serial.println("The product [mInverse]{data}:");
matrixMultiply((float*)mInverse, (float*)data, 3, 3, 1, (float*) newA); // Check that the [m][b] product is the identity matrix
matrixPrint((float*)newA, 3, 1, "Resulting product from the library:");
Serial.println();
Serial.println("The product [mInverse]{data} using my routines:");
matMult((float*)mInverse, (float*)data, (float*) newA); // [mInverse]{data} = {newA}
newMatMult();
// No trouble when duplicates of the calculated arrays are used
Serial.println("The product [mTest]{xTest} (Yields correct results):");
matMult((float*)mTest, (float*)xTest, (float*) yTest); // [mTest]{xTest} = {yTest}
}
void loop(){}