Kalman filter for Eurobot
Diff: MatrixMath/MatrixMath.cpp
- Revision:
- 0:a0285293f6a6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath/MatrixMath.cpp Tue Mar 20 12:43:16 2012 +0000 @@ -0,0 +1,175 @@ +/** + * @brief Still under work version 0.2 + * @file MatrixMath.cpp + * @author Erneseto Palacios + * + * Develop Under GPL v3.0 License + * http://www.gnu.org/licenses/gpl-3.0.html + */ + +#include "mbed.h" +#include "MatrixMath.h" + +///Transpose matrix +Matrix MatrixMath::Transpose(const Matrix& Mat) +{ + Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix + + for( int i = 0; i < result._nRows; i++ ) + for( int j = 0; j < result._nCols; j++ ) + result._matrix[i][j] = Mat._matrix[j][i]; + + return result; +} + +Matrix MatrixMath::Inv(const Matrix& Mat) +{ + if( Mat._nRows == Mat._nCols ) + { + if( Mat._nRows == 2 ) // 2x2 Matrices + { + float det = MatrixMath::det( Mat ); + if( det != 0 ) + { + Matrix Inv(2,2); + Inv._matrix[0][0] = Mat._matrix[1][1]; + Inv._matrix[1][0] = -Mat._matrix[1][0]; + Inv._matrix[0][1] = -Mat._matrix[0][1]; + Inv._matrix[1][1] = Mat._matrix[0][0] ; + + Inv *= 1/det; + + return Inv; + + }else{ + printf( "\n\nWANRING: same matrix returned"); + printf( "\nSingular Matrix, cannot perform Invert @matrix " ); +// Mat.print(); + printf( "\n _____________\n" ); + + return Mat; + } + + }else{ // nxn Matrices + + float det = MatrixMath::det( Mat ); + if( det!= 0 ) + { + Matrix Inv( Mat ); // + Matrix SubMat; + + // Matrix of Co-factors + for( int i = 0; i < Mat._nRows; i++ ) + for( int j = 0; j < Mat._nCols; j++ ) + { + SubMat = Mat ; + + Matrix::DeleteRow( SubMat, i+1 ); + Matrix::DeleteCol( SubMat, j+1 ); + + if( (i+j)%2 == 0 ) + Inv._matrix[i][j] = MatrixMath::det( SubMat ); + else + Inv._matrix[i][j] = -MatrixMath::det( SubMat ); + } + + // Adjugate Matrix + Inv = MatrixMath::Transpose( Inv ); + + // Inverse Matrix + Inv = 1/det * Inv; + + return Inv; + + }else{ + printf( "\n\nWANRING: same matrix returned"); + printf( "\nSingular Matrix, cannot perform Invert @matrix " ); + // Mat.print(); + printf( "\n _____________\n" ); + + return Mat; + } + + } + + }else{ + printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " ); + } +} + +float MatrixMath::det(const Matrix& Mat) +{ + if( Mat._nRows == Mat._nCols ) + { + + if( Mat._nRows == 2 ) // 2x2 Matrix + { + float det; + det = Mat._matrix[0][0] * Mat._matrix[1][1] - + Mat._matrix[1][0] * Mat._matrix[0][1]; + return det; + } + else if( Mat._nRows == 3 ) // 3x3 Matrix + { + float det; + MatrixMath dummy; + + det = dummy.Det3x3( Mat ); + return det; + + } else { + + float part1= 0; + float part2= 0; + + //Find +/- on First Row + for( int i = 0; i < Mat._nCols; i++) + { + Matrix reduced( Mat ); // Copy Original Matrix + Matrix::DeleteRow( reduced, 1); // Delete First Row + + if( i%2 == 0 ) //Odd Rows + { + + Matrix::DeleteCol( reduced, i+1); + part1 += Mat._matrix[0][i] * MatrixMath::det(reduced); + } + else // Even Rows + { + Matrix::DeleteCol( reduced, i+1); + part2 += Mat._matrix[0][i] * MatrixMath::det(reduced); + } + } + return part1 - part2; // + } + + }else{ + printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det"); + } +} + + +/************************************/ + +//Private Functions + +/**@brief + * Expands the Matrix adding first and second column to the Matrix then + * performs the Algorithm. + * @param Mat + * @return Determinant + */ +float MatrixMath::Det3x3(const Matrix& Mat) +{ + Matrix D( Mat ); //Copy Initial matrix + + Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column + Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column + + float det = 0; + for( int i = 0; i < 3; i++ ) + det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i] + - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i]; + + return det; +} \ No newline at end of file