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