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@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?

UserRevisionLine numberNew 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 }