Oskar Weigl / Kalman

Kalman filter for Eurobot

MatrixMath/MatrixMath.cpp@0:a0285293f6a6, 2012-03-20 (annotated)

Committer:
madcowswe
Date:
Tue Mar 20 12:43:16 2012 +0000
Revision:
0:a0285293f6a6
Algo done, vars not done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:a0285293f6a6 1 /**
madcowswe 0:a0285293f6a6 2 * @brief Still under work version 0.2
madcowswe 0:a0285293f6a6 3 * @file MatrixMath.cpp
madcowswe 0:a0285293f6a6 4 * @author Erneseto Palacios
madcowswe 0:a0285293f6a6 5 *
madcowswe 0:a0285293f6a6 6 * Develop Under GPL v3.0 License
madcowswe 0:a0285293f6a6 7 * http://www.gnu.org/licenses/gpl-3.0.html
madcowswe 0:a0285293f6a6 8 */
madcowswe 0:a0285293f6a6 9
madcowswe 0:a0285293f6a6 10 #include "mbed.h"
madcowswe 0:a0285293f6a6 11 #include "MatrixMath.h"
madcowswe 0:a0285293f6a6 12
madcowswe 0:a0285293f6a6 13 ///Transpose matrix
madcowswe 0:a0285293f6a6 14 Matrix MatrixMath::Transpose(const Matrix& Mat)
madcowswe 0:a0285293f6a6 15 {
madcowswe 0:a0285293f6a6 16 Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
madcowswe 0:a0285293f6a6 17
madcowswe 0:a0285293f6a6 18 for( int i = 0; i < result._nRows; i++ )
madcowswe 0:a0285293f6a6 19 for( int j = 0; j < result._nCols; j++ )
madcowswe 0:a0285293f6a6 20 result._matrix[i][j] = Mat._matrix[j][i];
madcowswe 0:a0285293f6a6 21
madcowswe 0:a0285293f6a6 22 return result;
madcowswe 0:a0285293f6a6 23 }
madcowswe 0:a0285293f6a6 24
madcowswe 0:a0285293f6a6 25 Matrix MatrixMath::Inv(const Matrix& Mat)
madcowswe 0:a0285293f6a6 26 {
madcowswe 0:a0285293f6a6 27 if( Mat._nRows == Mat._nCols )
madcowswe 0:a0285293f6a6 28 {
madcowswe 0:a0285293f6a6 29 if( Mat._nRows == 2 ) // 2x2 Matrices
madcowswe 0:a0285293f6a6 30 {
madcowswe 0:a0285293f6a6 31 float det = MatrixMath::det( Mat );
madcowswe 0:a0285293f6a6 32 if( det != 0 )
madcowswe 0:a0285293f6a6 33 {
madcowswe 0:a0285293f6a6 34 Matrix Inv(2,2);
madcowswe 0:a0285293f6a6 35 Inv._matrix[0][0] = Mat._matrix[1][1];
madcowswe 0:a0285293f6a6 36 Inv._matrix[1][0] = -Mat._matrix[1][0];
madcowswe 0:a0285293f6a6 37 Inv._matrix[0][1] = -Mat._matrix[0][1];
madcowswe 0:a0285293f6a6 38 Inv._matrix[1][1] = Mat._matrix[0][0] ;
madcowswe 0:a0285293f6a6 39
madcowswe 0:a0285293f6a6 40 Inv *= 1/det;
madcowswe 0:a0285293f6a6 41
madcowswe 0:a0285293f6a6 42 return Inv;
madcowswe 0:a0285293f6a6 43
madcowswe 0:a0285293f6a6 44 }else{
madcowswe 0:a0285293f6a6 45 printf( "\n\nWANRING: same matrix returned");
madcowswe 0:a0285293f6a6 46 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
madcowswe 0:a0285293f6a6 47 // Mat.print();
madcowswe 0:a0285293f6a6 48 printf( "\n _____________\n" );
madcowswe 0:a0285293f6a6 49
madcowswe 0:a0285293f6a6 50 return Mat;
madcowswe 0:a0285293f6a6 51 }
madcowswe 0:a0285293f6a6 52
madcowswe 0:a0285293f6a6 53 }else{ // nxn Matrices
madcowswe 0:a0285293f6a6 54
madcowswe 0:a0285293f6a6 55 float det = MatrixMath::det( Mat );
madcowswe 0:a0285293f6a6 56 if( det!= 0 )
madcowswe 0:a0285293f6a6 57 {
madcowswe 0:a0285293f6a6 58 Matrix Inv( Mat ); //
madcowswe 0:a0285293f6a6 59 Matrix SubMat;
madcowswe 0:a0285293f6a6 60
madcowswe 0:a0285293f6a6 61 // Matrix of Co-factors
madcowswe 0:a0285293f6a6 62 for( int i = 0; i < Mat._nRows; i++ )
madcowswe 0:a0285293f6a6 63 for( int j = 0; j < Mat._nCols; j++ )
madcowswe 0:a0285293f6a6 64 {
madcowswe 0:a0285293f6a6 65 SubMat = Mat ;
madcowswe 0:a0285293f6a6 66
madcowswe 0:a0285293f6a6 67 Matrix::DeleteRow( SubMat, i+1 );
madcowswe 0:a0285293f6a6 68 Matrix::DeleteCol( SubMat, j+1 );
madcowswe 0:a0285293f6a6 69
madcowswe 0:a0285293f6a6 70 if( (i+j)%2 == 0 )
madcowswe 0:a0285293f6a6 71 Inv._matrix[i][j] = MatrixMath::det( SubMat );
madcowswe 0:a0285293f6a6 72 else
madcowswe 0:a0285293f6a6 73 Inv._matrix[i][j] = -MatrixMath::det( SubMat );
madcowswe 0:a0285293f6a6 74 }
madcowswe 0:a0285293f6a6 75
madcowswe 0:a0285293f6a6 76 // Adjugate Matrix
madcowswe 0:a0285293f6a6 77 Inv = MatrixMath::Transpose( Inv );
madcowswe 0:a0285293f6a6 78
madcowswe 0:a0285293f6a6 79 // Inverse Matrix
madcowswe 0:a0285293f6a6 80 Inv = 1/det * Inv;
madcowswe 0:a0285293f6a6 81
madcowswe 0:a0285293f6a6 82 return Inv;
madcowswe 0:a0285293f6a6 83
madcowswe 0:a0285293f6a6 84 }else{
madcowswe 0:a0285293f6a6 85 printf( "\n\nWANRING: same matrix returned");
madcowswe 0:a0285293f6a6 86 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
madcowswe 0:a0285293f6a6 87 // Mat.print();
madcowswe 0:a0285293f6a6 88 printf( "\n _____________\n" );
madcowswe 0:a0285293f6a6 89
madcowswe 0:a0285293f6a6 90 return Mat;
madcowswe 0:a0285293f6a6 91 }
madcowswe 0:a0285293f6a6 92
madcowswe 0:a0285293f6a6 93 }
madcowswe 0:a0285293f6a6 94
madcowswe 0:a0285293f6a6 95 }else{
madcowswe 0:a0285293f6a6 96 printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
madcowswe 0:a0285293f6a6 97 }
madcowswe 0:a0285293f6a6 98 }
madcowswe 0:a0285293f6a6 99
madcowswe 0:a0285293f6a6 100 float MatrixMath::det(const Matrix& Mat)
madcowswe 0:a0285293f6a6 101 {
madcowswe 0:a0285293f6a6 102 if( Mat._nRows == Mat._nCols )
madcowswe 0:a0285293f6a6 103 {
madcowswe 0:a0285293f6a6 104
madcowswe 0:a0285293f6a6 105 if( Mat._nRows == 2 ) // 2x2 Matrix
madcowswe 0:a0285293f6a6 106 {
madcowswe 0:a0285293f6a6 107 float det;
madcowswe 0:a0285293f6a6 108 det = Mat._matrix[0][0] * Mat._matrix[1][1] -
madcowswe 0:a0285293f6a6 109 Mat._matrix[1][0] * Mat._matrix[0][1];
madcowswe 0:a0285293f6a6 110 return det;
madcowswe 0:a0285293f6a6 111 }
madcowswe 0:a0285293f6a6 112 else if( Mat._nRows == 3 ) // 3x3 Matrix
madcowswe 0:a0285293f6a6 113 {
madcowswe 0:a0285293f6a6 114 float det;
madcowswe 0:a0285293f6a6 115 MatrixMath dummy;
madcowswe 0:a0285293f6a6 116
madcowswe 0:a0285293f6a6 117 det = dummy.Det3x3( Mat );
madcowswe 0:a0285293f6a6 118 return det;
madcowswe 0:a0285293f6a6 119
madcowswe 0:a0285293f6a6 120 } else {
madcowswe 0:a0285293f6a6 121
madcowswe 0:a0285293f6a6 122 float part1= 0;
madcowswe 0:a0285293f6a6 123 float part2= 0;
madcowswe 0:a0285293f6a6 124
madcowswe 0:a0285293f6a6 125 //Find +/- on First Row
madcowswe 0:a0285293f6a6 126 for( int i = 0; i < Mat._nCols; i++)
madcowswe 0:a0285293f6a6 127 {
madcowswe 0:a0285293f6a6 128 Matrix reduced( Mat ); // Copy Original Matrix
madcowswe 0:a0285293f6a6 129 Matrix::DeleteRow( reduced, 1); // Delete First Row
madcowswe 0:a0285293f6a6 130
madcowswe 0:a0285293f6a6 131 if( i%2 == 0 ) //Odd Rows
madcowswe 0:a0285293f6a6 132 {
madcowswe 0:a0285293f6a6 133
madcowswe 0:a0285293f6a6 134 Matrix::DeleteCol( reduced, i+1);
madcowswe 0:a0285293f6a6 135 part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
madcowswe 0:a0285293f6a6 136 }
madcowswe 0:a0285293f6a6 137 else // Even Rows
madcowswe 0:a0285293f6a6 138 {
madcowswe 0:a0285293f6a6 139 Matrix::DeleteCol( reduced, i+1);
madcowswe 0:a0285293f6a6 140 part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
madcowswe 0:a0285293f6a6 141 }
madcowswe 0:a0285293f6a6 142 }
madcowswe 0:a0285293f6a6 143 return part1 - part2; //
madcowswe 0:a0285293f6a6 144 }
madcowswe 0:a0285293f6a6 145
madcowswe 0:a0285293f6a6 146 }else{
madcowswe 0:a0285293f6a6 147 printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
madcowswe 0:a0285293f6a6 148 }
madcowswe 0:a0285293f6a6 149 }
madcowswe 0:a0285293f6a6 150
madcowswe 0:a0285293f6a6 151
madcowswe 0:a0285293f6a6 152 /************************************/
madcowswe 0:a0285293f6a6 153
madcowswe 0:a0285293f6a6 154 //Private Functions
madcowswe 0:a0285293f6a6 155
madcowswe 0:a0285293f6a6 156 /**@brief
madcowswe 0:a0285293f6a6 157 * Expands the Matrix adding first and second column to the Matrix then
madcowswe 0:a0285293f6a6 158 * performs the Algorithm.
madcowswe 0:a0285293f6a6 159 * @param Mat
madcowswe 0:a0285293f6a6 160 * @return Determinant
madcowswe 0:a0285293f6a6 161 */
madcowswe 0:a0285293f6a6 162 float MatrixMath::Det3x3(const Matrix& Mat)
madcowswe 0:a0285293f6a6 163 {
madcowswe 0:a0285293f6a6 164 Matrix D( Mat ); //Copy Initial matrix
madcowswe 0:a0285293f6a6 165
madcowswe 0:a0285293f6a6 166 Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
madcowswe 0:a0285293f6a6 167 Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
madcowswe 0:a0285293f6a6 168
madcowswe 0:a0285293f6a6 169 float det = 0;
madcowswe 0:a0285293f6a6 170 for( int i = 0; i < 3; i++ )
madcowswe 0:a0285293f6a6 171 det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
madcowswe 0:a0285293f6a6 172 - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
madcowswe 0:a0285293f6a6 173
madcowswe 0:a0285293f6a6 174 return det;
madcowswe 0:a0285293f6a6 175 }