altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

matrix.cpp@4:3c21fb0c9e84, 2019-05-03 (annotated)

Committer:
altb2
Date:
Fri May 03 13:46:40 2019 +0000
Revision:
4:3c21fb0c9e84
Child:
6:5824bd96b6cf
mit EKF, noch nicht lauffaehig

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 4:3c21fb0c9e84 1 #include "matrix.h"
altb2 4:3c21fb0c9e84 2 using namespace std;
altb2 4:3c21fb0c9e84 3
altb2 4:3c21fb0c9e84 4 matrix::matrix(uint8_t I,uint8_t J,float e)
altb2 4:3c21fb0c9e84 5 {
altb2 4:3c21fb0c9e84 6 this->I = I;
altb2 4:3c21fb0c9e84 7 this->J = J;
altb2 4:3c21fb0c9e84 8 //int **mat = (int **)malloc(rows * sizeof(int*));
altb2 4:3c21fb0c9e84 9 //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
altb2 4:3c21fb0c9e84 10 a = (float **)malloc(I * sizeof(float *));
altb2 4:3c21fb0c9e84 11 for (uint8_t i=0; i<I; i++)
altb2 4:3c21fb0c9e84 12 a[i] = (float *)malloc(J * sizeof(float));
altb2 4:3c21fb0c9e84 13 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 14 for(uint8_t j=0;j<J;j++)
altb2 4:3c21fb0c9e84 15 a[i][j]=e;//;*/
altb2 4:3c21fb0c9e84 16 }
altb2 4:3c21fb0c9e84 17 // create Unity matrix
altb2 4:3c21fb0c9e84 18 matrix::matrix(uint8_t IJ)
altb2 4:3c21fb0c9e84 19 {
altb2 4:3c21fb0c9e84 20 this->I = IJ;
altb2 4:3c21fb0c9e84 21 this->J = IJ;
altb2 4:3c21fb0c9e84 22 //int **mat = (int **)malloc(rows * sizeof(int*));
altb2 4:3c21fb0c9e84 23 //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
altb2 4:3c21fb0c9e84 24 a = (float **)malloc(IJ * sizeof(float *));
altb2 4:3c21fb0c9e84 25 for (uint8_t i=0; i<IJ; i++)
altb2 4:3c21fb0c9e84 26 a[i] = (float *)malloc(IJ * sizeof(float));
altb2 4:3c21fb0c9e84 27 for(uint8_t i=0;i<IJ;i++)
altb2 4:3c21fb0c9e84 28 for(uint8_t j=0;j<IJ;j++)
altb2 4:3c21fb0c9e84 29 a[i][j]=0.0;//;*/
altb2 4:3c21fb0c9e84 30 for(uint8_t i=0;i<IJ;i++)
altb2 4:3c21fb0c9e84 31 a[i][i]=1.0;//;*/
altb2 4:3c21fb0c9e84 32 }
altb2 4:3c21fb0c9e84 33 // create Unity matrix
altb2 4:3c21fb0c9e84 34 matrix::matrix(uint8_t IJ,float e)
altb2 4:3c21fb0c9e84 35 {
altb2 4:3c21fb0c9e84 36 this->I = IJ;
altb2 4:3c21fb0c9e84 37 this->J = IJ;
altb2 4:3c21fb0c9e84 38 //int **mat = (int **)malloc(rows * sizeof(int*));
altb2 4:3c21fb0c9e84 39 //for(int i = 0; i < rows; i++) mat[i] = (int *)malloc(cols * sizeof(int));
altb2 4:3c21fb0c9e84 40 a = (float **)malloc(IJ * sizeof(float *));
altb2 4:3c21fb0c9e84 41 for (uint8_t i=0; i<IJ; i++)
altb2 4:3c21fb0c9e84 42 a[i] = (float *)malloc(IJ * sizeof(float));
altb2 4:3c21fb0c9e84 43 for(uint8_t i=0;i<IJ;i++)
altb2 4:3c21fb0c9e84 44 for(uint8_t j=0;j<IJ;j++)
altb2 4:3c21fb0c9e84 45 a[i][j]=0.0;//;*/
altb2 4:3c21fb0c9e84 46 for(uint8_t i=0;i<IJ;i++)
altb2 4:3c21fb0c9e84 47 a[i][i]=e;//;*/
altb2 4:3c21fb0c9e84 48 }
altb2 4:3c21fb0c9e84 49
altb2 4:3c21fb0c9e84 50 void matrix::printout(void)
altb2 4:3c21fb0c9e84 51 {
altb2 4:3c21fb0c9e84 52 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 53 {
altb2 4:3c21fb0c9e84 54 for(uint8_t j=0;j<J;j++)
altb2 4:3c21fb0c9e84 55 printf("%.8e ",a[i][j]);
altb2 4:3c21fb0c9e84 56 printf("..\r\n");
altb2 4:3c21fb0c9e84 57 }
altb2 4:3c21fb0c9e84 58 }
altb2 4:3c21fb0c9e84 59
altb2 4:3c21fb0c9e84 60 matrix::~matrix(void) {
altb2 4:3c21fb0c9e84 61 for (uint8_t i=0; i<I; i++)
altb2 4:3c21fb0c9e84 62 delete a[i];
altb2 4:3c21fb0c9e84 63 delete [] a;
altb2 4:3c21fb0c9e84 64 // printf("Matrix is being deleted\r\n");
altb2 4:3c21fb0c9e84 65 }
altb2 4:3c21fb0c9e84 66 uint8_t matrix::getI(void)
altb2 4:3c21fb0c9e84 67 {return I;}
altb2 4:3c21fb0c9e84 68 uint8_t matrix::getJ(void)
altb2 4:3c21fb0c9e84 69 {return J;}
altb2 4:3c21fb0c9e84 70
altb2 4:3c21fb0c9e84 71
altb2 4:3c21fb0c9e84 72 matrix matrix::operator*(const matrix& B) {
altb2 4:3c21fb0c9e84 73 if (J != B.I) {
altb2 4:3c21fb0c9e84 74 printf("Matrices shapes mismatch");
altb2 4:3c21fb0c9e84 75 }
altb2 4:3c21fb0c9e84 76 matrix C(I, B.J,0.0);
altb2 4:3c21fb0c9e84 77 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 78 for(uint8_t j=0;j<B.J;j++)
altb2 4:3c21fb0c9e84 79 for(uint8_t k=0;k<J;k++)
altb2 4:3c21fb0c9e84 80 C.a[i][j]+=(a[i][k]*B.a[k][j]);
altb2 4:3c21fb0c9e84 81 return C;
altb2 4:3c21fb0c9e84 82 }
altb2 4:3c21fb0c9e84 83 // calculate A'*B
altb2 4:3c21fb0c9e84 84 matrix matrix::operator *=(const matrix& B) {
altb2 4:3c21fb0c9e84 85 if (I != B.I) {
altb2 4:3c21fb0c9e84 86 printf("Matrices shapes mismatch");
altb2 4:3c21fb0c9e84 87 }
altb2 4:3c21fb0c9e84 88 matrix C(J, B.J,0.0);
altb2 4:3c21fb0c9e84 89 for(uint8_t j1=0;j1<J;j1++)
altb2 4:3c21fb0c9e84 90 for(uint8_t j=0;j<B.J;j++)
altb2 4:3c21fb0c9e84 91 for(uint8_t k=0;k<I;k++)
altb2 4:3c21fb0c9e84 92 C.a[j1][j]+=(a[k][j1]*B.a[k][j]);
altb2 4:3c21fb0c9e84 93 return C;
altb2 4:3c21fb0c9e84 94 }
altb2 4:3c21fb0c9e84 95 // calculate A*B'
altb2 4:3c21fb0c9e84 96 matrix matrix::operator /=(const matrix& B) {
altb2 4:3c21fb0c9e84 97 if (J != B.J) {
altb2 4:3c21fb0c9e84 98 printf("Matrices shapes mismatch");
altb2 4:3c21fb0c9e84 99 }
altb2 4:3c21fb0c9e84 100 matrix C(I, B.I,0.0);
altb2 4:3c21fb0c9e84 101 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 102 for(uint8_t i1=0;i1<B.I;i1++)
altb2 4:3c21fb0c9e84 103 for(uint8_t k=0;k<J;k++)
altb2 4:3c21fb0c9e84 104 C.a[i][i1]+=(a[i][k]*B.a[i1][k]);
altb2 4:3c21fb0c9e84 105 return C;
altb2 4:3c21fb0c9e84 106 }
altb2 4:3c21fb0c9e84 107 // calc Matrix A+B
altb2 4:3c21fb0c9e84 108 matrix matrix::operator+(const matrix& B) {
altb2 4:3c21fb0c9e84 109 matrix C(I, J,0.0);
altb2 4:3c21fb0c9e84 110 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 111 for(uint8_t j=0;j<B.J;j++)
altb2 4:3c21fb0c9e84 112 C.a[i][j]=(a[i][j]+B.a[i][j]);
altb2 4:3c21fb0c9e84 113 return C;
altb2 4:3c21fb0c9e84 114 }
altb2 4:3c21fb0c9e84 115 // calc Matrix A=A+B
altb2 4:3c21fb0c9e84 116 void matrix::operator+=(const matrix& B) {
altb2 4:3c21fb0c9e84 117 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 118 for(uint8_t j=0;j<B.J;j++)
altb2 4:3c21fb0c9e84 119 a[i][j]+=B.a[i][j];
altb2 4:3c21fb0c9e84 120 }
altb2 4:3c21fb0c9e84 121 // calc Matrix A+B
altb2 4:3c21fb0c9e84 122 matrix matrix::operator-(const matrix& B) {
altb2 4:3c21fb0c9e84 123 matrix C(I, J,0.0);
altb2 4:3c21fb0c9e84 124 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 125 for(uint8_t j=0;j<B.J;j++)
altb2 4:3c21fb0c9e84 126 C.a[i][j]=(a[i][j]-B.a[i][j]);
altb2 4:3c21fb0c9e84 127 return C;
altb2 4:3c21fb0c9e84 128 }
altb2 4:3c21fb0c9e84 129
altb2 4:3c21fb0c9e84 130 // calc Matrix inv(A), A must be 2x2
altb2 4:3c21fb0c9e84 131 matrix matrix::inv_2x2(void) {
altb2 4:3c21fb0c9e84 132 matrix C(2, 2,0.0);
altb2 4:3c21fb0c9e84 133 float idet=1/(a[0][0]*a[1][1]-a[0][1]*a[1][0]);
altb2 4:3c21fb0c9e84 134 C.a[0][0] = idet * a[1][1];
altb2 4:3c21fb0c9e84 135 C.a[0][1] = -idet * a[0][1];
altb2 4:3c21fb0c9e84 136 C.a[1][0] = -idet * a[1][0];
altb2 4:3c21fb0c9e84 137 C.a[1][1] = idet * a[0][0];
altb2 4:3c21fb0c9e84 138 return C;
altb2 4:3c21fb0c9e84 139 }
altb2 4:3c21fb0c9e84 140 void matrix::scale(float sc)
altb2 4:3c21fb0c9e84 141 {
altb2 4:3c21fb0c9e84 142 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 143 for(uint8_t j=0;j<J;j++)
altb2 4:3c21fb0c9e84 144 a[i][j]*=sc;
altb2 4:3c21fb0c9e84 145 }
altb2 4:3c21fb0c9e84 146 void matrix::put_entry(uint8_t i,uint8_t j,float e)
altb2 4:3c21fb0c9e84 147 {
altb2 4:3c21fb0c9e84 148 a[i-1][j-1]=e;
altb2 4:3c21fb0c9e84 149 }
altb2 4:3c21fb0c9e84 150 // Fill Column
altb2 4:3c21fb0c9e84 151 void matrix::fill_row(uint8_t i,float *e)
altb2 4:3c21fb0c9e84 152 {
altb2 4:3c21fb0c9e84 153 for(uint8_t j=0;j<J;j++)
altb2 4:3c21fb0c9e84 154 a[i-1][j]=e[j];
altb2 4:3c21fb0c9e84 155 }
altb2 4:3c21fb0c9e84 156
altb2 4:3c21fb0c9e84 157 // Fill Row
altb2 4:3c21fb0c9e84 158 void matrix::fill_col(uint8_t j,float *e)
altb2 4:3c21fb0c9e84 159 {
altb2 4:3c21fb0c9e84 160 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 161 a[i][j-1]=e[i];
altb2 4:3c21fb0c9e84 162 }
altb2 4:3c21fb0c9e84 163 // Copy matrix
altb2 4:3c21fb0c9e84 164 void matrix::mcopy(matrix *B)
altb2 4:3c21fb0c9e84 165 {
altb2 4:3c21fb0c9e84 166 for(uint8_t i=0;i<I;i++)
altb2 4:3c21fb0c9e84 167 for(uint8_t j=0;j<J;j++)
altb2 4:3c21fb0c9e84 168 a[i][j]=B->a[i][j];
altb2 4:3c21fb0c9e84 169
altb2 4:3c21fb0c9e84 170 }