Oskar Weigl / Kalman

Kalman filter for Eurobot

Matrix/Operators.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 Source Code for the Operator of Matrix Class.
madcowswe 0:a0285293f6a6 3 * @file Operators.cpp
madcowswe 0:a0285293f6a6 4 * @author Ernesto Palacios
madcowswe 0:a0285293f6a6 5 *
madcowswe 0:a0285293f6a6 6 * Created on September 2011.
madcowswe 0:a0285293f6a6 7 *
madcowswe 0:a0285293f6a6 8 * Develop Under GPL v3.0 License
madcowswe 0:a0285293f6a6 9 * http://www.gnu.org/licenses/gpl-3.0.html
madcowswe 0:a0285293f6a6 10 *
madcowswe 0:a0285293f6a6 11 */
madcowswe 0:a0285293f6a6 12 #include "mbed.h"
madcowswe 0:a0285293f6a6 13 #include "Matrix.h"
madcowswe 0:a0285293f6a6 14
madcowswe 0:a0285293f6a6 15 /// Subindex in Matrix left side
madcowswe 0:a0285293f6a6 16 float& Matrix::operator ()(int row, int col)
madcowswe 0:a0285293f6a6 17 {
madcowswe 0:a0285293f6a6 18 --row; --col;
madcowswe 0:a0285293f6a6 19
madcowswe 0:a0285293f6a6 20 if( row >= _nRows || col >= _nCols)
madcowswe 0:a0285293f6a6 21 {
madcowswe 0:a0285293f6a6 22 printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
madcowswe 0:a0285293f6a6 23 }else{
madcowswe 0:a0285293f6a6 24 return _matrix[row][col];
madcowswe 0:a0285293f6a6 25 }
madcowswe 0:a0285293f6a6 26 }
madcowswe 0:a0285293f6a6 27
madcowswe 0:a0285293f6a6 28 /// Subindex in Matrix right side
madcowswe 0:a0285293f6a6 29 float Matrix::operator ()(int row, int col) const
madcowswe 0:a0285293f6a6 30 {
madcowswe 0:a0285293f6a6 31 --row; --col;
madcowswe 0:a0285293f6a6 32
madcowswe 0:a0285293f6a6 33 if( row >= _nRows || col >= _nCols)
madcowswe 0:a0285293f6a6 34 {
madcowswe 0:a0285293f6a6 35 printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
madcowswe 0:a0285293f6a6 36 }else{
madcowswe 0:a0285293f6a6 37 return _matrix[row][col];
madcowswe 0:a0285293f6a6 38 }
madcowswe 0:a0285293f6a6 39 }
madcowswe 0:a0285293f6a6 40
madcowswe 0:a0285293f6a6 41
madcowswe 0:a0285293f6a6 42 /// Overloaded Asign Operator. Resizes Matrix
madcowswe 0:a0285293f6a6 43 Matrix& Matrix::operator = ( const Matrix& rightM )
madcowswe 0:a0285293f6a6 44 {
madcowswe 0:a0285293f6a6 45 if (this != &rightM )
madcowswe 0:a0285293f6a6 46 {
madcowswe 0:a0285293f6a6 47
madcowswe 0:a0285293f6a6 48 _nRows = rightM._nRows;
madcowswe 0:a0285293f6a6 49 _nCols = rightM._nCols;
madcowswe 0:a0285293f6a6 50
madcowswe 0:a0285293f6a6 51 _matrix.resize( rightM._nRows );
madcowswe 0:a0285293f6a6 52 for( int i = 0; i < rightM._nRows; i++ )
madcowswe 0:a0285293f6a6 53 _matrix [i].resize(rightM._nCols);
madcowswe 0:a0285293f6a6 54
madcowswe 0:a0285293f6a6 55 for( int i = 0; i < _nRows; i++ )
madcowswe 0:a0285293f6a6 56 for( int j = 0; j < _nCols; j++ )
madcowswe 0:a0285293f6a6 57 _matrix[i][j] = rightM._matrix[i][j];
madcowswe 0:a0285293f6a6 58 }
madcowswe 0:a0285293f6a6 59 return *this;
madcowswe 0:a0285293f6a6 60
madcowswe 0:a0285293f6a6 61 }
madcowswe 0:a0285293f6a6 62
madcowswe 0:a0285293f6a6 63
madcowswe 0:a0285293f6a6 64 const Matrix Matrix::operator -()
madcowswe 0:a0285293f6a6 65 {
madcowswe 0:a0285293f6a6 66 Matrix result( _nRows, _nCols );
madcowswe 0:a0285293f6a6 67
madcowswe 0:a0285293f6a6 68 for( int i = 0; i < _nRows; i++ )
madcowswe 0:a0285293f6a6 69 for( int j = 0; j < _nCols; j++ )
madcowswe 0:a0285293f6a6 70 result._matrix[i][j] = _matrix[i][j] * -1;
madcowswe 0:a0285293f6a6 71
madcowswe 0:a0285293f6a6 72 return result;
madcowswe 0:a0285293f6a6 73 }
madcowswe 0:a0285293f6a6 74
madcowswe 0:a0285293f6a6 75
madcowswe 0:a0285293f6a6 76 /// Comapre element by element
madcowswe 0:a0285293f6a6 77 bool operator == ( const Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 78 {
madcowswe 0:a0285293f6a6 79 if( leftM._nRows == rightM._nRows && leftM._nCols == rightM._nCols )
madcowswe 0:a0285293f6a6 80 {
madcowswe 0:a0285293f6a6 81 bool equal = false;
madcowswe 0:a0285293f6a6 82
madcowswe 0:a0285293f6a6 83 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 84 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 85 if( leftM._matrix[i][j] != rightM._matrix[i][j] )
madcowswe 0:a0285293f6a6 86 equal = equal || true;
madcowswe 0:a0285293f6a6 87
madcowswe 0:a0285293f6a6 88 return !equal;
madcowswe 0:a0285293f6a6 89
madcowswe 0:a0285293f6a6 90 }else{ return false; }
madcowswe 0:a0285293f6a6 91 }
madcowswe 0:a0285293f6a6 92
madcowswe 0:a0285293f6a6 93
madcowswe 0:a0285293f6a6 94 /// Calls for '==' operator
madcowswe 0:a0285293f6a6 95 bool operator != ( const Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 96 {
madcowswe 0:a0285293f6a6 97 return !( leftM == rightM );
madcowswe 0:a0285293f6a6 98 }
madcowswe 0:a0285293f6a6 99
madcowswe 0:a0285293f6a6 100
madcowswe 0:a0285293f6a6 101 /// Matrices must be same size.
madcowswe 0:a0285293f6a6 102 /// Element by element adition.
madcowswe 0:a0285293f6a6 103 Matrix& operator +=( Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 104 {
madcowswe 0:a0285293f6a6 105 if( leftM._nRows == rightM._nRows && leftM._nCols == rightM._nCols )
madcowswe 0:a0285293f6a6 106 {
madcowswe 0:a0285293f6a6 107 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 108 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 109 leftM._matrix[i][j] += rightM._matrix[i][j];
madcowswe 0:a0285293f6a6 110
madcowswe 0:a0285293f6a6 111 return leftM;
madcowswe 0:a0285293f6a6 112
madcowswe 0:a0285293f6a6 113 }else{ printf( "\n\nERROR:\nDiferent Dimensions @ += operator\n" ); }
madcowswe 0:a0285293f6a6 114 }
madcowswe 0:a0285293f6a6 115
madcowswe 0:a0285293f6a6 116
madcowswe 0:a0285293f6a6 117 /// Matrices must be same size.
madcowswe 0:a0285293f6a6 118 /// Element by element Substraction
madcowswe 0:a0285293f6a6 119 Matrix& operator -=( Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 120 {
madcowswe 0:a0285293f6a6 121 if( leftM._nRows == rightM._nRows && leftM._nCols == rightM._nCols )
madcowswe 0:a0285293f6a6 122 {
madcowswe 0:a0285293f6a6 123 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 124 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 125 leftM._matrix[i][j] -= rightM._matrix[i][j];
madcowswe 0:a0285293f6a6 126
madcowswe 0:a0285293f6a6 127 return leftM;
madcowswe 0:a0285293f6a6 128
madcowswe 0:a0285293f6a6 129 }else{
madcowswe 0:a0285293f6a6 130 printf( "\n\nERROR:\nDiferent Dimensions @ -= operator\n" );
madcowswe 0:a0285293f6a6 131 }
madcowswe 0:a0285293f6a6 132 }
madcowswe 0:a0285293f6a6 133
madcowswe 0:a0285293f6a6 134
madcowswe 0:a0285293f6a6 135 Matrix& operator *=( Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 136 {
madcowswe 0:a0285293f6a6 137 if( leftM._nCols == rightM._nRows )
madcowswe 0:a0285293f6a6 138 {
madcowswe 0:a0285293f6a6 139 Matrix resultM ( leftM._nRows, rightM._nCols );
madcowswe 0:a0285293f6a6 140
madcowswe 0:a0285293f6a6 141 for( int i = 0; i < resultM._nRows; i++ )
madcowswe 0:a0285293f6a6 142 for( int j = 0; j < resultM._nCols; j++ )
madcowswe 0:a0285293f6a6 143 for( int m = 0; m < rightM._nRows; m++ )
madcowswe 0:a0285293f6a6 144 resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
madcowswe 0:a0285293f6a6 145
madcowswe 0:a0285293f6a6 146 return resultM;
madcowswe 0:a0285293f6a6 147 }else{
madcowswe 0:a0285293f6a6 148 printf( "\n\nERROR:\nDiferent Dimensions @ *= operator\n" );
madcowswe 0:a0285293f6a6 149 }
madcowswe 0:a0285293f6a6 150 }
madcowswe 0:a0285293f6a6 151
madcowswe 0:a0285293f6a6 152
madcowswe 0:a0285293f6a6 153 Matrix& operator *=( Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 154 {
madcowswe 0:a0285293f6a6 155 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 156 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 157 leftM._matrix[i][j] *= number;
madcowswe 0:a0285293f6a6 158
madcowswe 0:a0285293f6a6 159 return leftM;
madcowswe 0:a0285293f6a6 160 }
madcowswe 0:a0285293f6a6 161
madcowswe 0:a0285293f6a6 162
madcowswe 0:a0285293f6a6 163 /*****************************************************************************/
madcowswe 0:a0285293f6a6 164
madcowswe 0:a0285293f6a6 165 // Overload operators
madcowswe 0:a0285293f6a6 166
madcowswe 0:a0285293f6a6 167
madcowswe 0:a0285293f6a6 168 const Matrix operator +=( Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 169 {
madcowswe 0:a0285293f6a6 170 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 171 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 172 leftM._matrix[i][j] += number;
madcowswe 0:a0285293f6a6 173 return leftM;
madcowswe 0:a0285293f6a6 174 }
madcowswe 0:a0285293f6a6 175
madcowswe 0:a0285293f6a6 176
madcowswe 0:a0285293f6a6 177 const Matrix operator -=( Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 178 {
madcowswe 0:a0285293f6a6 179 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 180 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 181 leftM._matrix[i][j] -= number;
madcowswe 0:a0285293f6a6 182 return leftM;
madcowswe 0:a0285293f6a6 183 }
madcowswe 0:a0285293f6a6 184
madcowswe 0:a0285293f6a6 185
madcowswe 0:a0285293f6a6 186 const Matrix operator +( const Matrix& leftM, const Matrix& rightM)
madcowswe 0:a0285293f6a6 187 {
madcowswe 0:a0285293f6a6 188 if( leftM._nRows == rightM._nRows && leftM._nCols == rightM._nCols )
madcowswe 0:a0285293f6a6 189 {
madcowswe 0:a0285293f6a6 190 Matrix result( leftM._nRows, leftM._nCols );
madcowswe 0:a0285293f6a6 191
madcowswe 0:a0285293f6a6 192 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 193 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 194 result._matrix[i][j] = leftM._matrix[i][j] + rightM._matrix[i][j];
madcowswe 0:a0285293f6a6 195
madcowswe 0:a0285293f6a6 196 return result;
madcowswe 0:a0285293f6a6 197
madcowswe 0:a0285293f6a6 198 }else{
madcowswe 0:a0285293f6a6 199 printf( "\n\nERROR\nDiferent Dimensions @ + operator \n" );
madcowswe 0:a0285293f6a6 200 //Matrix error(4);
madcowswe 0:a0285293f6a6 201 //error.Clear();
madcowswe 0:a0285293f6a6 202 //return error;
madcowswe 0:a0285293f6a6 203 }
madcowswe 0:a0285293f6a6 204 }
madcowswe 0:a0285293f6a6 205
madcowswe 0:a0285293f6a6 206
madcowswe 0:a0285293f6a6 207 const Matrix operator +( const Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 208 {
madcowswe 0:a0285293f6a6 209 Matrix result( leftM._nRows, leftM._nCols );
madcowswe 0:a0285293f6a6 210
madcowswe 0:a0285293f6a6 211 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 212 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 213 result._matrix[i][j] = leftM._matrix[i][j] + number;
madcowswe 0:a0285293f6a6 214
madcowswe 0:a0285293f6a6 215 return result;
madcowswe 0:a0285293f6a6 216 }
madcowswe 0:a0285293f6a6 217
madcowswe 0:a0285293f6a6 218
madcowswe 0:a0285293f6a6 219 const Matrix operator +( float number, const Matrix& leftM )
madcowswe 0:a0285293f6a6 220 {
madcowswe 0:a0285293f6a6 221 return ( leftM + number );
madcowswe 0:a0285293f6a6 222 }
madcowswe 0:a0285293f6a6 223
madcowswe 0:a0285293f6a6 224
madcowswe 0:a0285293f6a6 225 const Matrix operator -( const Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 226 {
madcowswe 0:a0285293f6a6 227 if( leftM._nRows == rightM._nRows && leftM._nCols == rightM._nCols )
madcowswe 0:a0285293f6a6 228 {
madcowswe 0:a0285293f6a6 229 Matrix result( leftM._nRows, leftM._nCols );
madcowswe 0:a0285293f6a6 230
madcowswe 0:a0285293f6a6 231 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 232 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 233 result._matrix[i][j] = leftM._matrix[i][j] - rightM._matrix[i][j];
madcowswe 0:a0285293f6a6 234
madcowswe 0:a0285293f6a6 235 return result;
madcowswe 0:a0285293f6a6 236
madcowswe 0:a0285293f6a6 237 }else{
madcowswe 0:a0285293f6a6 238 printf( "\n\nERROR:\nDiferent Dimensions @ + operator \n" );
madcowswe 0:a0285293f6a6 239
madcowswe 0:a0285293f6a6 240 }
madcowswe 0:a0285293f6a6 241 }
madcowswe 0:a0285293f6a6 242
madcowswe 0:a0285293f6a6 243
madcowswe 0:a0285293f6a6 244 const Matrix operator -( const Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 245 {
madcowswe 0:a0285293f6a6 246 Matrix result( leftM._nRows, leftM._nCols );
madcowswe 0:a0285293f6a6 247
madcowswe 0:a0285293f6a6 248 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 249 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 250 result._matrix[i][j] = leftM._matrix[i][j] - number;
madcowswe 0:a0285293f6a6 251
madcowswe 0:a0285293f6a6 252 return result;
madcowswe 0:a0285293f6a6 253 }
madcowswe 0:a0285293f6a6 254
madcowswe 0:a0285293f6a6 255
madcowswe 0:a0285293f6a6 256 const Matrix operator -( float number, const Matrix& leftM )
madcowswe 0:a0285293f6a6 257 {
madcowswe 0:a0285293f6a6 258 return ( leftM - number );
madcowswe 0:a0285293f6a6 259 }
madcowswe 0:a0285293f6a6 260
madcowswe 0:a0285293f6a6 261
madcowswe 0:a0285293f6a6 262 const Matrix operator *( const Matrix& leftM, const Matrix& rightM )
madcowswe 0:a0285293f6a6 263 {
madcowswe 0:a0285293f6a6 264 if( leftM._nCols == rightM._nRows )
madcowswe 0:a0285293f6a6 265 {
madcowswe 0:a0285293f6a6 266 Matrix resultM ( leftM._nRows, rightM._nCols );
madcowswe 0:a0285293f6a6 267 resultM.Clear();
madcowswe 0:a0285293f6a6 268
madcowswe 0:a0285293f6a6 269 for( int i = 0; i < resultM._nRows; i++ )
madcowswe 0:a0285293f6a6 270 for( int j = 0; j < resultM._nCols; j++ )
madcowswe 0:a0285293f6a6 271 for( int m = 0; m < rightM._nRows; m++ )
madcowswe 0:a0285293f6a6 272 resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
madcowswe 0:a0285293f6a6 273
madcowswe 0:a0285293f6a6 274 return resultM;
madcowswe 0:a0285293f6a6 275
madcowswe 0:a0285293f6a6 276 } else {
madcowswe 0:a0285293f6a6 277
madcowswe 0:a0285293f6a6 278 printf("\n\nERROR:\nDiferent Dimension matrices @ * operator");
madcowswe 0:a0285293f6a6 279 }
madcowswe 0:a0285293f6a6 280
madcowswe 0:a0285293f6a6 281 }
madcowswe 0:a0285293f6a6 282
madcowswe 0:a0285293f6a6 283
madcowswe 0:a0285293f6a6 284 const Matrix operator *( const Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 285 {
madcowswe 0:a0285293f6a6 286 Matrix result( leftM._nRows, leftM._nCols );
madcowswe 0:a0285293f6a6 287
madcowswe 0:a0285293f6a6 288 for( int i = 0; i < leftM._nRows; i++ )
madcowswe 0:a0285293f6a6 289 for( int j = 0; j < leftM._nCols; j++ )
madcowswe 0:a0285293f6a6 290 result._matrix[i][j] = leftM._matrix[i][j] * number;
madcowswe 0:a0285293f6a6 291
madcowswe 0:a0285293f6a6 292 return result;
madcowswe 0:a0285293f6a6 293 }
madcowswe 0:a0285293f6a6 294
madcowswe 0:a0285293f6a6 295 const Matrix operator *( float number, const Matrix& leftM )
madcowswe 0:a0285293f6a6 296 {
madcowswe 0:a0285293f6a6 297 return ( leftM * number );
madcowswe 0:a0285293f6a6 298 }
madcowswe 0:a0285293f6a6 299
madcowswe 0:a0285293f6a6 300
madcowswe 0:a0285293f6a6 301 Matrix& operator <<( Matrix& leftM, float number )
madcowswe 0:a0285293f6a6 302 {
madcowswe 0:a0285293f6a6 303 if( leftM._pCol == leftM._nCols ) //end of Row
madcowswe 0:a0285293f6a6 304 {
madcowswe 0:a0285293f6a6 305 leftM._pCol = 0;
madcowswe 0:a0285293f6a6 306 leftM._pRow++;
madcowswe 0:a0285293f6a6 307 }
madcowswe 0:a0285293f6a6 308 if( leftM._pRow > leftM._nRows )
madcowswe 0:a0285293f6a6 309 {
madcowswe 0:a0285293f6a6 310 printf( "\n\nERROR:\nAssignment out of limits @ << operator" );
madcowswe 0:a0285293f6a6 311 return leftM;
madcowswe 0:a0285293f6a6 312
madcowswe 0:a0285293f6a6 313 }else{
madcowswe 0:a0285293f6a6 314
madcowswe 0:a0285293f6a6 315 leftM._matrix[ leftM._pRow ][ leftM._pCol ] = number;
madcowswe 0:a0285293f6a6 316 leftM._pCol++;
madcowswe 0:a0285293f6a6 317
madcowswe 0:a0285293f6a6 318 return leftM;
madcowswe 0:a0285293f6a6 319 }
madcowswe 0:a0285293f6a6 320 }