Michael Shimniok / Mbed 2 deprecated DataBus

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@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
23:a34af501ea89
Working version with priorities set and update time display

Who changed what in which revision?

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