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@24:46318b2bf974, 2018-11-30 (annotated)
- Committer:
- shimniok
- Date:
- Fri Nov 30 15:44:40 2018 +0000
- Revision:
- 24:46318b2bf974
- Parent:
- 23:a34af501ea89
Restored contents of Matrix.cpp
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 24:46318b2bf974 | 1 | #include <stdio.h> |
shimniok | 24:46318b2bf974 | 2 | #include "Matrix.h" |
shimniok | 24:46318b2bf974 | 3 | #include "util.h" |
shimniok | 24:46318b2bf974 | 4 | |
shimniok | 24:46318b2bf974 | 5 | unsigned int matrix_error = 0; |
shimniok | 24:46318b2bf974 | 6 | |
shimniok | 24:46318b2bf974 | 7 | void Matrix_Add(int n, int m, float *C, float *A, float *B) |
shimniok | 24:46318b2bf974 | 8 | { |
shimniok | 24:46318b2bf974 | 9 | for (int i = 0; i < n*m; i++) { |
shimniok | 24:46318b2bf974 | 10 | C[i] = A[i] + B[i]; |
shimniok | 24:46318b2bf974 | 11 | } |
shimniok | 24:46318b2bf974 | 12 | } |
shimniok | 24:46318b2bf974 | 13 | |
shimniok | 24:46318b2bf974 | 14 | void Matrix_Subtract(int n, int m, float *C, float *A, float *B) |
shimniok | 24:46318b2bf974 | 15 | { |
shimniok | 24:46318b2bf974 | 16 | for (int i = 0; i < n*m; i++) { |
shimniok | 24:46318b2bf974 | 17 | C[i] = A[i] - B[i]; |
shimniok | 24:46318b2bf974 | 18 | } |
shimniok | 24:46318b2bf974 | 19 | } |
shimniok | 24:46318b2bf974 | 20 | |
shimniok | 24:46318b2bf974 | 21 | // grabbed from MatrixMath library for Arduino |
shimniok | 24:46318b2bf974 | 22 | // http://arduino.cc/playground/Code/MatrixMath |
shimniok | 24:46318b2bf974 | 23 | // E.g., the equivalent Octave script: |
shimniok | 24:46318b2bf974 | 24 | // A=[x; y; z]; |
shimniok | 24:46318b2bf974 | 25 | // B=[xx xy xz; yx yy yz; zx xy zz]; |
shimniok | 24:46318b2bf974 | 26 | // C=A*B; |
shimniok | 24:46318b2bf974 | 27 | // Would be called like this: |
shimniok | 24:46318b2bf974 | 28 | // Matrix_Mulitply(1, 3, 3, C, A, B); |
shimniok | 24:46318b2bf974 | 29 | // |
shimniok | 24:46318b2bf974 | 30 | void Matrix_Multiply(int m, int p, int n, float *C, float *A, float *B) |
shimniok | 24:46318b2bf974 | 31 | { |
shimniok | 24:46318b2bf974 | 32 | // A = input matrix (m x p) |
shimniok | 24:46318b2bf974 | 33 | // B = input matrix (p x n) |
shimniok | 24:46318b2bf974 | 34 | // m = number of rows in A |
shimniok | 24:46318b2bf974 | 35 | // p = number of columns in A = number of rows in B |
shimniok | 24:46318b2bf974 | 36 | // n = number of columns in B |
shimniok | 24:46318b2bf974 | 37 | // C = output matrix = A*B (m x n) |
shimniok | 24:46318b2bf974 | 38 | for (int i=0; i < m; i++) { |
shimniok | 24:46318b2bf974 | 39 | for(int j=0; j < n; j++) { |
shimniok | 24:46318b2bf974 | 40 | C[n*i+j] = 0; |
shimniok | 24:46318b2bf974 | 41 | for (int k=0; k < p; k++) { |
shimniok | 24:46318b2bf974 | 42 | C[i*n+j] += A[i*p+k] * B[k*n+j]; |
shimniok | 24:46318b2bf974 | 43 | } |
shimniok | 24:46318b2bf974 | 44 | } |
shimniok | 24:46318b2bf974 | 45 | } |
shimniok | 24:46318b2bf974 | 46 | |
shimniok | 24:46318b2bf974 | 47 | return; |
shimniok | 24:46318b2bf974 | 48 | } |
shimniok | 24:46318b2bf974 | 49 | |
shimniok | 24:46318b2bf974 | 50 | |
shimniok | 24:46318b2bf974 | 51 | void Matrix_Transpose(int n, int m, float *C, float *A) |
shimniok | 24:46318b2bf974 | 52 | { |
shimniok | 24:46318b2bf974 | 53 | for (int i=0; i < n; i++) { |
shimniok | 24:46318b2bf974 | 54 | for (int j=0; j < m; j++) { |
shimniok | 24:46318b2bf974 | 55 | C[j*n+i] = A[i*m+j]; |
shimniok | 24:46318b2bf974 | 56 | } |
shimniok | 24:46318b2bf974 | 57 | } |
shimniok | 24:46318b2bf974 | 58 | } |
shimniok | 24:46318b2bf974 | 59 | |
shimniok | 24:46318b2bf974 | 60 | #define fabs(x) (((x) < 0) ? -x : x) |
shimniok | 24:46318b2bf974 | 61 | |
shimniok | 24:46318b2bf974 | 62 | // Grabbed from MatrixMath library for Arduino |
shimniok | 24:46318b2bf974 | 63 | // http://arduino.cc/playground/Code/MatrixMath |
shimniok | 24:46318b2bf974 | 64 | //Matrix Inversion Routine |
shimniok | 24:46318b2bf974 | 65 | // * This function inverts a matrix based on the Gauss Jordan method. |
shimniok | 24:46318b2bf974 | 66 | // * Specifically, it uses partial pivoting to improve numeric stability. |
shimniok | 24:46318b2bf974 | 67 | // * The algorithm is drawn from those presented in |
shimniok | 24:46318b2bf974 | 68 | // NUMERICAL RECIPES: The Art of Scientific Computing. |
shimniok | 24:46318b2bf974 | 69 | // * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED |
shimniok | 24:46318b2bf974 | 70 | void Matrix_Inverse(int n, float *A) |
shimniok | 24:46318b2bf974 | 71 | { |
shimniok | 24:46318b2bf974 | 72 | // A = input matrix AND result matrix |
shimniok | 24:46318b2bf974 | 73 | // n = number of rows = number of columns in A (n x n) |
shimniok | 24:46318b2bf974 | 74 | int pivrow=0; // keeps track of current pivot row |
shimniok | 24:46318b2bf974 | 75 | int k,i,j; // k: overall index along diagonal; i: row index; j: col index |
shimniok | 24:46318b2bf974 | 76 | int pivrows[n]; // keeps track of rows swaps to undo at end |
shimniok | 24:46318b2bf974 | 77 | float tmp; // used for finding max value and making column swaps |
shimniok | 24:46318b2bf974 | 78 | |
shimniok | 24:46318b2bf974 | 79 | for (k = 0; k < n; k++) { |
shimniok | 24:46318b2bf974 | 80 | // find pivot row, the row with biggest entry in current column |
shimniok | 24:46318b2bf974 | 81 | tmp = 0; |
shimniok | 24:46318b2bf974 | 82 | for (i = k; i < n; i++) { |
shimniok | 24:46318b2bf974 | 83 | if (fabs(A[i*n+k]) >= tmp) { // 'Avoid using other functions inside abs()?' |
shimniok | 24:46318b2bf974 | 84 | tmp = fabs(A[i*n+k]); |
shimniok | 24:46318b2bf974 | 85 | pivrow = i; |
shimniok | 24:46318b2bf974 | 86 | } |
shimniok | 24:46318b2bf974 | 87 | } |
shimniok | 24:46318b2bf974 | 88 | |
shimniok | 24:46318b2bf974 | 89 | // check for singular matrix |
shimniok | 24:46318b2bf974 | 90 | if (A[pivrow*n+k] == 0.0f) { |
shimniok | 24:46318b2bf974 | 91 | matrix_error |= SINGULAR_MATRIX; |
shimniok | 24:46318b2bf974 | 92 | //fprintf(stdout, "Inversion failed due to singular matrix"); |
shimniok | 24:46318b2bf974 | 93 | return; |
shimniok | 24:46318b2bf974 | 94 | } |
shimniok | 24:46318b2bf974 | 95 | |
shimniok | 24:46318b2bf974 | 96 | // Execute pivot (row swap) if needed |
shimniok | 24:46318b2bf974 | 97 | if (pivrow != k) { |
shimniok | 24:46318b2bf974 | 98 | // swap row k with pivrow |
shimniok | 24:46318b2bf974 | 99 | for (j = 0; j < n; j++) { |
shimniok | 24:46318b2bf974 | 100 | tmp = A[k*n+j]; |
shimniok | 24:46318b2bf974 | 101 | A[k*n+j] = A[pivrow*n+j]; |
shimniok | 24:46318b2bf974 | 102 | A[pivrow*n+j] = tmp; |
shimniok | 24:46318b2bf974 | 103 | } |
shimniok | 24:46318b2bf974 | 104 | } |
shimniok | 24:46318b2bf974 | 105 | pivrows[k] = pivrow; // record row swap (even if no swap happened) |
shimniok | 24:46318b2bf974 | 106 | |
shimniok | 24:46318b2bf974 | 107 | tmp = 1.0f/A[k*n+k]; // invert pivot element |
shimniok | 24:46318b2bf974 | 108 | A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix |
shimniok | 24:46318b2bf974 | 109 | |
shimniok | 24:46318b2bf974 | 110 | // Perform row reduction (divide every element by pivot) |
shimniok | 24:46318b2bf974 | 111 | for (j = 0; j < n; j++) { |
shimniok | 24:46318b2bf974 | 112 | A[k*n+j] = A[k*n+j]*tmp; |
shimniok | 24:46318b2bf974 | 113 | } |
shimniok | 24:46318b2bf974 | 114 | |
shimniok | 24:46318b2bf974 | 115 | // Now eliminate all other entries in this column |
shimniok | 24:46318b2bf974 | 116 | for (i = 0; i < n; i++) { |
shimniok | 24:46318b2bf974 | 117 | if (i != k) { |
shimniok | 24:46318b2bf974 | 118 | tmp = A[i*n+k]; |
shimniok | 24:46318b2bf974 | 119 | A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat |
shimniok | 24:46318b2bf974 | 120 | for (j = 0; j < n; j++) { |
shimniok | 24:46318b2bf974 | 121 | A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp; |
shimniok | 24:46318b2bf974 | 122 | } |
shimniok | 24:46318b2bf974 | 123 | } |
shimniok | 24:46318b2bf974 | 124 | } |
shimniok | 24:46318b2bf974 | 125 | } |
shimniok | 24:46318b2bf974 | 126 | |
shimniok | 24:46318b2bf974 | 127 | // Done, now need to undo pivot row swaps by doing column swaps in reverse order |
shimniok | 24:46318b2bf974 | 128 | for (k = n-1; k >= 0; k--) { |
shimniok | 24:46318b2bf974 | 129 | if (pivrows[k] != k) { |
shimniok | 24:46318b2bf974 | 130 | for (i = 0; i < n; i++) { |
shimniok | 24:46318b2bf974 | 131 | tmp = A[i*n+k]; |
shimniok | 24:46318b2bf974 | 132 | A[i*n+k] = A[i*n+pivrows[k]]; |
shimniok | 24:46318b2bf974 | 133 | A[i*n+pivrows[k]] = tmp; |
shimniok | 24:46318b2bf974 | 134 | } |
shimniok | 24:46318b2bf974 | 135 | } |
shimniok | 24:46318b2bf974 | 136 | } |
shimniok | 24:46318b2bf974 | 137 | return; |
shimniok | 24:46318b2bf974 | 138 | } |
shimniok | 24:46318b2bf974 | 139 | |
shimniok | 24:46318b2bf974 | 140 | |
shimniok | 24:46318b2bf974 | 141 | void Matrix_Copy(int n, int m, float *C, float *A) |
shimniok | 24:46318b2bf974 | 142 | { |
shimniok | 24:46318b2bf974 | 143 | for (int i=0; i < n*m; i++) |
shimniok | 24:46318b2bf974 | 144 | C[i] = A[i]; |
shimniok | 24:46318b2bf974 | 145 | } |
shimniok | 24:46318b2bf974 | 146 | |
shimniok | 24:46318b2bf974 | 147 | void Matrix_print(int n, int m, float *A, const char *name) |
shimniok | 24:46318b2bf974 | 148 | { |
shimniok | 24:46318b2bf974 | 149 | fputs(name, stdout); |
shimniok | 24:46318b2bf974 | 150 | fputs("=[", stdout); |
shimniok | 24:46318b2bf974 | 151 | for (int i=0; i < n; i++) { |
shimniok | 24:46318b2bf974 | 152 | for (int j=0; j < m; j++) { |
shimniok | 24:46318b2bf974 | 153 | fputs(cvftos(A[i*m+j], 5), stdout); |
shimniok | 24:46318b2bf974 | 154 | if (j < m-1) fputs(", ", stdout); |
shimniok | 24:46318b2bf974 | 155 | } |
shimniok | 24:46318b2bf974 | 156 | if (i < n-1) fputs("; ", stdout); |
shimniok | 24:46318b2bf974 | 157 | } |
shimniok | 24:46318b2bf974 | 158 | fprintf(stdout, "]\n"); |
shimniok | 24:46318b2bf974 | 159 | } |