AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Revision:
6:5824bd96b6cf
Parent:
4:3c21fb0c9e84
diff -r eee47600b772 -r 5824bd96b6cf matrix.cpp
--- a/matrix.cpp	Fri May 03 14:43:47 2019 +0000
+++ b/matrix.cpp	Fri Jun 14 07:05:20 2019 +0000
@@ -5,8 +5,6 @@
 {
     this->I = I;
     this->J = J;
-    //int **mat = (int **)malloc(rows * sizeof(int*));
-    //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
     a = (float **)malloc(I * sizeof(float *)); 
     for (uint8_t i=0; i<I; i++) 
          a[i] = (float *)malloc(J * sizeof(float)); 
@@ -19,8 +17,6 @@
 {
     this->I = IJ;
     this->J = IJ;
-    //int **mat = (int **)malloc(rows * sizeof(int*));
-    //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
     a = (float **)malloc(IJ * sizeof(float *)); 
     for (uint8_t i=0; i<IJ; i++) 
          a[i] = (float *)malloc(IJ * sizeof(float)); 
@@ -30,13 +26,11 @@
     for(uint8_t i=0;i<IJ;i++)
             a[i][i]=1.0;//;*/
     }
-// create Unity matrix
+// create diagonal matrix with Entry e
 matrix::matrix(uint8_t IJ,float e)
 {
     this->I = IJ;
     this->J = IJ;
-    //int **mat = (int **)malloc(rows * sizeof(int*));
-    //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
     a = (float **)malloc(IJ * sizeof(float *)); 
     for (uint8_t i=0; i<IJ; i++) 
          a[i] = (float *)malloc(IJ * sizeof(float)); 
@@ -47,6 +41,7 @@
             a[i][i]=e;//;*/
     }
 
+// display matrix on screen
 void matrix::printout(void)
 {
     for(uint8_t i=0;i<I;i++)
@@ -104,7 +99,7 @@
                 C.a[i][i1]+=(a[i][k]*B.a[i1][k]);
     return C;
 }
-// calc Matrix A+B
+// calc Matrix C=A+B
 matrix matrix::operator+(const matrix& B) {
     matrix C(I, J,0.0);
     for(uint8_t i=0;i<I;i++)
@@ -118,7 +113,7 @@
         for(uint8_t j=0;j<B.J;j++)
             a[i][j]+=B.a[i][j];
 }
-// calc Matrix A+B
+// calc Matrix C=A-B
 matrix matrix::operator-(const matrix& B) {
     matrix C(I, J,0.0);
     for(uint8_t i=0;i<I;i++)
@@ -145,20 +140,20 @@
     }
 void matrix::put_entry(uint8_t i,uint8_t j,float e)
 {
-    a[i-1][j-1]=e;
+    a[i][j]=e;
     }
 // Fill Column
 void matrix::fill_row(uint8_t i,float *e)
 {
     for(uint8_t j=0;j<J;j++)
-        a[i-1][j]=e[j];
+        a[i][j]=e[j];
     }
     
 // Fill Row
 void matrix::fill_col(uint8_t j,float *e)
 {
     for(uint8_t i=0;i<I;i++)
-        a[i][j-1]=e[i];
+        a[i][j]=e[i];
     }
 // Copy matrix
 void matrix::mcopy(matrix *B)