Kalman filter for Eurobot

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Matrix.cpp Source File

Matrix.cpp

Go to the documentation of this file.
00001 /**
00002  * @brief  Source Code for the Matrix Class.
00003  * @file   Matrix.cpp
00004  * @author Ernesto Palacios
00005  *
00006  * Created on September 2011.
00007  *
00008  * Develop Under  GPL v3.0 License
00009  * http://www.gnu.org/licenses/gpl-3.0.html
00010  *
00011  */
00012 
00013 #include "mbed.h"
00014 #include "Matrix.h"
00015 
00016 /// Rows by Cols Matrix Constructor
00017 Matrix::Matrix(int Rows, int Cols): _nRows(Rows), _nCols(Cols)
00018 {
00019     _matrix.resize(_nRows);
00020     for( int i = 0; i < _nRows; i++ )
00021         _matrix[i].resize(_nCols);
00022 
00023     _pRow = 0;
00024     _pCol = 0;
00025 
00026     this->Clear();  //Make all elements zero by default.
00027 }
00028 
00029 
00030 /// Copies one matrix into a new one
00031 Matrix::Matrix(const Matrix& base)
00032 {
00033     _nCols = base._nCols;
00034     _nRows = base._nRows;
00035 
00036     _pRow  = base._pRow;
00037     _pCol  = base._pCol;
00038 
00039     _matrix.resize(_nRows);
00040     for( int i = 0; i < _nRows; i++ )
00041         _matrix[i].resize(_nCols);
00042 
00043     for( int i = 0; i < _nRows; i++ )
00044         for( int j = 0; j < _nCols; j++ )
00045             _matrix[i][j] = base._matrix[i][j];
00046 }
00047 
00048 
00049 /// Default Constructor
00050 Matrix::Matrix()
00051 {
00052     _nCols = 0;
00053     _nRows = 0;
00054 
00055     _pRow = 0;
00056     _pCol = 0;
00057 
00058 }
00059 
00060 /***********************************************************************/
00061 
00062 /// Returns true if matrix is full of zeros
00063 bool Matrix::isZero() const
00064 {
00065     bool zero = false;
00066     for( int i = 0; i < this->_nRows; i++ )
00067         for( int j = 0; j < this->_nCols; j++ )
00068             if( _matrix[i][j] != 0 )
00069                 zero = zero || true;
00070     return !zero;
00071 }
00072 
00073 
00074 /// Returns true if Matrix is Single Row ot Single Column.
00075 bool Matrix::isVector() const
00076 {
00077     if( _nRows == 1 || _nCols == 1 )
00078         return true;
00079     else
00080         return false;
00081 }
00082 
00083 /*************************************************************************/
00084 
00085 /// Returns all elements in Matrix as a single Row vector.
00086 const Matrix Matrix::ToPackedVector( const Matrix& Mat )
00087 {
00088 
00089     Matrix Crushed( 1, Mat._nRows * Mat._nCols );
00090 
00091     int cont = 0;
00092 
00093     for( int i = 0; i < Mat._nRows; i++ )
00094         for( int j = 0; j < Mat._nCols; j++ )
00095         {
00096             Crushed._matrix[0][cont] = Mat._matrix[i][j];
00097             cont++;
00098         }
00099 
00100     Crushed._pRow = Crushed._nRows;
00101     Crushed._pCol = Crushed._nCols;
00102 
00103     return Crushed;
00104 }
00105 
00106 
00107 
00108 /// To add (Insert) a Single Row to a Matrix.
00109 void Matrix::AddRow(Matrix& Mat, int index)
00110 {
00111     --index;
00112 
00113     if( index > Mat._nRows + 1)
00114     {
00115         printf("\n\nERROR:\nRow out of Limits @ AddRow()\n");
00116 
00117     }else{
00118 
00119        Mat._nRows++;
00120        Mat._matrix.resize( Mat._nRows );
00121 
00122        Mat._matrix[ Mat._nRows - 1 ].resize( Mat._nCols );
00123 
00124        for( int i = Mat._nRows - 1; i > index; i-- )
00125            for( int j = 0; j < Mat._nCols; j++ )
00126                Mat._matrix[i][j] = Mat._matrix[i - 1][j];
00127 
00128        for( int j = 0; j < Mat._nCols; j++ )
00129            Mat._matrix[index][j] = 0.0;
00130     }
00131 }
00132 
00133 
00134 void Matrix::AddRow(Matrix& Receip, const Matrix& Row, int index)
00135 {
00136     Matrix::AddRow( Receip, index );  //Make Room
00137 
00138     --index;
00139     for( int i = 0; i < Receip._nCols; i++ )
00140         Receip._matrix[index][i] = Row._matrix[0][i];   //Copy Data.
00141 
00142 }
00143 
00144 
00145 /// To add (Insert) a single Column to a Matrix
00146 void Matrix::AddCol( Matrix& Mat, int index )
00147 {
00148     --index;
00149 
00150     if( index > Mat._nCols + 1 )
00151     {
00152         printf("\n\nERROR:\nRow out of Limits on AddCol()\n");
00153 
00154     }else{
00155 
00156 
00157             Mat._nCols++;
00158             for( int i = 0; i < Mat._nRows; i++ )
00159                 Mat._matrix[i].resize( Mat._nCols );
00160 
00161             for( int i = 0; i < Mat._nRows; i++ )
00162                 for( int j = Mat._nCols; j > index; j-- )
00163                     Mat._matrix[i][j] = Mat._matrix[i][j - 1];
00164 
00165             for( int i = 0; i < Mat._nRows; i++ )
00166                 Mat._matrix[i][index] = 0.0;
00167 
00168     }
00169 }
00170 
00171 
00172 void Matrix::AddCol(Matrix& Receip, const Matrix& Row, int index)
00173 {
00174     Matrix::AddCol( Receip, index ); // Make Rom
00175 
00176     --index;
00177     for( int i = 0; i < Receip._nRows; i++ )
00178         Receip._matrix[i][index] = Row._matrix[i][0];   //Copy Data.
00179 }
00180 
00181 
00182 /// Delete a Single Column From Matrix.
00183 void Matrix::DeleteCol( Matrix& Mat, int Col)
00184 {
00185     --Col; // Because of Column zero.
00186 
00187     if( Col > Mat._nCols )
00188     {
00189         printf("\n\nERROR:\nColumn out of Limits @ DeleteCol()\n");
00190 
00191     }else{
00192 
00193         for( int i = 0; i < Mat._nRows; i++ )
00194             for( int j = Col; j < Mat._nCols; j++ )
00195                 Mat._matrix[i][j] = Mat._matrix[i][j+1];
00196 
00197         // If adressing last element of Column,
00198         // wich no longer exists
00199         if( Mat._pCol == Mat._nCols )
00200             Mat._pCol--;
00201 
00202         // Decrease one column
00203         Mat._nCols--;
00204 
00205         //Erase last Column
00206         for( int i = 0; i < Mat._nRows; i++ )
00207             Mat._matrix[i].reserve(Mat._nCols);
00208 
00209     }
00210 }
00211 
00212 
00213 /// Delete a Single Row form Matrix
00214 void Matrix::DeleteRow(Matrix& Mat, int Row)
00215 {
00216     --Row;
00217 
00218     if( Row > Mat._nRows )
00219     {
00220         printf("\n\nERROR:\nColumn out of Limits @ DeleteCol()\n");
00221 
00222     }else{
00223 
00224         for( int i = Row; i < Mat._nRows - 1; i++ )
00225 
00226             for( int j = 0; j < Mat._nCols; j++ )
00227                 Mat._matrix[i][j] = Mat._matrix[i+1][j];
00228         Mat._nRows--;
00229         Mat._matrix.resize(Mat._nRows);
00230     }
00231 }
00232 
00233 /*****************************************************************************************/
00234 
00235 /// Extracts a single row form calling matrix and saves it to another matrix.
00236 const Matrix Matrix::ExportRow( const Matrix& Mat, int row )
00237 {
00238     --row;
00239 
00240     if( row > Mat._nRows )
00241     {
00242         printf( "\n\nERROR:\nRow out of dimmensions @ GetRow\n"
00243                 "Nothing Done.\n\n" );
00244 
00245     }else{
00246 
00247         Matrix SingleRow( 1 , Mat._nCols );
00248         SingleRow.Clear();
00249 
00250         for( int j = 0; j < Mat._nCols; j++ )
00251         SingleRow._matrix[0][j] = Mat._matrix[row][j];
00252 
00253         SingleRow._pCol = SingleRow._nCols;
00254         SingleRow._pRow = 0;
00255 
00256         return SingleRow;
00257     }
00258 }
00259 
00260 
00261 /// Extracts a single column form calling matrix and saves it to another matrix.
00262 const Matrix Matrix::ExportCol( const Matrix& Mat, int col )
00263 {
00264     --col;
00265 
00266     if( col > Mat._nCols )
00267     {
00268         printf( "\n\nERROR:\nColumn out of dimmensions.\n"
00269                 "Nothing Done.\n\n" );
00270     }else{
00271 
00272         Matrix SingleCol( Mat._nRows, 1 );
00273         for(int i = 0; i < Mat._nRows; i++ )
00274             SingleCol._matrix[i][0] = Mat._matrix[i][col];
00275 
00276         SingleCol._pCol = 0;
00277         SingleCol._pRow = SingleCol._nRows;
00278 
00279         return SingleCol;
00280     }
00281 }
00282 
00283 
00284 /// Makes matrix Bigger!
00285 void Matrix::Resize( int Rows, int Cols )
00286 {
00287     _nRows = Rows;  //Decreases one because internally
00288     _nCols = Cols; // Index starts at zero.
00289 
00290     _matrix.resize( _nRows );
00291 
00292     for( int i = 0; i< _nRows ; i++ )
00293         _matrix[i].resize(_nCols);
00294 
00295     _pRow = 0; // If matrix is resized the <<
00296     _pCol = 0; // operator overwrites everything!
00297 }
00298 
00299 
00300 /// Ask user for elemnts in Matrix
00301 void Matrix::FillMatrix()
00302 {
00303     for(int i = 0; i < _nRows; i++)
00304     {
00305         for(int j = 0; j < _nCols; j++)
00306         {
00307             printf( "Position [%u][%u]: ", i, j );
00308             float numero;
00309             scanf( "%f", &numero );
00310             printf("%.3f ", numero);
00311             this->_matrix[i][j] = numero;
00312         }
00313         printf("\n");
00314     }
00315     printf("\n");
00316 
00317     _pRow = _nRows;
00318     _pCol = _nCols;
00319 }
00320 
00321 
00322 /// Prints out Matrix.
00323 void Matrix::print() const
00324 {
00325     for( int i = 0; i < _nRows; i++ )
00326     {
00327         for( int j = 0; j < _nCols; j++ )
00328         {
00329             printf( "%.3f, ",_matrix[i][j] );
00330 
00331         }
00332         printf( "\n" );
00333     }
00334 }
00335 
00336 
00337 /// Fills matrix with zeros.
00338 void Matrix::Clear()
00339 {
00340     for( int i = 0; i < _nRows; i++ )
00341        for( int j = 0; j < _nCols; j++ )
00342            _matrix[i][j] = 0;
00343 
00344     _pCol = 0;  // New data can be added
00345     _pRow = 0;
00346 }
00347 
00348 /********************************************************************************/
00349 
00350 
00351 /// Inserts a Single element in a desired Position( Index starts at [1][1] );
00352 void Matrix::add(int Row, int Col, float number)
00353 {
00354     --Col; --Row;
00355 
00356     if( Row > _nRows || Col > _nCols )
00357     {
00358         printf("\n\nERROR:\nOut of limits of Matrix @ mat.Add()");
00359 
00360     }else{
00361         _matrix[Row][Col] = number;
00362     }
00363 }
00364 
00365 
00366 /// Adds all elements in matrix and returns the answer.
00367 float Matrix::sum() const
00368 {
00369     float total = 0;
00370 
00371     for( int i = 0; i < _nRows; i++ )
00372         for( int j = 0; j < _nCols; j++ )
00373             total += _matrix[i][j];
00374     return total;
00375 }
00376 
00377 
00378 /// Returns the specified element. Index Starts at [1][1].
00379 float Matrix::getNumber( int Row, int Col ) const
00380 { return this->_matrix[Row -1][Col - 1]; }
00381 
00382 /// Returns the number of Rows in Matrix.
00383 int Matrix::getRows() const{ return this->_nRows; }
00384 
00385 
00386 /// Returns the number of Columns in Matrix.
00387 int Matrix::getCols() const{ return this->_nCols; }