matrix.cpp matrix.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/matrix.cpp matrix.h void void madd (int *m1, int *m2, int *result, unsigned int rows, unsigned int columns) madd int * m1 int * m2 int * result unsigned int rows unsigned int columns void void maddD (int *m1, int *m2, unsigned int minrowcol, unsigned int columns) maddD int * m1 int * m2 unsigned int minrowcol unsigned int columns void void msub (int *m1, int *m2, int *result, unsigned int rows, unsigned int columns) msub int * m1 int * m2 int * result unsigned int rows unsigned int columns void void mmult (int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns) mmult int * m1 int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns void void mmultt (int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns) mmultt int * m1 int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns void void mmult15 (int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns) mmult15 int * m1 int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns void void mmultt15 (int *m1, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns) mmultt15 int * m1 int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns void void mmult1530 (int *m1, long *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns) mmult1530 int * m1 long * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns void void mmultDl (int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns, unsigned int minrowcol) mmultDl int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns unsigned int minrowcol void void mmultDl15 (int *DIAG, int *m2, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns, unsigned int minrowcol) mmultDl15 int * DIAG int * m2 int * result unsigned int rows unsigned int columnsx unsigned int columns unsigned int minrowcol void void mmultDr (int *m1, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns, unsigned int minrowcol) mmultDr int * m1 int * result unsigned int rows unsigned int columnsx unsigned int columns unsigned int minrowcol void void mmultDr15 (int *m1, int *DIAG, int *result, unsigned int rows, unsigned int columnsx, unsigned int columns, unsigned int minrowcol) mmultDr15 int * m1 int * DIAG int * result unsigned int rows unsigned int columnsx unsigned int columns unsigned int minrowcol void void mtrans (int *m1, int *result, unsigned int rows, unsigned int columns) mtrans int * m1 int * result unsigned int rows unsigned int columns void void minv2 (int *matrix, long *result) minv2 int * matrix long * result void void choice_P (int *m, int *result, unsigned int columns) choice_P int * m int * result unsigned int columns void void choice_x (int *m, int *result) choice_x int * m int * result /************************************ ExtendedKalmanFilter Matrixoperations Z.Peroutka Rev.15.3.2008 15.3.2008Kompletnikontrolavypoctu+zamenaq15->intaq30->long *************************************/ #include"matrix.h" /*Vsechnymezeseudavajivetvaru(rozmer_matice-1),abychmelveFORkonstantnihornimez)*/ /*Matrixadditioninq15:m1+m2=result[rows,columns]*/ voidmadd(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumns); /*adddiagonalmatrixm2tomatrixm1-bothinformatq15,minrowcol=min(rows,columns)*/ voidmaddD(int*m1,int*m2,unsignedintminrowcol,unsignedintcolumns); /*Matrixsubstractioninq15:m1-m2=result[rows,columns]*/ voidmsub(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumns); /*Matrixmultiplyinq15:m1[rows,columnsx]*m2[columnsx,columns]=result[rows,columns]*/ voidmmult(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns); /*Matrixmultiplicationinq15:m1[rows,columnsx]*(m2[columnsx,columns]transpose)=result[rows,columns]*/ voidmmultt(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns); /*matrixmultiplicationinq15:sumisinq15*/ voidmmult15(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns); /*Matrixmultiplicationinq15(sumisinq15):m1[rows,columnsx]*(m2[columns,columnsx]transpose)=result[rows,columns]*/ voidmmultt15(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns); /*Matrixmultiplication-Q15*Q30format->RESULTinQ15*/ voidmmult1530(int*m1,long*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns); /*LeftmatrixmultiplicationwithDIAGonesmatrixinq15:DIAG[rows,columnsx]*m2[columnsx,columns]=result[rows,columns] minrowcol=minimum(rows,columnsx)*/ voidmmultDl(int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol); /*LeftmatrixmultiplicationwithDIAGmatrixinq15:DIAG[rows,columnsx]*m2[columnsx,columns]=result[rows,columns] minrowcol=minimum(rows,columnsx)*/ voidmmultDl15(int*DIAG,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol); /*RightmatrixmultiplicationwithDIAGonesmatrixinq15:m1[rows,columnsx]*DIAG[columnsx,columns]=result[rows,columns] minrowcol=minimum(columnsx,columns)*/ voidmmultDr(int*m1,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol); /*RightmatrixmultiplicationwithDIAGmatrixinq15:m1[rows,columnsx]*DIAG[columnsx,columns]=result[rows,columns] minrowcol=minimum(columnsx,columns)*/ voidmmultDr15(int*m1,int*DIAG,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol); /*Matrixtranspositioninq15:m1.'=result[rows,columns]*/ voidmtrans(int*m1,int*result,unsignedintrows,unsignedintcolumns); /*Matrix[2,2]inversioninq15:inv(m1)=result[rows,columns]*/ voidminv2(int*matrix,long*result); voidchoice_P(int*m,int*result,unsignedintcolumns); voidchoice_x(int*m,int*result); /*matrixadditioninq15*/ voidmadd(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumns) { unsignedinti,j; for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *result++=*m1+++*m2++; } /*adddiagonalmatrixm2tomatrixm1-bothinformatq15,minrowcol=min(rows,columns)*/ voidmaddD(int*m1,int*m2,unsignedintminrowcol,unsignedintcolumns) { unsignedinti; for(i=0;i<=minrowcol;i++) /**(m1+i*(columns+1)+i)+=*(m2+i*(columns+1)+i);*/ { *m1+=*m2; m1+=(columns+2); m2+=(columns+2); } } /*Matrixsubstractioninq15*/ voidmsub(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumns) { unsignedinti,j; for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *result++=*m1++-*m2++; } /*matrixmultiplicationinq15*/ voidmmult(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns) { unsignedinti,j,k; longtmp_sum=0; int*m2pom; for(i=0;i<=rows;i++) { for(j=0;j<=columns;j++) { m2pom=m2+j; for(k=0;k<=columnsx;k++) { tmp_sum+=(long)(*m1++)**m2pom; m2pom+=columns+1; } //saturationeffect tmp_sum=tmp_sum>>15; if(tmp_sum>32767)tmp_sum=32767; if(tmp_sum<-32768)tmp_sum=-32768; *result++=tmp_sum; tmp_sum=0; m1-=(columnsx+1); } m1+=(columnsx+1); } } /*Matrixmultiplicationinq15:m1[rows,columnsx]*(m2[columns,columnsx]transpose)=result[rows,columns]*/ voidmmultt(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns) { unsignedinti,j,k; longtmp_sum=0; int*m2pom=m2; for(i=0;i<=rows;i++) { for(j=0;j<=columns;j++) { for(k=0;k<=columnsx;k++) tmp_sum+=(long)(*m1++)*(*m2pom++); //saturationeffect tmp_sum=tmp_sum>>15; if(tmp_sum>32767)tmp_sum=32767; if(tmp_sum<-32768)tmp_sum=-32768; *result++=tmp_sum; tmp_sum=0; m1-=(columnsx+1); } m2pom=m2; m1+=(columnsx+1); } } /*matrixmultiplicationinq15:sumisinq15*/ voidmmult15(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns) { unsignedinti,j,k; longtmp_sum=0; int*m2pom; for(i=0;i<=rows;i++) { for(j=0;j<=columns;j++) { m2pom=m2+j; for(k=0;k<=columnsx;k++) { tmp_sum+=((long)(*m1++)**m2pom)>>15; m2pom+=columns+1; } //saturationeffect if(tmp_sum>32767)tmp_sum=32767; if(tmp_sum<-32768)tmp_sum=-32768; *result++=tmp_sum; tmp_sum=0; m1-=(columnsx+1); } m1+=(columnsx+1); } } /*Matrixmultiplicationinq15(sumisinq15):m1[rows,columnsx]*(m2[columns,columnsx]transpose)=result[rows,columns]*/ voidmmultt15(int*m1,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns) { unsignedinti,j,k; longtmp_sum=0; int*m2pom=m2; for(i=0;i<=rows;i++) { for(j=0;j<=columns;j++) { for(k=0;k<=columnsx;k++) tmp_sum+=((long)(*m1++)*(*m2pom++))>>15; //saturationeffect if(tmp_sum>32767)tmp_sum=32767; if(tmp_sum<-32768)tmp_sum=-32768; *result++=tmp_sum; tmp_sum=0; m1-=(columnsx+1); } m2pom=m2; m1+=(columnsx+1); } } /*Matrixmultiplication-Q15*Q30format->RESULTinQ15*/ voidmmult1530(int*m1,long*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns) { unsignedinti,j,k; longtmp_sum=0; long*m2pom; for(i=0;i<=rows;i++) { for(j=0;j<=columns;j++) { m2pom=m2+j; for(k=0;k<=columnsx;k++) { tmp_sum+=(long)(*m1++)**m2pom; m2pom+=columns+1; } //saturationeffect tmp_sum=tmp_sum>>15; if(tmp_sum>32767)tmp_sum=32767; if(tmp_sum<-32768)tmp_sum=-32768; *result++=tmp_sum; tmp_sum=0; m1-=(columnsx+1); } m1+=(columnsx+1); } } /*LeftmatrixmultiplicationwithDIAGonesmatrixinq15:DIAG[rows,columnsx]*m2[columnsx,columns]=result[rows,columns] minrowcol=minimum(rows,columnsx)*/ voidmmultDl(int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol) { unsignedinti,j; /*int*res_tmp=result;*/ /*ThiserasesequencehavetobeenabledincasethatresultmatrixhasmoretermsthanDIAGmatrix*/ /*for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *res_tmp++=0; /*ENDOFERASESEQUENCE*/ for(i=0;i<=minrowcol;i++) for(j=0;j<=columns;j++) *result++=*m2++; } /*LeftmatrixmultiplicationwithDIAGmatrixinq15:DIAG[rows,columnsx]*m2[columnsx,columns]=result[rows,columns] minrowcol=minimum(rows,columnsx)*/ voidmmultDl15(int*DIAG,int*m2,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol) { unsignedinti,j; /*int*res_tmp=result;*/ /*ThiserasesequencehavetobeenabledincasethatresultmatrixhasmoretermsthanDIAGmatrix*/ /*for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *res_tmp++=0; /*ENDOFERASESEQUENCE*/ for(i=0;i<=minrowcol;i++) { for(j=0;j<=columns;j++) *result++=((long)(*DIAG)*(*m2++))>>15; DIAG+=(columnsx+1); } } /*RightmatrixmultiplicationwithDIAGonesmatrixinq15:m1[rows,columnsx]*DIAG[columnsx,columns]=result[rows,columns] minrowcol=minimum(columnsx,columns)*/ voidmmultDr(int*m1,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol) { unsignedinti,j; /*int*res_tmp=result;*/ /*ThiserasesequencehavetobeenabledincasethatresultmatrixhasmoretermsthanDIAGmatrix*/ /*for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *res_tmp++=0; /*ENDOFERASESEQUENCE*/ for(i=0;i<=rows;i++) { for(j=0;j<=minrowcol;j++) *result++=*m1++; m1+=(columnsx-minrowcol); result+=(columns-minrowcol); } } /*RightmatrixmultiplicationwithDIAGmatrixinq15:m1[rows,columnsx]*DIAG[columnsx,columns]=result[rows,columns] minrowcol=minimum(columnsx,columns)*/ voidmmultDr15(int*m1,int*DIAG,int*result,unsignedintrows,unsignedintcolumnsx,unsignedint columns,unsignedintminrowcol) { unsignedinti,j; int*DIAG_tmp=DIAG; /*int*res_tmp=result;*/ /*ThiserasesequencehavetobeenabledincasethatresultmatrixhasmoretermsthanDIAGmatrix*/ /*for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *res_tmp++=0; /*ENDOFERASESEQUENCE*/ for(i=0;i<=rows;i++) { for(j=0;j<=minrowcol;j++) { *result++=((long)(*m1++)**DIAG_tmp)>>15; DIAG_tmp+=columns+2; } DIAG_tmp=DIAG; m1+=(columnsx-minrowcol); result+=(columns-minrowcol); } } /*matrixtransposition-nenioptimalizovana,protozesenepouziva*/ voidmtrans(int*m1,int*result,unsignedintrows,unsignedintcolumns) { unsignedinti,j; for(i=0;i<=rows;i++) for(j=0;j<=columns;j++) *result++=*(m1+(columns+1)*j+i); } /*matrix[2,2]inversion*/ voidminv2(int*matrix,long*result) { intdet; unsignedinti; det=((long)*matrix**(matrix+3)-(long)*(matrix+1)**(matrix+2))>>15; if(det==0)det=1; *result++=(((long)*(matrix+3))<<15)/det; *result++=(((long)*(matrix+1))<<15)/det; *result++=(((long)*(matrix+2))<<15)/det; *result++=(((long)*matrix)<<15)/det; } voidchoice_P(int*m,int*result,unsignedintcolumns) { *result++=*m; *result++=*(m+1); *result++=*(m+columns+1); *result++=*(m+columns+2); } voidchoice_x(int*m,int*result) { *result++=*m++; *result=*m; }