Kalman filter for Eurobot

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Operators.cpp Source File

Operators.cpp

Go to the documentation of this file.
00001 /**
00002  * @brief  Source Code for the Operator of Matrix Class.
00003  * @file   Operators.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 #include "mbed.h"
00013 #include "Matrix.h"
00014 
00015 /// Subindex in Matrix left side
00016 float& Matrix::operator ()(int row, int col)
00017 {
00018     --row; --col;
00019 
00020     if( row >= _nRows || col >= _nCols)
00021     {
00022         printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
00023     }else{
00024         return _matrix[row][col];
00025     }
00026 }
00027 
00028 /// Subindex in Matrix right side
00029 float Matrix::operator ()(int row, int col) const
00030 {
00031     --row; --col;
00032 
00033     if( row >= _nRows || col >= _nCols)
00034     {
00035         printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
00036     }else{
00037         return _matrix[row][col];
00038     }
00039 }
00040 
00041 
00042 /// Overloaded Asign Operator. Resizes Matrix
00043 Matrix& Matrix::operator = ( const Matrix& rightM )
00044 {
00045     if (this != &rightM )
00046     {
00047 
00048          _nRows = rightM._nRows;
00049          _nCols = rightM._nCols;
00050 
00051          _matrix.resize( rightM._nRows );
00052          for( int i = 0; i < rightM._nRows; i++ )
00053              _matrix [i].resize(rightM._nCols);
00054 
00055          for( int i = 0; i < _nRows; i++ )
00056              for( int j = 0; j < _nCols; j++ )
00057                  _matrix[i][j] = rightM._matrix[i][j];
00058     }
00059      return *this;
00060 
00061 }
00062 
00063 
00064 const Matrix Matrix::operator -()
00065 {
00066     Matrix result( _nRows, _nCols );
00067 
00068      for( int i = 0; i < _nRows; i++ )
00069            for( int j = 0; j < _nCols; j++ )
00070               result._matrix[i][j] = _matrix[i][j] * -1;
00071 
00072     return result;
00073 }
00074 
00075 
00076 /// Comapre element by element
00077 bool operator == ( const Matrix& leftM, const Matrix& rightM )
00078 {
00079     if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
00080     {
00081         bool equal = false;
00082 
00083         for( int i = 0; i < leftM._nRows; i++ )
00084             for( int j = 0; j < leftM._nCols; j++ )
00085                 if( leftM._matrix[i][j] != rightM._matrix[i][j] )
00086                     equal = equal || true;
00087 
00088         return !equal;
00089 
00090     }else{  return false;  }
00091 }
00092 
00093 
00094 /// Calls for '==' operator
00095 bool operator != ( const Matrix& leftM, const Matrix& rightM )
00096 {
00097     return !( leftM == rightM );
00098 }
00099 
00100 
00101 /// Matrices must be same size.
00102 /// Element by element adition.
00103 Matrix& operator +=( Matrix& leftM, const Matrix& rightM )
00104 {
00105     if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
00106     {
00107         for( int i = 0; i < leftM._nRows; i++ )
00108             for( int j = 0; j < leftM._nCols; j++ )
00109                 leftM._matrix[i][j] += rightM._matrix[i][j];
00110 
00111         return leftM;
00112 
00113     }else{ printf( "\n\nERROR:\nDiferent Dimensions @ += operator\n" );  }
00114 }
00115 
00116 
00117 /// Matrices must be same size.
00118 /// Element by element Substraction
00119 Matrix& operator -=( Matrix& leftM, const Matrix& rightM )
00120 {
00121     if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
00122     {
00123         for( int i = 0; i < leftM._nRows; i++ )
00124             for( int j = 0; j < leftM._nCols; j++ )
00125                 leftM._matrix[i][j] -= rightM._matrix[i][j];
00126 
00127         return leftM;
00128 
00129     }else{
00130         printf( "\n\nERROR:\nDiferent Dimensions @ -= operator\n" );
00131     }
00132 }
00133 
00134 
00135 Matrix& operator *= ( Matrix& leftM, const Matrix& rightM )
00136 {
00137     if( leftM._nCols == rightM._nRows )
00138     {
00139         Matrix resultM ( leftM._nRows, rightM._nCols );
00140 
00141         for( int i = 0; i < resultM._nRows; i++ )
00142             for( int j = 0; j < resultM._nCols; j++ )
00143                 for( int m = 0; m < rightM._nRows; m++ )
00144                     resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
00145 
00146         return resultM;
00147     }else{
00148         printf( "\n\nERROR:\nDiferent Dimensions @ *= operator\n" );
00149     }
00150 }
00151 
00152 
00153 Matrix& operator *= ( Matrix& leftM, float number )
00154 {
00155     for( int i = 0; i < leftM._nRows; i++ )
00156             for( int j = 0; j < leftM._nCols; j++ )
00157                 leftM._matrix[i][j] *= number;
00158 
00159     return leftM;
00160 }
00161 
00162 
00163 /*****************************************************************************/
00164 
00165 // Overload operators
00166 
00167 
00168 const Matrix operator +=( Matrix& leftM, float number )
00169 {
00170     for( int i = 0; i < leftM._nRows; i++ )
00171            for( int j = 0; j < leftM._nCols; j++ )
00172               leftM._matrix[i][j] += number;
00173     return leftM;
00174 }
00175 
00176 
00177 const Matrix operator -=( Matrix& leftM, float number )
00178 {
00179     for( int i = 0; i < leftM._nRows; i++ )
00180            for( int j = 0; j < leftM._nCols; j++ )
00181               leftM._matrix[i][j] -= number;
00182     return leftM;
00183 }
00184 
00185 
00186 const Matrix operator + ( const Matrix& leftM, const Matrix& rightM)
00187 {
00188     if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
00189     {
00190         Matrix result( leftM._nRows, leftM._nCols );
00191 
00192         for( int i = 0; i < leftM._nRows; i++ )
00193            for( int j = 0; j < leftM._nCols; j++ )
00194               result._matrix[i][j] = leftM._matrix[i][j] + rightM._matrix[i][j];
00195 
00196         return result;
00197 
00198     }else{
00199         printf( "\n\nERROR\nDiferent Dimensions @ + operator \n" );
00200         //Matrix error(4);
00201         //error.Clear();
00202         //return error;
00203     }
00204 }
00205 
00206 
00207 const Matrix operator + ( const Matrix& leftM, float number )
00208 {
00209     Matrix result( leftM._nRows, leftM._nCols );
00210 
00211     for( int i = 0; i < leftM._nRows; i++ )
00212         for( int j = 0; j < leftM._nCols; j++ )
00213             result._matrix[i][j] = leftM._matrix[i][j] + number;
00214 
00215     return result;
00216 }
00217 
00218 
00219 const Matrix operator + ( float number, const Matrix& leftM )
00220 {
00221     return ( leftM + number );
00222 }
00223 
00224 
00225 const Matrix operator - ( const Matrix& leftM, const Matrix& rightM )
00226 {
00227     if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
00228     {
00229         Matrix result( leftM._nRows, leftM._nCols );
00230 
00231         for( int i = 0; i < leftM._nRows; i++ )
00232            for( int j = 0; j < leftM._nCols; j++ )
00233               result._matrix[i][j] = leftM._matrix[i][j] - rightM._matrix[i][j];
00234 
00235         return result;
00236 
00237     }else{
00238         printf( "\n\nERROR:\nDiferent Dimensions @ + operator \n" );
00239 
00240     }
00241 }
00242 
00243 
00244 const Matrix operator - ( const Matrix& leftM, float number )
00245 {
00246     Matrix result( leftM._nRows, leftM._nCols );
00247 
00248     for( int i = 0; i < leftM._nRows; i++ )
00249        for( int j = 0; j < leftM._nCols; j++ )
00250           result._matrix[i][j] = leftM._matrix[i][j] - number;
00251 
00252     return result;
00253 }
00254 
00255 
00256 const Matrix operator - ( float number, const Matrix& leftM )
00257 {
00258     return ( leftM - number );
00259 }
00260 
00261 
00262 const Matrix operator * ( const Matrix& leftM, const Matrix& rightM )
00263 {
00264     if( leftM._nCols == rightM._nRows )
00265     {
00266         Matrix resultM ( leftM._nRows, rightM._nCols );
00267         resultM.Clear();
00268 
00269         for( int i = 0; i < resultM._nRows; i++ )
00270             for( int j = 0; j < resultM._nCols; j++ )
00271                 for( int m = 0; m < rightM._nRows; m++ )
00272                     resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
00273 
00274         return resultM;
00275 
00276     } else {
00277 
00278         printf("\n\nERROR:\nDiferent Dimension matrices @ * operator");
00279     }
00280 
00281 }
00282 
00283 
00284 const Matrix operator * ( const Matrix& leftM, float number )
00285 {
00286     Matrix result( leftM._nRows, leftM._nCols );
00287 
00288     for( int i = 0; i < leftM._nRows; i++ )
00289        for( int j = 0; j < leftM._nCols; j++ )
00290           result._matrix[i][j] = leftM._matrix[i][j] * number;
00291 
00292     return result;
00293 }
00294 
00295 const Matrix operator * ( float number, const Matrix& leftM )
00296 {
00297     return ( leftM * number );
00298 }
00299 
00300 
00301 Matrix& operator << ( Matrix& leftM, float number )
00302 {
00303     if( leftM._pCol == leftM._nCols ) //end of Row
00304     {
00305         leftM._pCol = 0;
00306         leftM._pRow++;
00307     }
00308     if( leftM._pRow > leftM._nRows )
00309     {
00310         printf( "\n\nERROR:\nAssignment out of limits @ << operator" );
00311         return leftM;
00312 
00313     }else{
00314 
00315         leftM._matrix[ leftM._pRow ][ leftM._pCol ] = number;
00316         leftM._pCol++;
00317 
00318         return leftM;
00319     }
00320 }