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
Diff: Estimation/Matrix/Matrix.cpp
- Revision:
- 24:46318b2bf974
- Parent:
- 23:a34af501ea89
--- a/Estimation/Matrix/Matrix.cpp Fri Nov 30 15:41:05 2018 +0000 +++ b/Estimation/Matrix/Matrix.cpp Fri Nov 30 15:44:40 2018 +0000 @@ -0,0 +1,159 @@ +#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"); +}