Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Estimation/Matrix/Matrix.cpp
- Committer:
- shimniok
- Date:
- 2018-11-30
- Revision:
- 24:46318b2bf974
- Parent:
- 23:a34af501ea89
File content as of revision 24:46318b2bf974:
#include <stdio.h> #include "Matrix.h" #include "util.h" unsigned int matrix_error = 0; void Matrix_Add(int n, int m, float *C, float *A, float *B) { for (int i = 0; i < n*m; i++) { C[i] = A[i] + B[i]; } } void Matrix_Subtract(int n, int m, float *C, float *A, float *B) { for (int i = 0; i < n*m; i++) { C[i] = A[i] - B[i]; } } // grabbed from MatrixMath library for Arduino // http://arduino.cc/playground/Code/MatrixMath // E.g., the equivalent Octave script: // A=[x; y; z]; // B=[xx xy xz; yx yy yz; zx xy zz]; // C=A*B; // Would be called like this: // Matrix_Mulitply(1, 3, 3, C, A, B); // void Matrix_Multiply(int m, int p, int n, float *C, float *A, float *B) { // 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) for (int i=0; i < m; i++) { for(int j=0; j < n; j++) { C[n*i+j] = 0; for (int k=0; k < p; k++) { C[i*n+j] += A[i*p+k] * B[k*n+j]; } } } return; } void Matrix_Transpose(int n, int m, float *C, float *A) { for (int i=0; i < n; i++) { for (int j=0; j < m; j++) { C[j*n+i] = A[i*m+j]; } } } #define fabs(x) (((x) < 0) ? -x : x) // Grabbed from MatrixMath library for Arduino // http://arduino.cc/playground/Code/MatrixMath //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. // * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED void Matrix_Inverse(int n, float *A) { // A = input matrix AND result matrix // n = number of rows = number of columns in A (n x n) int pivrow=0; // 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 (fabs(A[i*n+k]) >= tmp) { // 'Avoid using other functions inside abs()?' tmp = fabs(A[i*n+k]); pivrow = i; } } // check for singular matrix if (A[pivrow*n+k] == 0.0f) { matrix_error |= SINGULAR_MATRIX; //fprintf(stdout, "Inversion failed due to singular matrix"); return; } // 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; } void Matrix_Copy(int n, int m, float *C, float *A) { for (int i=0; i < n*m; i++) C[i] = A[i]; } void Matrix_print(int n, int m, float *A, const char *name) { fputs(name, stdout); fputs("=[", stdout); for (int i=0; i < n; i++) { for (int j=0; j < m; j++) { fputs(cvftos(A[i*m+j], 5), stdout); if (j < m-1) fputs(", ", stdout); } if (i < n-1) fputs("; ", stdout); } fprintf(stdout, "]\n"); }