altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

matrix.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
6:5824bd96b6cf 
Correct magnetometer.

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 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 }