AHRS
Dependencies: Eigen
matrix.cpp@6:5824bd96b6cf, 2019-06-14 (annotated)
- Committer:
- altb2
- Date:
- Fri Jun 14 07:05:20 2019 +0000
- Revision:
- 6:5824bd96b6cf
- Parent:
- 4:3c21fb0c9e84
fixed Bugs in Matrix class
Who changed what in which revision?
User | Revision | Line number | New 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 | a = (float **)malloc(I * sizeof(float *)); |
altb2 | 4:3c21fb0c9e84 | 9 | for (uint8_t i=0; i<I; i++) |
altb2 | 4:3c21fb0c9e84 | 10 | a[i] = (float *)malloc(J * sizeof(float)); |
altb2 | 4:3c21fb0c9e84 | 11 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 12 | for(uint8_t j=0;j<J;j++) |
altb2 | 4:3c21fb0c9e84 | 13 | a[i][j]=e;//;*/ |
altb2 | 4:3c21fb0c9e84 | 14 | } |
altb2 | 4:3c21fb0c9e84 | 15 | // create Unity matrix |
altb2 | 4:3c21fb0c9e84 | 16 | matrix::matrix(uint8_t IJ) |
altb2 | 4:3c21fb0c9e84 | 17 | { |
altb2 | 4:3c21fb0c9e84 | 18 | this->I = IJ; |
altb2 | 4:3c21fb0c9e84 | 19 | this->J = IJ; |
altb2 | 4:3c21fb0c9e84 | 20 | a = (float **)malloc(IJ * sizeof(float *)); |
altb2 | 4:3c21fb0c9e84 | 21 | for (uint8_t i=0; i<IJ; i++) |
altb2 | 4:3c21fb0c9e84 | 22 | a[i] = (float *)malloc(IJ * sizeof(float)); |
altb2 | 4:3c21fb0c9e84 | 23 | for(uint8_t i=0;i<IJ;i++) |
altb2 | 4:3c21fb0c9e84 | 24 | for(uint8_t j=0;j<IJ;j++) |
altb2 | 4:3c21fb0c9e84 | 25 | a[i][j]=0.0;//;*/ |
altb2 | 4:3c21fb0c9e84 | 26 | for(uint8_t i=0;i<IJ;i++) |
altb2 | 4:3c21fb0c9e84 | 27 | a[i][i]=1.0;//;*/ |
altb2 | 4:3c21fb0c9e84 | 28 | } |
altb2 | 6:5824bd96b6cf | 29 | // create diagonal matrix with Entry e |
altb2 | 4:3c21fb0c9e84 | 30 | matrix::matrix(uint8_t IJ,float e) |
altb2 | 4:3c21fb0c9e84 | 31 | { |
altb2 | 4:3c21fb0c9e84 | 32 | this->I = IJ; |
altb2 | 4:3c21fb0c9e84 | 33 | this->J = IJ; |
altb2 | 4:3c21fb0c9e84 | 34 | a = (float **)malloc(IJ * sizeof(float *)); |
altb2 | 4:3c21fb0c9e84 | 35 | for (uint8_t i=0; i<IJ; i++) |
altb2 | 4:3c21fb0c9e84 | 36 | a[i] = (float *)malloc(IJ * sizeof(float)); |
altb2 | 4:3c21fb0c9e84 | 37 | for(uint8_t i=0;i<IJ;i++) |
altb2 | 4:3c21fb0c9e84 | 38 | for(uint8_t j=0;j<IJ;j++) |
altb2 | 4:3c21fb0c9e84 | 39 | a[i][j]=0.0;//;*/ |
altb2 | 4:3c21fb0c9e84 | 40 | for(uint8_t i=0;i<IJ;i++) |
altb2 | 4:3c21fb0c9e84 | 41 | a[i][i]=e;//;*/ |
altb2 | 4:3c21fb0c9e84 | 42 | } |
altb2 | 4:3c21fb0c9e84 | 43 | |
altb2 | 6:5824bd96b6cf | 44 | // display matrix on screen |
altb2 | 4:3c21fb0c9e84 | 45 | void matrix::printout(void) |
altb2 | 4:3c21fb0c9e84 | 46 | { |
altb2 | 4:3c21fb0c9e84 | 47 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 48 | { |
altb2 | 4:3c21fb0c9e84 | 49 | for(uint8_t j=0;j<J;j++) |
altb2 | 4:3c21fb0c9e84 | 50 | printf("%.8e ",a[i][j]); |
altb2 | 4:3c21fb0c9e84 | 51 | printf("..\r\n"); |
altb2 | 4:3c21fb0c9e84 | 52 | } |
altb2 | 4:3c21fb0c9e84 | 53 | } |
altb2 | 4:3c21fb0c9e84 | 54 | |
altb2 | 4:3c21fb0c9e84 | 55 | matrix::~matrix(void) { |
altb2 | 4:3c21fb0c9e84 | 56 | for (uint8_t i=0; i<I; i++) |
altb2 | 4:3c21fb0c9e84 | 57 | delete a[i]; |
altb2 | 4:3c21fb0c9e84 | 58 | delete [] a; |
altb2 | 4:3c21fb0c9e84 | 59 | // printf("Matrix is being deleted\r\n"); |
altb2 | 4:3c21fb0c9e84 | 60 | } |
altb2 | 4:3c21fb0c9e84 | 61 | uint8_t matrix::getI(void) |
altb2 | 4:3c21fb0c9e84 | 62 | {return I;} |
altb2 | 4:3c21fb0c9e84 | 63 | uint8_t matrix::getJ(void) |
altb2 | 4:3c21fb0c9e84 | 64 | {return J;} |
altb2 | 4:3c21fb0c9e84 | 65 | |
altb2 | 4:3c21fb0c9e84 | 66 | |
altb2 | 4:3c21fb0c9e84 | 67 | matrix matrix::operator*(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 68 | if (J != B.I) { |
altb2 | 4:3c21fb0c9e84 | 69 | printf("Matrices shapes mismatch"); |
altb2 | 4:3c21fb0c9e84 | 70 | } |
altb2 | 4:3c21fb0c9e84 | 71 | matrix C(I, B.J,0.0); |
altb2 | 4:3c21fb0c9e84 | 72 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 73 | for(uint8_t j=0;j<B.J;j++) |
altb2 | 4:3c21fb0c9e84 | 74 | for(uint8_t k=0;k<J;k++) |
altb2 | 4:3c21fb0c9e84 | 75 | C.a[i][j]+=(a[i][k]*B.a[k][j]); |
altb2 | 4:3c21fb0c9e84 | 76 | return C; |
altb2 | 4:3c21fb0c9e84 | 77 | } |
altb2 | 4:3c21fb0c9e84 | 78 | // calculate A'*B |
altb2 | 4:3c21fb0c9e84 | 79 | matrix matrix::operator *=(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 80 | if (I != B.I) { |
altb2 | 4:3c21fb0c9e84 | 81 | printf("Matrices shapes mismatch"); |
altb2 | 4:3c21fb0c9e84 | 82 | } |
altb2 | 4:3c21fb0c9e84 | 83 | matrix C(J, B.J,0.0); |
altb2 | 4:3c21fb0c9e84 | 84 | for(uint8_t j1=0;j1<J;j1++) |
altb2 | 4:3c21fb0c9e84 | 85 | for(uint8_t j=0;j<B.J;j++) |
altb2 | 4:3c21fb0c9e84 | 86 | for(uint8_t k=0;k<I;k++) |
altb2 | 4:3c21fb0c9e84 | 87 | C.a[j1][j]+=(a[k][j1]*B.a[k][j]); |
altb2 | 4:3c21fb0c9e84 | 88 | return C; |
altb2 | 4:3c21fb0c9e84 | 89 | } |
altb2 | 4:3c21fb0c9e84 | 90 | // calculate A*B' |
altb2 | 4:3c21fb0c9e84 | 91 | matrix matrix::operator /=(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 92 | if (J != B.J) { |
altb2 | 4:3c21fb0c9e84 | 93 | printf("Matrices shapes mismatch"); |
altb2 | 4:3c21fb0c9e84 | 94 | } |
altb2 | 4:3c21fb0c9e84 | 95 | matrix C(I, B.I,0.0); |
altb2 | 4:3c21fb0c9e84 | 96 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 97 | for(uint8_t i1=0;i1<B.I;i1++) |
altb2 | 4:3c21fb0c9e84 | 98 | for(uint8_t k=0;k<J;k++) |
altb2 | 4:3c21fb0c9e84 | 99 | C.a[i][i1]+=(a[i][k]*B.a[i1][k]); |
altb2 | 4:3c21fb0c9e84 | 100 | return C; |
altb2 | 4:3c21fb0c9e84 | 101 | } |
altb2 | 6:5824bd96b6cf | 102 | // calc Matrix C=A+B |
altb2 | 4:3c21fb0c9e84 | 103 | matrix matrix::operator+(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 104 | matrix C(I, J,0.0); |
altb2 | 4:3c21fb0c9e84 | 105 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 106 | for(uint8_t j=0;j<B.J;j++) |
altb2 | 4:3c21fb0c9e84 | 107 | C.a[i][j]=(a[i][j]+B.a[i][j]); |
altb2 | 4:3c21fb0c9e84 | 108 | return C; |
altb2 | 4:3c21fb0c9e84 | 109 | } |
altb2 | 4:3c21fb0c9e84 | 110 | // calc Matrix A=A+B |
altb2 | 4:3c21fb0c9e84 | 111 | void matrix::operator+=(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 112 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 113 | for(uint8_t j=0;j<B.J;j++) |
altb2 | 4:3c21fb0c9e84 | 114 | a[i][j]+=B.a[i][j]; |
altb2 | 4:3c21fb0c9e84 | 115 | } |
altb2 | 6:5824bd96b6cf | 116 | // calc Matrix C=A-B |
altb2 | 4:3c21fb0c9e84 | 117 | matrix matrix::operator-(const matrix& B) { |
altb2 | 4:3c21fb0c9e84 | 118 | matrix C(I, J,0.0); |
altb2 | 4:3c21fb0c9e84 | 119 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 120 | for(uint8_t j=0;j<B.J;j++) |
altb2 | 4:3c21fb0c9e84 | 121 | C.a[i][j]=(a[i][j]-B.a[i][j]); |
altb2 | 4:3c21fb0c9e84 | 122 | return C; |
altb2 | 4:3c21fb0c9e84 | 123 | } |
altb2 | 4:3c21fb0c9e84 | 124 | |
altb2 | 4:3c21fb0c9e84 | 125 | // calc Matrix inv(A), A must be 2x2 |
altb2 | 4:3c21fb0c9e84 | 126 | matrix matrix::inv_2x2(void) { |
altb2 | 4:3c21fb0c9e84 | 127 | matrix C(2, 2,0.0); |
altb2 | 4:3c21fb0c9e84 | 128 | float idet=1/(a[0][0]*a[1][1]-a[0][1]*a[1][0]); |
altb2 | 4:3c21fb0c9e84 | 129 | C.a[0][0] = idet * a[1][1]; |
altb2 | 4:3c21fb0c9e84 | 130 | C.a[0][1] = -idet * a[0][1]; |
altb2 | 4:3c21fb0c9e84 | 131 | C.a[1][0] = -idet * a[1][0]; |
altb2 | 4:3c21fb0c9e84 | 132 | C.a[1][1] = idet * a[0][0]; |
altb2 | 4:3c21fb0c9e84 | 133 | return C; |
altb2 | 4:3c21fb0c9e84 | 134 | } |
altb2 | 4:3c21fb0c9e84 | 135 | void matrix::scale(float sc) |
altb2 | 4:3c21fb0c9e84 | 136 | { |
altb2 | 4:3c21fb0c9e84 | 137 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 138 | for(uint8_t j=0;j<J;j++) |
altb2 | 4:3c21fb0c9e84 | 139 | a[i][j]*=sc; |
altb2 | 4:3c21fb0c9e84 | 140 | } |
altb2 | 4:3c21fb0c9e84 | 141 | void matrix::put_entry(uint8_t i,uint8_t j,float e) |
altb2 | 4:3c21fb0c9e84 | 142 | { |
altb2 | 6:5824bd96b6cf | 143 | a[i][j]=e; |
altb2 | 4:3c21fb0c9e84 | 144 | } |
altb2 | 4:3c21fb0c9e84 | 145 | // Fill Column |
altb2 | 4:3c21fb0c9e84 | 146 | void matrix::fill_row(uint8_t i,float *e) |
altb2 | 4:3c21fb0c9e84 | 147 | { |
altb2 | 4:3c21fb0c9e84 | 148 | for(uint8_t j=0;j<J;j++) |
altb2 | 6:5824bd96b6cf | 149 | a[i][j]=e[j]; |
altb2 | 4:3c21fb0c9e84 | 150 | } |
altb2 | 4:3c21fb0c9e84 | 151 | |
altb2 | 4:3c21fb0c9e84 | 152 | // Fill Row |
altb2 | 4:3c21fb0c9e84 | 153 | void matrix::fill_col(uint8_t j,float *e) |
altb2 | 4:3c21fb0c9e84 | 154 | { |
altb2 | 4:3c21fb0c9e84 | 155 | for(uint8_t i=0;i<I;i++) |
altb2 | 6:5824bd96b6cf | 156 | a[i][j]=e[i]; |
altb2 | 4:3c21fb0c9e84 | 157 | } |
altb2 | 4:3c21fb0c9e84 | 158 | // Copy matrix |
altb2 | 4:3c21fb0c9e84 | 159 | void matrix::mcopy(matrix *B) |
altb2 | 4:3c21fb0c9e84 | 160 | { |
altb2 | 4:3c21fb0c9e84 | 161 | for(uint8_t i=0;i<I;i++) |
altb2 | 4:3c21fb0c9e84 | 162 | for(uint8_t j=0;j<J;j++) |
altb2 | 4:3c21fb0c9e84 | 163 | a[i][j]=B->a[i][j]; |
altb2 | 4:3c21fb0c9e84 | 164 | |
altb2 | 4:3c21fb0c9e84 | 165 | } |