Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Estimation/Matrix/Matrix.cpp@0:826c6171fc1b, 2012-06-20 (annotated)
- Committer:
- shimniok
- Date:
- Wed Jun 20 14:57:48 2012 +0000
- Revision:
- 0:826c6171fc1b
Updated documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:826c6171fc1b | 1 | #include <stdio.h> |
shimniok | 0:826c6171fc1b | 2 | #include "Matrix.h" |
shimniok | 0:826c6171fc1b | 3 | |
shimniok | 0:826c6171fc1b | 4 | unsigned int matrix_error = 0; |
shimniok | 0:826c6171fc1b | 5 | |
shimniok | 0:826c6171fc1b | 6 | void Vector_Cross_Product(float C[3], float A[3], float B[3]) |
shimniok | 0:826c6171fc1b | 7 | { |
shimniok | 0:826c6171fc1b | 8 | C[0] = (A[1] * B[2]) - (A[2] * B[1]); |
shimniok | 0:826c6171fc1b | 9 | C[1] = (A[2] * B[0]) - (A[0] * B[2]); |
shimniok | 0:826c6171fc1b | 10 | C[2] = (A[0] * B[1]) - (A[1] * B[0]); |
shimniok | 0:826c6171fc1b | 11 | |
shimniok | 0:826c6171fc1b | 12 | return; |
shimniok | 0:826c6171fc1b | 13 | } |
shimniok | 0:826c6171fc1b | 14 | |
shimniok | 0:826c6171fc1b | 15 | void Vector_Scale(float C[3], float A[3], float b) |
shimniok | 0:826c6171fc1b | 16 | { |
shimniok | 0:826c6171fc1b | 17 | for (int m = 0; m < 3; m++) |
shimniok | 0:826c6171fc1b | 18 | C[m] = A[m] * b; |
shimniok | 0:826c6171fc1b | 19 | |
shimniok | 0:826c6171fc1b | 20 | return; |
shimniok | 0:826c6171fc1b | 21 | } |
shimniok | 0:826c6171fc1b | 22 | |
shimniok | 0:826c6171fc1b | 23 | float Vector_Dot_Product(float A[3], float B[3]) |
shimniok | 0:826c6171fc1b | 24 | { |
shimniok | 0:826c6171fc1b | 25 | float result = 0.0; |
shimniok | 0:826c6171fc1b | 26 | |
shimniok | 0:826c6171fc1b | 27 | for (int i = 0; i < 3; i++) { |
shimniok | 0:826c6171fc1b | 28 | result += A[i] * B[i]; |
shimniok | 0:826c6171fc1b | 29 | } |
shimniok | 0:826c6171fc1b | 30 | |
shimniok | 0:826c6171fc1b | 31 | return result; |
shimniok | 0:826c6171fc1b | 32 | } |
shimniok | 0:826c6171fc1b | 33 | |
shimniok | 0:826c6171fc1b | 34 | void Vector_Add(float C[3], float A[3], float B[3]) |
shimniok | 0:826c6171fc1b | 35 | { |
shimniok | 0:826c6171fc1b | 36 | for (int m = 0; m < 3; m++) |
shimniok | 0:826c6171fc1b | 37 | C[m] = A[m] + B[m]; |
shimniok | 0:826c6171fc1b | 38 | |
shimniok | 0:826c6171fc1b | 39 | return; |
shimniok | 0:826c6171fc1b | 40 | } |
shimniok | 0:826c6171fc1b | 41 | |
shimniok | 0:826c6171fc1b | 42 | void Vector_Add(float C[3][3], float A[3][3], float B[3][3]) |
shimniok | 0:826c6171fc1b | 43 | { |
shimniok | 0:826c6171fc1b | 44 | for (int m = 0; m < 3; m++) |
shimniok | 0:826c6171fc1b | 45 | for (int n = 0; n < 3; n++) |
shimniok | 0:826c6171fc1b | 46 | C[m][n] = A[m][n] + B[m][n]; |
shimniok | 0:826c6171fc1b | 47 | } |
shimniok | 0:826c6171fc1b | 48 | |
shimniok | 0:826c6171fc1b | 49 | void Matrix_Add(float C[3][3], float A[3][3], float B[3][3]) |
shimniok | 0:826c6171fc1b | 50 | { |
shimniok | 0:826c6171fc1b | 51 | for (int i = 0; i < 3; i++) { |
shimniok | 0:826c6171fc1b | 52 | for (int j = 0; j < 3; j++) { |
shimniok | 0:826c6171fc1b | 53 | C[i][j] = A[i][j] + B[i][j]; |
shimniok | 0:826c6171fc1b | 54 | } |
shimniok | 0:826c6171fc1b | 55 | } |
shimniok | 0:826c6171fc1b | 56 | } |
shimniok | 0:826c6171fc1b | 57 | |
shimniok | 0:826c6171fc1b | 58 | void Matrix_Add(int n, int m, float *C, float *A, float *B) |
shimniok | 0:826c6171fc1b | 59 | { |
shimniok | 0:826c6171fc1b | 60 | for (int i = 0; i < n*m; i++) { |
shimniok | 0:826c6171fc1b | 61 | C[i] = A[i] + B[i]; |
shimniok | 0:826c6171fc1b | 62 | } |
shimniok | 0:826c6171fc1b | 63 | } |
shimniok | 0:826c6171fc1b | 64 | |
shimniok | 0:826c6171fc1b | 65 | void Matrix_Subtract(int n, int m, float *C, float *A, float *B) |
shimniok | 0:826c6171fc1b | 66 | { |
shimniok | 0:826c6171fc1b | 67 | for (int i = 0; i < n*m; i++) { |
shimniok | 0:826c6171fc1b | 68 | C[i] = A[i] - B[i]; |
shimniok | 0:826c6171fc1b | 69 | } |
shimniok | 0:826c6171fc1b | 70 | } |
shimniok | 0:826c6171fc1b | 71 | |
shimniok | 0:826c6171fc1b | 72 | |
shimniok | 0:826c6171fc1b | 73 | |
shimniok | 0:826c6171fc1b | 74 | // grabbed from MatrixMath library for Arduino |
shimniok | 0:826c6171fc1b | 75 | // http://arduino.cc/playground/Code/MatrixMath |
shimniok | 0:826c6171fc1b | 76 | // E.g., the equivalent Octave script: |
shimniok | 0:826c6171fc1b | 77 | // A=[x; y; z]; |
shimniok | 0:826c6171fc1b | 78 | // B=[xx xy xz; yx yy yz; zx xy zz]; |
shimniok | 0:826c6171fc1b | 79 | // C=A*B; |
shimniok | 0:826c6171fc1b | 80 | // Would be called like this: |
shimniok | 0:826c6171fc1b | 81 | // Matrix_Mulitply(1, 3, 3, C, A, B); |
shimniok | 0:826c6171fc1b | 82 | // |
shimniok | 0:826c6171fc1b | 83 | void Matrix_Multiply(int m, int p, int n, float *C, float *A, float *B) |
shimniok | 0:826c6171fc1b | 84 | { |
shimniok | 0:826c6171fc1b | 85 | // A = input matrix (m x p) |
shimniok | 0:826c6171fc1b | 86 | // B = input matrix (p x n) |
shimniok | 0:826c6171fc1b | 87 | // m = number of rows in A |
shimniok | 0:826c6171fc1b | 88 | // p = number of columns in A = number of rows in B |
shimniok | 0:826c6171fc1b | 89 | // n = number of columns in B |
shimniok | 0:826c6171fc1b | 90 | // C = output matrix = A*B (m x n) |
shimniok | 0:826c6171fc1b | 91 | for (int i=0; i < m; i++) { |
shimniok | 0:826c6171fc1b | 92 | for(int j=0; j < n; j++) { |
shimniok | 0:826c6171fc1b | 93 | C[n*i+j] = 0; |
shimniok | 0:826c6171fc1b | 94 | for (int k=0; k < p; k++) { |
shimniok | 0:826c6171fc1b | 95 | C[i*n+j] += A[i*p+k] * B[k*n+j]; |
shimniok | 0:826c6171fc1b | 96 | } |
shimniok | 0:826c6171fc1b | 97 | } |
shimniok | 0:826c6171fc1b | 98 | } |
shimniok | 0:826c6171fc1b | 99 | |
shimniok | 0:826c6171fc1b | 100 | return; |
shimniok | 0:826c6171fc1b | 101 | } |
shimniok | 0:826c6171fc1b | 102 | |
shimniok | 0:826c6171fc1b | 103 | void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3]) |
shimniok | 0:826c6171fc1b | 104 | { |
shimniok | 0:826c6171fc1b | 105 | for (int i = 0; i < 3; i++) { |
shimniok | 0:826c6171fc1b | 106 | for (int j = 0; j < 3; j++) { |
shimniok | 0:826c6171fc1b | 107 | C[i][j] = 0; |
shimniok | 0:826c6171fc1b | 108 | for (int k = 0; k < 3; k++) { |
shimniok | 0:826c6171fc1b | 109 | C[i][j] += A[i][k] * B[k][j]; |
shimniok | 0:826c6171fc1b | 110 | } |
shimniok | 0:826c6171fc1b | 111 | } |
shimniok | 0:826c6171fc1b | 112 | } |
shimniok | 0:826c6171fc1b | 113 | } |
shimniok | 0:826c6171fc1b | 114 | |
shimniok | 0:826c6171fc1b | 115 | |
shimniok | 0:826c6171fc1b | 116 | void Matrix_Transpose(int n, int m, float *C, float *A) |
shimniok | 0:826c6171fc1b | 117 | { |
shimniok | 0:826c6171fc1b | 118 | for (int i=0; i < n; i++) { |
shimniok | 0:826c6171fc1b | 119 | for (int j=0; j < m; j++) { |
shimniok | 0:826c6171fc1b | 120 | C[j*n+i] = A[i*m+j]; |
shimniok | 0:826c6171fc1b | 121 | } |
shimniok | 0:826c6171fc1b | 122 | } |
shimniok | 0:826c6171fc1b | 123 | } |
shimniok | 0:826c6171fc1b | 124 | |
shimniok | 0:826c6171fc1b | 125 | #define fabs(x) (((x) < 0) ? -x : x) |
shimniok | 0:826c6171fc1b | 126 | |
shimniok | 0:826c6171fc1b | 127 | // grabbed from MatrixMath library for Arduino |
shimniok | 0:826c6171fc1b | 128 | // http://arduino.cc/playground/Code/MatrixMath |
shimniok | 0:826c6171fc1b | 129 | //Matrix Inversion Routine |
shimniok | 0:826c6171fc1b | 130 | // * This function inverts a matrix based on the Gauss Jordan method. |
shimniok | 0:826c6171fc1b | 131 | // * Specifically, it uses partial pivoting to improve numeric stability. |
shimniok | 0:826c6171fc1b | 132 | // * The algorithm is drawn from those presented in |
shimniok | 0:826c6171fc1b | 133 | // NUMERICAL RECIPES: The Art of Scientific Computing. |
shimniok | 0:826c6171fc1b | 134 | // * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED |
shimniok | 0:826c6171fc1b | 135 | void Matrix_Inverse(int n, float *A) |
shimniok | 0:826c6171fc1b | 136 | { |
shimniok | 0:826c6171fc1b | 137 | // A = input matrix AND result matrix |
shimniok | 0:826c6171fc1b | 138 | // n = number of rows = number of columns in A (n x n) |
shimniok | 0:826c6171fc1b | 139 | int pivrow=0; // keeps track of current pivot row |
shimniok | 0:826c6171fc1b | 140 | int k,i,j; // k: overall index along diagonal; i: row index; j: col index |
shimniok | 0:826c6171fc1b | 141 | int pivrows[n]; // keeps track of rows swaps to undo at end |
shimniok | 0:826c6171fc1b | 142 | float tmp; // used for finding max value and making column swaps |
shimniok | 0:826c6171fc1b | 143 | |
shimniok | 0:826c6171fc1b | 144 | for (k = 0; k < n; k++) { |
shimniok | 0:826c6171fc1b | 145 | // find pivot row, the row with biggest entry in current column |
shimniok | 0:826c6171fc1b | 146 | tmp = 0; |
shimniok | 0:826c6171fc1b | 147 | for (i = k; i < n; i++) { |
shimniok | 0:826c6171fc1b | 148 | if (fabs(A[i*n+k]) >= tmp) { // 'Avoid using other functions inside abs()?' |
shimniok | 0:826c6171fc1b | 149 | tmp = fabs(A[i*n+k]); |
shimniok | 0:826c6171fc1b | 150 | pivrow = i; |
shimniok | 0:826c6171fc1b | 151 | } |
shimniok | 0:826c6171fc1b | 152 | } |
shimniok | 0:826c6171fc1b | 153 | |
shimniok | 0:826c6171fc1b | 154 | // check for singular matrix |
shimniok | 0:826c6171fc1b | 155 | if (A[pivrow*n+k] == 0.0f) { |
shimniok | 0:826c6171fc1b | 156 | matrix_error |= SINGULAR_MATRIX; |
shimniok | 0:826c6171fc1b | 157 | //fprintf(stdout, "Inversion failed due to singular matrix"); |
shimniok | 0:826c6171fc1b | 158 | return; |
shimniok | 0:826c6171fc1b | 159 | } |
shimniok | 0:826c6171fc1b | 160 | |
shimniok | 0:826c6171fc1b | 161 | // Execute pivot (row swap) if needed |
shimniok | 0:826c6171fc1b | 162 | if (pivrow != k) { |
shimniok | 0:826c6171fc1b | 163 | // swap row k with pivrow |
shimniok | 0:826c6171fc1b | 164 | for (j = 0; j < n; j++) { |
shimniok | 0:826c6171fc1b | 165 | tmp = A[k*n+j]; |
shimniok | 0:826c6171fc1b | 166 | A[k*n+j] = A[pivrow*n+j]; |
shimniok | 0:826c6171fc1b | 167 | A[pivrow*n+j] = tmp; |
shimniok | 0:826c6171fc1b | 168 | } |
shimniok | 0:826c6171fc1b | 169 | } |
shimniok | 0:826c6171fc1b | 170 | pivrows[k] = pivrow; // record row swap (even if no swap happened) |
shimniok | 0:826c6171fc1b | 171 | |
shimniok | 0:826c6171fc1b | 172 | tmp = 1.0f/A[k*n+k]; // invert pivot element |
shimniok | 0:826c6171fc1b | 173 | A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix |
shimniok | 0:826c6171fc1b | 174 | |
shimniok | 0:826c6171fc1b | 175 | // Perform row reduction (divide every element by pivot) |
shimniok | 0:826c6171fc1b | 176 | for (j = 0; j < n; j++) { |
shimniok | 0:826c6171fc1b | 177 | A[k*n+j] = A[k*n+j]*tmp; |
shimniok | 0:826c6171fc1b | 178 | } |
shimniok | 0:826c6171fc1b | 179 | |
shimniok | 0:826c6171fc1b | 180 | // Now eliminate all other entries in this column |
shimniok | 0:826c6171fc1b | 181 | for (i = 0; i < n; i++) { |
shimniok | 0:826c6171fc1b | 182 | if (i != k) { |
shimniok | 0:826c6171fc1b | 183 | tmp = A[i*n+k]; |
shimniok | 0:826c6171fc1b | 184 | A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat |
shimniok | 0:826c6171fc1b | 185 | for (j = 0; j < n; j++) { |
shimniok | 0:826c6171fc1b | 186 | A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp; |
shimniok | 0:826c6171fc1b | 187 | } |
shimniok | 0:826c6171fc1b | 188 | } |
shimniok | 0:826c6171fc1b | 189 | } |
shimniok | 0:826c6171fc1b | 190 | } |
shimniok | 0:826c6171fc1b | 191 | |
shimniok | 0:826c6171fc1b | 192 | // Done, now need to undo pivot row swaps by doing column swaps in reverse order |
shimniok | 0:826c6171fc1b | 193 | for (k = n-1; k >= 0; k--) { |
shimniok | 0:826c6171fc1b | 194 | if (pivrows[k] != k) { |
shimniok | 0:826c6171fc1b | 195 | for (i = 0; i < n; i++) { |
shimniok | 0:826c6171fc1b | 196 | tmp = A[i*n+k]; |
shimniok | 0:826c6171fc1b | 197 | A[i*n+k] = A[i*n+pivrows[k]]; |
shimniok | 0:826c6171fc1b | 198 | A[i*n+pivrows[k]] = tmp; |
shimniok | 0:826c6171fc1b | 199 | } |
shimniok | 0:826c6171fc1b | 200 | } |
shimniok | 0:826c6171fc1b | 201 | } |
shimniok | 0:826c6171fc1b | 202 | return; |
shimniok | 0:826c6171fc1b | 203 | } |
shimniok | 0:826c6171fc1b | 204 | |
shimniok | 0:826c6171fc1b | 205 | |
shimniok | 0:826c6171fc1b | 206 | void Matrix_Copy(int n, int m, float *C, float *A) |
shimniok | 0:826c6171fc1b | 207 | { |
shimniok | 0:826c6171fc1b | 208 | for (int i=0; i < n*m; i++) |
shimniok | 0:826c6171fc1b | 209 | C[i] = A[i]; |
shimniok | 0:826c6171fc1b | 210 | } |
shimniok | 0:826c6171fc1b | 211 | |
shimniok | 0:826c6171fc1b | 212 | void Matrix_print(int n, int m, float *A, const char *name) |
shimniok | 0:826c6171fc1b | 213 | { |
shimniok | 0:826c6171fc1b | 214 | fprintf(stdout, "%s=[", name); |
shimniok | 0:826c6171fc1b | 215 | for (int i=0; i < n; i++) { |
shimniok | 0:826c6171fc1b | 216 | for (int j=0; j < m; j++) { |
shimniok | 0:826c6171fc1b | 217 | fprintf(stdout, "%5.5f", A[i*m+j]); |
shimniok | 0:826c6171fc1b | 218 | if (j < m-1) fprintf(stdout, ", "); |
shimniok | 0:826c6171fc1b | 219 | } |
shimniok | 0:826c6171fc1b | 220 | if (i < n-1) fprintf(stdout, "; "); |
shimniok | 0:826c6171fc1b | 221 | } |
shimniok | 0:826c6171fc1b | 222 | fprintf(stdout, "]\n"); |
shimniok | 0:826c6171fc1b | 223 | } |
shimniok | 0:826c6171fc1b | 224 | |
shimniok | 0:826c6171fc1b | 225 | |
shimniok | 0:826c6171fc1b | 226 | void Vector_Print(float A[3], const char *name) |
shimniok | 0:826c6171fc1b | 227 | { |
shimniok | 0:826c6171fc1b | 228 | fprintf(stdout, "%s=[ ", name); |
shimniok | 0:826c6171fc1b | 229 | for (int i=0; i < 3; i++) |
shimniok | 0:826c6171fc1b | 230 | fprintf(stdout, "%5.5f ", A[i]); |
shimniok | 0:826c6171fc1b | 231 | fprintf(stdout, "]\n"); |
shimniok | 0:826c6171fc1b | 232 | |
shimniok | 0:826c6171fc1b | 233 | return; |
shimniok | 0:826c6171fc1b | 234 | } |
shimniok | 0:826c6171fc1b | 235 |