int main(void) { Matrix A; Matrix B; double vec[3] = {1,1,1}; double vec2[3] = {2,3,4}; A.setMatrix(vec,3,1); B.setMatrix(vec2,1,3); std::cout << "1xn Vector multiplied by 1xn Vector\n"; mMult(A,B,1); B.printMatrix(); return 0; }
int main(void) { Matrix A; Matrix B; double* aData = new double[4]; double* bData = new double[4]; for (int i = 0; i < 4; i++) aData[i] = i+1; for (int i = 0; i < 4; i++) bData[i] = i+4; A.setMatrix(aData,2,2); B.setMatrix(bData,2,2); std::cout << "mxn Matrix multiplied by mxn Matrix\n"; mMult(A,B,1); B.printMatrix(); return 0; }
void EKF_Correction(ekf_data xpred, ekf_data *xcorr, float *obs ){ float expMeas[NUM_SENS]; // Expected measurements int idseg[NUM_SENS]; // Segment id for expected measurements int idJac[NUM_SENS]; // To store the indexes required to build the Jacobian // float px_s, py_s, th_s; // Sensor odo odo s_odo[NUM_SENS]; float cth, sth; int i; int h_line; // Number of lines for the Jacobian float **mJ; float **dMeas; // z-h cth=cos(xpred.th); sth=sin(xpred.th); // Compute the expected measurements for (i=0; i<NUM_SENS; i++) { // pxs= px + cos(th)*px_sb -sin(th)*py_sb s_odo[i].x= xpred.x+cth*sa[i][SX]-sth*sa[i][SY]; // pys= py + sin(th)*px_sb +cos(th)*py_sb s_odo[i].y= xpred.y+sth*sa[i][SX]+cth*sa[i][SY]; // pth_s= th + th_sb s_odo[i].th= xpred.th+sa[i][STH]; ExpSensReading(s_odo[i], expMeas+i, idseg+i); #ifdef EKF_DEBUG printf("expM: %f\tidseg: %d\n",expMeas[i],idseg[i]); printf("s_b -- x: %f\ty: %f\t th: %f\n",sa[i][SX],sa[i][SY],sa[i][STH]); printf("s_odo -- x: %f\ty: %f\t th: %f\n",s_odo[i].x,s_odo[i].y,s_odo[i].th); #endif } // Let us find out the good observations along with the good expected measurements h_line=0; for (i=0; i<NUM_SENS; i++) { idJac[i]=-1; if ( obs[i] < MAX_DIST && expMeas[i]<MAX_DIST) { idJac[h_line]=i; // Id of the "used" sensors h_line++; } } //#ifdef EKF_DEBUG printf("\nh_line: %d\n", h_line); for (i=0; i<NUM_SENS; i++) { printf("%d [%d]\n",idJac[i],i); } //#endif // Main data Structure for the Jacobian // mJ = (float **) malloc(sizeof(float *)* h_line); mBuild(&mJ, h_line, X_SIZE); // Compute the Jacobian for (i=0; i<h_line; i++) { float num, den, d_num, d_den, s_num, s_den; float cths, sths; // *(mJ+i)=(float *) malloc(sizeof(float)*X_SIZE); // Compute the Jacobian // Let us consider the prediction robot odo x=(px, py, th) // Let us consider the sensor base odometry (px_sb, py_sb, th_sb) // Let us consider the sensor odometry (px_s, py_s, th_s) // | a*px_s + b*py_s +c | |num| // h(x,M) = ------------------------ = ---- // | a*cos(th_s) + b*sin(th_s) |den| // with: // pxs= px + cos(th)*px_sb -sin(th)*py_sb // pys= py + sin(th)*px_sb +cos(th)*py_sb // pth_s= th + th_sb // cths=cos(s_odo[idJac[i]].th); sths=sin(s_odo[idJac[i]].th); num = map[idseg[idJac[i]]][A]*s_odo[idJac[i]].x + map[idseg[idJac[i]]][B]*s_odo[idJac[i]].y + map[idseg[idJac[i]]][C]; s_num = sign(num); den = map[idseg[idJac[i]]][A]*cths+ map[idseg[idJac[i]]][B]*sths; s_den = sign(den); // // d num d |num| d num // ------ = a --> ------- = sign(num)* ------ = sign(num) * a // d px d px d px // // d num d |num| d num // ------ = b --> ------- = sign(num)* ------ = sign(num) * b // d py d py d py // // d num // d_num = ------ = a*(-px_sb*sin(th)-py_sb*cos(th))+b*(px_sb*cos(th)-py_sb*sin(th)) // d th // d num // d_den = ------ = -a*sin(th_s)+b*cos(th_s) // d th // // d_num = map[idseg[idJac[i]]][A]*(-sa[idJac[i]][SX]*sth-sa[idJac[i]][SY]*cth)+ map[idseg[idJac[i]]][B]* (sa[idJac[i]][SX]*cth -sa[idJac[i]][SY]*sth ); d_den = -map[idseg[idJac[i]]][A]*sths + map[idseg[idJac[i]]][B]*cths; // Let us build the Jacobian // // d h 1 d |num| 1 // ---- = ----- * ------- = ----- * sign(num) * a // d px |den| d px |den| // mJ[i][0]=s_num*map[idseg[idJac[i]]][A]/fabs(den); // d h 1 d |num| 1 // ---- = ----- * ------- = ----- * sign(num) * b // d py |den| d py |den| // mJ[i][1]=s_num*map[idseg[idJac[i]]][B]/fabs(den);; // d h sign(num)*d_num*den - sign(den)*d_den*num // ---- = ------------------------------------------ // d tx den^2 // mJ[i][2]=(s_num*d_num*den - s_den*d_den*num)/(den*den); } //#ifdef EKF_DEBUG printf("\n"); for (i=0; i<NUM_SENS; i++) printf("Sensor: %d\tReading: %f\tWall: %d\n",i+1,expMeas[i],(idseg[i]>-1)?idseg[i]+1:idseg[i]); printf("Xpre --- x: %f\ty: %f\t th: %f\n",xpred.x,xpred.y,xpred.th); //#endif for (i=0; i<h_line; i++) printf("Jx: %f\tJy: %f\tJth: %f\n",mJ[i][0],mJ[i][1],mJ[i][2]); float **mP1; float **mP2; float **mP3; float **mK; // Let us build the matrix // P (n x n) J (q x n) // P1=mPpred*J^T (n x n) x (n x q) = (n x q) mBuild(&mP1,X_SIZE,h_line); mMultTr2(mP1, X_SIZE, h_line, mPpred, X_SIZE, X_SIZE, mJ, h_line, X_SIZE); // printf("\nP1=Ppred*J^T\n"); // mPrint(mP1,X_SIZE,h_line); // P2= J*P1 (q x n) x (n x q) = (q x q) mBuild(&mP2,h_line,h_line); mMult(mP2,h_line, h_line, mJ, h_line, X_SIZE, mP1, X_SIZE, h_line); // printf("\nP2=J*P1\n"); // mPrint(mP2,h_line,h_line); // P2= P2+R (q x q) + (q x q) = (q x q) for (i=0; i<h_line; i++) mP2[i][i]+=vectR[idJac[i]]; // printf("\nP2=P2+R\n"); // mPrint(mP2,h_line,h_line); // P3= inv(P2) (q x q) mBuild(&mP3,h_line,h_line); mInv(mP3,h_line,h_line,mP2,h_line,h_line); // printf("\nP3=inv(P2)\n"); // mPrint(mP3,h_line,h_line); // K= P1*P3 (n x q) x (q x q) = (n x q) mBuild(&mK,X_SIZE,h_line); mMult(mK,X_SIZE,h_line,mP1, X_SIZE, h_line, mP3, h_line, h_line); printf("\nK=P1*P3\n"); mPrint(mK,X_SIZE,h_line); // To build the innovation vector mBuild(&dMeas, h_line,1); for (i=0; i<h_line; i++) { dMeas[i][0]=obs[idJac[i]]-expMeas[idJac[i]]; } // mPrint(dMeas,h_line,1); float **mP4; float **mP5; float **mInn; // Pk = (I-KJ)P // K*J // (n x q) x (q x n) = (n x n) mBuild(&mP4,X_SIZE,X_SIZE); mMult(mP4,X_SIZE,X_SIZE,mK,X_SIZE,h_line,mJ,h_line,X_SIZE); // printf("\nmJ\n"); // mPrint(mJ,h_line,X_SIZE); // printf("\nmP4\n"); // mPrint(mP4,X_SIZE,X_SIZE); // (I-K*J) mEye(&mP5,X_SIZE); mSub2(mP5,X_SIZE,X_SIZE,mP4,X_SIZE,X_SIZE); // (I-K*J)*P mMult(mPcorr, X_SIZE, X_SIZE, mP5, X_SIZE, X_SIZE, mPpred, X_SIZE, X_SIZE); printf("\nmPcorr\n"); mPrint(mPcorr,X_SIZE,X_SIZE); // K*vec // (n x h_line) x (h_line x 1) mBuild(&mInn, X_SIZE, 1); mMult(mInn, X_SIZE, 1, mK, X_SIZE, h_line, dMeas, h_line, 1); printf("\ndMeas\n"); mPrint(dMeas,h_line,1); printf("\nmInn\n"); mPrint(mInn,X_SIZE,1); // Xpost xcorr->x = xpred.x + mInn[0][0]; xcorr->y = xpred.y + mInn[1][0]; xcorr->th = xpred.th + mInn[2][0]; // Free Memory mFree(mJ,h_line); mFree(mP1,X_SIZE); mFree(mP2,h_line); mFree(mP3,h_line); mFree(mP4,X_SIZE); mFree(mP5,X_SIZE); mFree(mK,X_SIZE); mFree(mInn,X_SIZE); mFree(dMeas,h_line); }
int main(int argc, char *argv[]) { int i,j; float **m1; float **m2; float **m3; float **m4; float **m5; float *diag4; float **m1T; float **M1; float M2[5][5]; int row1, col1; // n x m int row1T,col1T; // m x n int row2, col2; // n x n int row3, col3; // n x n int row4, col4; // n x n int row5, col5; // n x n if (argc<2) { printf("Usage: %s randSeed\n",argv[0]); return -1; } // Test of functions that allocate the memory required to store the resulting matrix row1=ROW; col1=COL; mRand(&m1,row1,col1,10,atoi(argv[1])); mPrint(m1,row1,col1); // Transposition mTranspB(&m1T,&row1T,&col1T,m1,row1,col1); mPrint(m1T,row1T,col1T); // Multiplication mMultB(&m2,&row2,&col2,m1,row1,col1,m1T,row1T,col1T); mPrint(m2,row2,col2); // Inversion mInvB(&m3,&row3,&col3,m2,row2,col2); mPrint(m3,row3,col3); // Diagonal matrix row4=ROW; col4=ROW; diag4= (float *) malloc(sizeof(float)*row4); for (i=0; i<row4; i++) diag4[i]=i+1; mDiag(&m4,row4,diag4); mPrint(m4,row4,col4); mFree(m1,row1); mFree(m1T,row1T); mFree(m2,row2); mFree(m3,row3); mFree(m4,row4); // Test of functions that assume the memory required to store the // resulting matrix to be already allocated // row1=row2=row3=row4=row1T=0; // col1=col2=col3=col4=col1T=0; // Random Creation mRand(&m1,row1,col1,10,atoi(argv[1])); mPrint(m1,row1,col1); mBuild(&m1T,row1T,col1T); mBuild(&m2,row2,col2); mBuild(&m3,row3,col3); mBuild(&m4,row4,col4); // Transposition mTransp(m1T,row1T,col1T,m1,row1,col1); mPrint(m1T,row1T,col1T); // Multiplication mMult(m2,row2,col2,m1,row1,col1,m1T,row1T,col1T); mPrint(m2,row2,col2); // Inversion mInv(m3,row3,col3,m2,row2,col2); mPrint(m3,row3,col3); // Diagonal matrix row4=ROW; col4=ROW; diag4= (float *) malloc(sizeof(float)*row4); for (i=0; i<row4; i++) diag4[i]=i+1; // mDiag(&m4,row4,diag4); // mPrint(m4,row4,col4); // Mult mPrint(m1T,row1T,col1T); mFree(m2,row2); mRand(&m2,row2,col2,10,atoi(argv[1])*5); mPrint(m2,row2,col2); mFree(m4,row4); row4=row2; col4=row1T; mBuild(&m4,row4,col4); mMultTr2(m4,row4,col4,m2,row2,col2,m1T,row1T,col1T); mPrint(m4,row4,col4); // Copy row5=row4; col5=col4; printf("\ncopy\n"); mBuild(&m5,row5,col5); mPrint(m5,row5,col5); mCopy(m5,row5,col5,m4,row4,col4); mPrint(m4,row4,col4); mPrint(m5,row5,col5); mFree(m1,row1); mFree(m1T,row1T); mFree(m2,row2); mFree(m3,row3); mFree(m4,row4); // To check copy to and from float[r][c] // So far only squared matrix are copied! printf("\nLast\n"); mRand(&M1,5,5,10,atoi(argv[1])*5); mPrint(M1,5,5); mCopyM2A(5,M2,M1); for(i=0; i<5; i++) for(j=0; j<5; j++) M2[i][j]*=100; mCopyA2M(M1,5,M2); mPrint(M1,5,5); return 0; }