void myMxM(Matrix A, Matrix v, Matrix u) { Matrix temp = createMatrix(A->rows, v->cols); #pragma omp parallel { int* displ, *cols; splitVector(v->cols, num_threads(), &cols, &displ); MxM2(A, v, temp, displ[get_thread()], cols[get_thread()], displ[get_thread()], 1.0, 0.0); free(cols); free(displ); } #ifdef HAVE_MPI for (int i=0;i<v->as_vec->comm_size;++i) { Matrix t = subMatrix(temp, v->as_vec->displ[i]/v->cols, v->as_vec->sizes[i]/v->cols, 0, v->cols); MPI_Reduce(t->data[0], u->data[0], v->as_vec->sizes[i], MPI_DOUBLE, MPI_SUM, i, *v->as_vec->comm); freeMatrix(t); } #else memcpy(u->data[0], temp->data[0], u->as_vec->len*sizeof(double)); #endif freeMatrix(temp); }
/** * @brief Overrides - operator for IntMatrix to subtract a matrix from the current matrix. * @param IntMatrix we wish to subtract from the current matrix. * @return a new IntMatrix we created */ const IntMatrix IntMatrix :: operator-(const IntMatrix &other) const { // copy our matrix, use -= to subtract other from it, then return it IntMatrix subMatrix(*this); subMatrix -= other; return subMatrix; }
void gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt) { float D = elem(sizeOfMatrix(*m_opt),0,1)+1; //printf("%f\n", D); float w_opt = 1/D; //printf("%f\n", w_opt); float Nx = elem(sizeOfMatrix(*xEst),0,0); //printf("%f\n", Nx); float d = Nx*(D-1) + 1; //printf("%f\n", d); float w = 1/d; //printf("%f\n", w); /* Eigenvectors, Eigenvalues */ int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 ); Matrix Vec = zeroMatrix(dimC, dimC); Matrix Val = zeroMatrix(dimC, dimC); eig ( CEst, &Vec, &Val ); /* m1 = vec*sqrtf(val) */ int i; for ( i = 0; i < dimC; ++i ) setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i)))); Matrix m1 = mulMatrix(Vec, Val); freeMatrix(Vec); freeMatrix(Val); /* rotate & scale samples: m = m1*S */ Matrix m = scaledSamplePoints(m1, *m_opt); /* x = x*ones(1,d) */ Matrix x = fillMatrix(*xEst, d); /* shift samples: m = m + x */ m = addMatrix(m, x); //printMatrix(m); /* afun */ /* Prediction: mean xPredDiracs = feval(afun,m, [], [], t); xPred = w*sum(xPredDiracs, 2);*/ Matrix xPredDiracs = (*afun) (m, *dt); //printMatrix(xPredDiracs); Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2); //printMatrix(xPredDiracsSum); Matrix xPred = mulScalarMatrix(w, xPredDiracsSum); //printMatrix(xPred); //mxDiracs = xPredDiracs-repmat(xPred, 1, d); //CPred = w_opt*mxDiracs*mxDiracs'; Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d)); //printMatrix(mxDiracs); Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs))); //printMatrix(CPred); //RETURN *xEst = xPred; //printMatrix(*xEst); *CEst = CPred; //printMatrix(*CEst); freeMatrix(m); freeMatrix(xPredDiracs); freeMatrix(xPredDiracsSum); }
int gcm::patchRun(vector<SLR_ST_Skeleton> vSkeletonData, vector<Mat> vDepthData, vector<IplImage*> vColorData, int *rankIndex, double *rankScore) { int kernelFeatureDim = NClass*NTrainSample; //clock_t startT=clock(); oriData2Feature(vSkeletonData, vDepthData, vColorData); //cout<<"=======Time========="<<clock()-startT<<endl; gcmSubspace(); x[0].index = 0; for (int j=0; j<kernelFeatureDim; j++) { subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim); x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim); x[j+1].index=j+1; } x[kernelFeatureDim+1].index=-1; //int testID = svm_predict_probability(myModel, x, prob_estimates); int testID_noPro = svm_predict(myModel, x); int testID = svm_predict_probability(myModel_candi, x, prob_estimates); //Sort and get the former 5 ranks. vector<scoreAndIndex> rank; for (int i=0; i<myModel->nr_class; i++) { scoreAndIndex temp; temp.index = myModel->label[i]; temp.score = prob_estimates[i]; rank.push_back(temp); } sort(rank.begin(),rank.end(),comp); rankIndex[0] = testID_noPro; rankScore[0] = 1.0; int candiN = 0; //for (int i=1; i<5; i++) int seqCandiN = 1; while(seqCandiN<5) { if (rank[candiN].index == testID_noPro) { candiN++; continue; } rankIndex[seqCandiN] = rank[candiN].index; rankScore[seqCandiN] = rank[candiN].score; candiN++; seqCandiN++; } releaseResource(); return rankIndex[0]; }
matrix matrix::getRows(int nNum, const int c[]) { matrix subMatrix(nNum, col); for(int i=0; i<nNum; i++) { for(int j=0; j<col; j++) { subMatrix.array[i][j] = array[c[i]][j]; } } return subMatrix; }
matrix matrix::getColumns(int nNum, const int c[]) { matrix subMatrix(row, nNum); for(int i=0; i<row; i++) { for(int j=0; j<nNum; j++) { subMatrix.array[i][j] = array[i][c[j]]; } } return subMatrix; }
void kalmanProcess(kalman_s *mykalman) { int i,j; for(i=0;i<NM;i++) memset(mykalman->temp_4_4[i],0,sizeof(double)*NM); //第一个公式:x(k|k-1) = Ax(k-1|k-1) multiplyMatrix(mykalman->A,NS,NS,mykalman->X,NS,1,mykalman->temp_1); //X=A*X for (i=0;i<NS;i++) mykalman->X[i][0]=mykalman->temp_1[i][0]; //第二个公式: P = A*P*A'+Q multiplyMatrix(mykalman->A,NS,NS,mykalman->P,NS,NS,mykalman->temp_2_1); //temp_2_1 = A*P transpositionMatrix(mykalman->A, mykalman->temp_2, NS, NS); //temp_2 = A' multiplyMatrix(mykalman->temp_2_1,NS,NS,mykalman->temp_2,NS,NS,mykalman->P); //P = A*P*A’ addMatrix(mykalman->P,NS,NS,mykalman->Q,NS,NS,mykalman->P); //P = A*P*A’+Q //第三个公式: X = X+K*[Z-H*X] multiplyMatrix(mykalman->H,NM,NS,mykalman->X,NS,1,mykalman->temp_3_1); //temp_3_1=H*X subMatrix(mykalman->Z,NM,1,mykalman->temp_3_1,NM,1,mykalman->temp_3_1); //temp_3_1=Z-H*X multiplyMatrix(mykalman->K,NS,NM,mykalman->temp_3_1,NM,1,mykalman->temp_3_2); //temp_3_2 = K*(Z-H*X) addMatrix(mykalman->X,NS,1,mykalman->temp_3_2,NS,1,mykalman->X); //X = X+ K*(Z-H*X) //第四个公式:K = P*H'/[H*P*H'+R] transpositionMatrix(mykalman->H,mykalman->temp_4_3,NM,NS); //temp_4_3 = H' multiplyMatrix(mykalman->P,NS,NS,mykalman->temp_4_3,NS,NM,mykalman->temp_4_1); //temp_4_1 = P*H' multiplyMatrix(mykalman->H,NM,NS,mykalman->temp_4_1,NS,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H' addMatrix(mykalman->temp_4_2,NM,NM,mykalman->R,NM,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'+R InverseMatrix(mykalman->temp_4_2, mykalman->temp_4_4, NM,NM); //temp_4_4=~(H*P*H'+R) multiplyMatrix(mykalman->temp_4_1,NS,NM,mykalman->temp_4_4,NM,NM,mykalman->K); //K = P*H'*~(H*P*H'+R) //第五个公式:P = [I-K*H]*P multiplyMatrix(mykalman->K,NS,NM,mykalman->H,NM,NS,mykalman->temp_5); //temp_5 = K*H subMatrix(mykalman->E,NS,NS,mykalman->temp_5,NS,NS,mykalman->temp_5_1); //temp_5_1 = E-K*H multiplyMatrix(mykalman->temp_5_1,NS,NS,mykalman->P,NS,NS,mykalman->temp_5_2); //temp_5_2 = (E-K*H)*P for (i=0;i<NS;i++) for (j=0;j<NS;j++) mykalman->P[i][j] = mykalman->temp_5_2[i][j]; }
//========================================================================== // Class: Matrix // Function: GetSubMatrix // // Description: Returns a sub-matrix made up of the specified portion of // this matrix. // // Input Arguments: // StartRow = const unsigned int& specifying the starting row // StartColumn = const unsigned int& specifying the starting column // SubRows = const unsigned int& specifying the number of rows // SubColumns = const unsigned int& specifying the number of columns // // Output Arguments: // None // // Return Value: // Matrix contining the specified sub-matrix // //========================================================================== Matrix Matrix::GetSubMatrix(const unsigned int &startRow, const unsigned int &startColumn, const unsigned int &subRows, const unsigned int &subColumns) const { assert(startRow + subRows <= rows && startColumn + subColumns <= columns); Matrix subMatrix(subRows, subColumns); unsigned int i, j; for (i = 0; i < subRows; i++) { for (j = 0; j < subColumns; j++) subMatrix.elements[i][j] = elements[i + startRow][j + startColumn]; } return subMatrix; }
Matriz Matriz::getSubMatrix(int i_0, int j_0, int i, int j) { if (i<i_0) swap(i, i_0); if (j<j_0) swap(j, j_0); Matriz subMatrix(i-i_0 + 1, j-j_0 + 1); for (int ii=i_0;ii<=i;ii++) { for (int jj=j_0;jj<=j;jj++) { subMatrix.addValue(values[ii][jj]); } } return subMatrix; }
int maxRectSum(vector<vector<int> > &matrix) { int maxSum = 0; if (matrix.empty()) return 0; for (int i = 0; i < matrix.size(); i ++) { vector<int> subMatrix(matrix[0].size(), 0); for (int j = i; j < matrix.size(); ++ j) { for (int k = 0; k < matrix[0].size(); ++ k) { subMatrix[k] += matrix[j][k]; } } maxSum = max(maxSum, maxConsSum(subMatrix)); } return maxSum; }
/* Main-Funktion */ int main(int argc, char *argv[]) { // Zufallszahlengenerator initialisieren srand(time(NULL)); matrix a = initMatrixRand(5,5); matrix b = initMatrixRand(5,5); printf("a =\n"); prettyPrint(a); printf("\nb =\n"); prettyPrint(b); matrix c = addMatrix(a, b); printf("\na + b =\n"); prettyPrint(c); freeMatrix(c); c = subMatrix(a, b); printf("\na - b =\n"); prettyPrint(c); freeMatrix(c); c = multMatrix(a, b); printf("\na * b =\n"); prettyPrint(c); freeMatrix(c); c = transposeMatrix(a); printf("\na^T =\n"); prettyPrint(c); freeMatrix(c); printf("\ndet(a) = %.2f\n", determinante(a)); printf("detQuick(a) = %.2f\n\n", detQuick(a)); printf("\ndet(b) = %.2f\n", determinante(b)); printf("detQuick(b) = %.2f\n\n", detQuick(b)); freeMatrix(a); freeMatrix(b); return 0; }
void Matrix::extractQ ( const Vector& tau, const Matrix& qr ) { const size_t rows = countRows(), cols = countColumns(), qrRows = qr.countRows(), qrCols = qr.countColumns(), tauDims = tau.countDimensions(); assert( qrRows >= qrCols ); assert( qrCols == tauDims ); assert( rows == qrRows ); assert( cols == qrCols || cols == qrRows ); fill( 0.0 ); Vector diagonal = diagonalVector(); diagonal.fill( 1.0 ); for ( size_t col = qrCols; 0 < col--; ) { Matrix affected = subMatrix( col, col, rows - col, cols - col ); affected.householderTransform( tau.get( col ), const_cast<Matrix&>( qr ).columnVector( col ).subVector( col, rows - col ) ); } }
/** **************************************************************************************************** \fn void Matrix4Inverse( Matrix &i_matrix, Matrix &o_matrix ) \brief Calculate the inverse of matrix4x4 i_matrix \param i_matrix input matrix \param o_matrix output matrix \return NONE **************************************************************************************************** */ void GameEngine::Math::Matrix::Matrix4Inverse( Matrix &i_matrix, Matrix &o_matrix ) { float determinant = 0.0f; Matrix subMatrix( 3, 3 ); INT8 sign = 0; FUNCTION_START; assert( i_matrix._u32Column == 4 ); assert( i_matrix._u32Row == 4 ); assert( o_matrix._u32Column == 4 ); assert( o_matrix._u32Row == 4 ); determinant = Matrix4Determinant( i_matrix ); if( Utilities::Math::AreRelativelyEqual(determinant, 0.0f, 20) ) { o_matrix = i_matrix.Identity(); FUNCTION_FINISH; return; } for( UINT8 i = 0; i < 4; ++i ) { for( UINT8 j = 0; j < 4; ++j ) { sign = 1 - ( (i + j) % 2 ) * 2; Matrix4SubMatrix( i_matrix, subMatrix, i, j ); o_matrix( j, i ) = ( Matrix3Determinant(subMatrix) * sign ) / determinant; } } FUNCTION_FINISH; return; }
/** **************************************************************************************************** \fn float Matrix4Determinant( Matrix &i_matrix ) \brief Calculate the determinant of matrix4x4 i_matrix \param i_matrix input matrix \return float \retval The determinant of input matrix **************************************************************************************************** */ float GameEngine::Math::Matrix::Matrix4Determinant( Matrix &i_matrix ) { float retVal = 0.0f; float determinant = 0.0f; INT8 sign = 1; UINT8 n = 0; Matrix subMatrix( 3, 3 ); FUNCTION_START; assert( i_matrix._u32Column == 4 ); assert( i_matrix._u32Row == 4 ); for( n = 0; n < 4; n++, sign *= -1 ) { Matrix4SubMatrix( i_matrix, subMatrix, 0, n ); determinant = Matrix3Determinant( subMatrix ); retVal += (i_matrix(0, n) * determinant * sign); } FUNCTION_FINISH; return retVal; }
Matrix* BlockMatrix::block2distributed() { if(this->nBlockRows < 1 || this->nBlockCols < 1) { misc.error("Error: An internal error was happened. A cyclic matrix cannot be generated from an empty block matrix.", 0); } Matrix* result = new Matrix(MATRIX_DEFAULT_DISTRIBUTION, this->nGlobRows, this->nGlobCols); result->fillWithConstant(0.); int rowShift = 0; for(int r = 0; r < this->nBlockRows; r++) { int colShift = 0; for(int c = 0; c < this->nBlockCols; c++) { result->add( this->m[r][c], 1., 1., subMatrix(rowShift, colShift ,this->m[r][c]->nGlobRows, this->m[r][c]->nGlobCols), subMatrix(this->m[r][c]) ); colShift += this->m[r][c]->nGlobCols; } rowShift += this->m[r][0]->nGlobRows; } return result; }
FgMatrixV colVec(uint n) const {return subMatrix(0,n,nrows,1); };
//This function is used to recognize continuous SLR in a off-line way. //Since the data should be read in all at once, the class gcmCont is not necessary used. int gcm::patchRun_continuous_PQ(vector<SLR_ST_Skeleton> vSkeletonData, vector<Mat> vDepthData, vector<IplImage*> vColorData, int *rankIndex, double *rankScore) { int window = 40; int kernelFeatureDim = NClass*NTrainSample; //Computing features cout<<"Computing features..."<<endl; oriData2Feature(vSkeletonData, vDepthData, vColorData); ////////////////////////////////////////////////////////////////////////// //Compute P and Q all at once. vector<double*> P; vector<double**> Q; cout<<"Computing P and Q..."<<endl; //computePQ(P, Q, vColorData.size()); //white for (int d=0; d<nDimension; d++) { double dimAve = 0.0; for (int f=0; f<nFrames; f++) { dimAve += feature_ori[f][d]; } dimAve /= nFrames; for (int f=0; f<nFrames; f++) { feature_ori[f][d] -= dimAve; } } //Compute P and Q. int nFrames_PQ = vColorData.size(); for (int i=0; i<nFrames_PQ; i++) { cout<<"Current "<<i<<"/"<<nFrames_PQ<<endl; //Compute P double* tempP = new double[featureDim]; for (int j=0; j<featureDim; j++) { tempP[j] = 0; for (int k=0; k<i; k++) { tempP[j] += feature_ori[k][j]; } } P.push_back(tempP); //To release it when reaching the end of a sentence //Compute Q double** tempQ; tempQ = newMatrix(featureDim, featureDim); for (int f1=0; f1<featureDim; f1++) { for (int f2=0; f2<featureDim; f2++) { tempQ[f1][f2] = 0; for (int l=0; l<i; l++) { tempQ[f1][f2] += feature_ori[f1][l]*feature_ori[f2][l]; } } } Q.push_back(tempQ); //To release it when reaching the end of a sentence } ////////////////////////////////////////////////////////////////////////// double** C = newMatrix(featureDim, featureDim); double* p_temp = new double[featureDim]; //The deltaP double** Pm = newMatrix(featureDim, featureDim); for (int i=window; i<vColorData.size()-window; i++) { int begin = i-window/2; int end = i+window/2; //The matrix from p for (int pf=0; pf<featureDim; pf++) { p_temp[pf] = P[end][pf]-P[begin][pf]; } vector2matrix(p_temp, p_temp, Pm,featureDim); //Compute the covariance matrix for (int l=0; l<featureDim; l++) { for (int m=0; m<featureDim; m++) { C[l][m] = ((Q[end][l][m]-Q[begin][l][m])-Pm[l][m]/(end-begin+1))/(end-begin); } } //Regularization term added for (int d=0; d<nDimension; d++) { for (int d2=0; d2<nDimension; d2++) { //C[d][d2] /= nFrames; if (d == d2) { C[d][d2] += 0.001; } } } //The subspace matrix PQsubspace(C, gcm_subspace); //For debug ofstream foutDebug; foutDebug.open("..\\output\\debug.txt"); for (int i=0; i<featureDim; i++) { for (int j=0; j<subSpaceDim; j++) { foutDebug<<gcm_subspace[i][j]<<"\t"; } foutDebug<<"\n"; } foutDebug << flush; foutDebug.close(); //The SVM classification x[0].index = 0; for (int j=0; j<kernelFeatureDim; j++) { subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim); x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim); x[j+1].index=j+1; } x[kernelFeatureDim+1].index=-1; int testID = svm_predict_probability(myModel, x, prob_estimates); cout<<"Frame: "<<i<<"/"<<vColorData.size()-window<<" Result: "<<testID<<endl; } delete[] p_temp; deleteMatrix(Pm,featureDim); deleteMatrix(C, featureDim); //To delete P and Q return 1; // gcmSubspace(); // // x[0].index = 0; // for (int j=0; j<kernelFeatureDim; j++) // { // subMatrix(subFeaAll_model, subFea1, 0, featureDim, j*subSpaceDim, subSpaceDim); // x[j+1].value = myGcmKernel.Frobenius(subFea1, gcm_subspace, featureDim, subSpaceDim); // x[j+1].index=j+1; // } // x[kernelFeatureDim+1].index=-1; // // int testID = svm_predict_probability(myModel, x, prob_estimates); // // //Sort and get the former 5 ranks. // vector<scoreAndIndex> rank; // for (int i=0; i<myModel->nr_class; i++) // { // scoreAndIndex temp; // temp.index = myModel->label[i]; // temp.score = prob_estimates[i]; // rank.push_back(temp); // } // sort(rank.begin(),rank.end(),comp); // // for (int i=0; i<5; i++) // { // rankIndex[i] = rank[i].index; // rankScore[i] = rank[i].score; // } // // // //Release // nFrames = 0; // // return testID; }
void main(int argc, char *argv[]){ int id,np; int dim1,dim2,dim3; double *a,*b,*c; double *adist,*bdist,*cdist; MPI_Init(&argc,&argv); sscanf(argv[1],"%d",&dim1); sscanf(argv[2], "%d",&dim2); sscanf(argv[3],"%d",&dim3); MPI_Comm_rank(MPI_COMM_WORLD, &id); MPI_Comm_size(MPI_COMM_WORLD, &np); if(id==0){ a=malloc(sizeof(double)*dim1*dim2); b=malloc(sizeof(double)*dim2*dim3); matrizAleatoria(dim1,dim2,a); matrizAleatoria(dim2,dim3,b); b[1] = 8; b[5] = 10; b[15] = 0; printf("A = \n"); impMat(dim1,dim2,a); printf("B = \n"); impMat(dim1,dim2,b); } adist=malloc(sizeof(double)*(dim1*dim2)/np); bdist=malloc(sizeof(double)*(dim2*dim3)/np); MPI_Scatter(a,(dim1*dim2)/np,MPI_DOUBLE,adist,(dim1*dim2)/np,MPI_DOUBLE,0,MPI_COMM_WORLD); MPI_Scatter(b,(dim2*dim3)/np,MPI_DOUBLE,bdist,(dim2*dim3)/np,MPI_DOUBLE,0,MPI_COMM_WORLD); //impMat(dim1/np,dim2,adist); //impMat(dim2/np,dim3,bdist); char TRANSA, TRANSB; int M, N, K; double ALPHA, BETA; TRANSA = 'N'; TRANSB = 'N'; ALPHA = 1.0; BETA = 1.0; M=dim1/np; K=dim2/np; N=dim3; cdist=malloc(sizeof(double)*(dim1*dim3)/np); zerosMat(dim1/np,dim3,cdist); int j; double *asub; asub=malloc(sizeof(double)*(dim1/np*dim2/np)); for(j=0;j<np;j++){ asub=subMatrix(dim1,dim2,mod(id-j,np),np,adist); dgemm_(&TRANSA, &TRANSB, &N, &M, &K, &ALPHA, bdist, &N, asub, &K, &BETA, cdist, &N); printf("---------------------------------------\n"); printf("i = %d\tj = %d\tnump = %d\n", id, j, mod(id-j,np)); printf("Aij = \n"); impMat(dim1/np,dim2/np,asub); //printf("Ai = \n"); //impMat(dim1/np,dim2,adist); printf("Bi = \n"); impMat(dim2/np,dim3,bdist); printf("Ci (resultado parcial) = \n"); impMatTransp(dim3,dim1/np,cdist); printf("---------------------------------------\n"); MPI_Send(bdist, (dim2*dim3)/np, MPI_DOUBLE, mod(id + 1,np), mod(id-j,np),MPI_COMM_WORLD); MPI_Recv(bdist,(dim2*dim3)/np, MPI_DOUBLE, mod(id - 1,np), mod(id-j-1,np), MPI_COMM_WORLD,MPI_STATUS_IGNORE); } if(id==0){ free(a); free(b); } free(adist); free(bdist); free(asub); MPI_Finalize(); }
void mainfunction(int *network,int *minNetwork,int size) { //calculate the b w auto dn = genDN(network,size); auto bw = calmaxBW(dn); //76,272 //std::cout<<"B = "<<bw.first<<" W = "<<bw.second<<std::endl; //calculate the supplement node int* supp_node = new int [size]; for(int i = 0; i < size; ++i) supp_node[i] = 0; int *supp_network = subMatrix(network,minNetwork,size,size); for(int i = 0; i < size; ++i){ for(int j = 0; j < size; ++j){ if(supp_network[i*size+j] != 0){ supp_node[j] ++; //outdegree!!! supp_node[i] ++; //indegree!!! } } } //calculate the b d int *node_num = new int[size]; for(int i = 0; i < size; ++i){ node_num[i] = i+1; //set node index } double *bd = calBD(network,size); //pop sort for(int i = 0; i < size; ++i){ for(int j = 0; j < size; ++j){ if(bd[i]>bd[j]){ swap2(bd[i],bd[j]); swap2(node_num[i],node_num[j]); swap2(supp_node[i],supp_node[j]); } } } //print the result //for(int i = 0; i < size; ++i){ //std::cout<<std::setw(2)<<node_num[i]<<" "<<std::setw(10)<<bd[i]<<" "<<supp_node[i]<<std::endl; //} //calculate the correlation double correlation = 0; double bd_total = 0; double indegree_total = 0; double deviation = 0; for(int i = 1; i < size; ++i){ bd_total += bd[i] * bd[i]; indegree_total += supp_node[i] * supp_node[i]; } for(int i = 1; i < size; ++i){ correlation += (abs2(bd[i])/sqrt(bd_total)) * (supp_node[i]/sqrt(indegree_total)); deviation += supp_node[i] * supp_node[i]; } //std::cout<<sqrt(deviation)/indegree_total<<" "<<correlation<<std::endl; std::cout<<bw.first<<" "<<correlation<<std::endl; //else std::cout<<0.1<<" "<<correlation<<std::endl; delete supp_node; delete node_num; delete bd; }
void gaussianEstimator_Est (Matrix *xEst, Matrix *CEst, Matrix *y, Matrix *Cv, Matrix (*hfun)(Matrix m), Matrix *m_opt) { //printMatrix(*xEst); //printMatrix(*CEst);system("PAUSE"); Matrix tmp = sizeOfMatrix(*m_opt); float D = elem(tmp,0,1)+1; //printf("%f\n", D); freeMatrix(tmp); float w_opt = 1/D; //printf("%f\n", w_opt); tmp = sizeOfMatrix(*xEst); float Nx = elem(tmp,0,0); // printf("%f\n", Nx); freeMatrix(tmp); float d = Nx*(D-1) + 1; //printf("%f\n", d); float w = 1/d; // printf("%f\n", w);system("PAUSE"); // Eigenvectors, Eigenvalues tmp = sizeOfMatrix(*CEst); int dimC = elem ( tmp, 0, 0 ); freeMatrix(tmp); Matrix Vec = zeroMatrix(dimC, dimC); Matrix Val = zeroMatrix(dimC, dimC); eig ( CEst, &Vec, &Val ); //printMatrix(Vec);printMatrix(Val);system("PAUSE"); // m1 = vec*sqrtf(val) int i; for ( i = 0; i < dimC; ++i ) setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i)))); Matrix m1 = mulMatrix(Vec, Val); //printMatrix(m1); system("PAUSE"); freeMatrix(Vec); freeMatrix(Val); //* rotate & scale samples: m = m1*S Matrix m = scaledSamplePoints(m1, *m_opt); // printMatrix(m); system("PAUSE"); Matrix mxDiracs = mulScalarMatrix(1, m); //* x = x*ones(1,d) Matrix x = fillMatrix(*xEst, d); // shift samples: m = m + x tmp = addMatrix(m, x); appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ) ; //printMatrix(m); freeMatrix(tmp); //% Predicted Measurements //* hfun // yPredDiracs = feval(hfun, m, [], [], t); // yPred = w*sum(yPredDiracs, 2); Matrix yPredDiracs = (*hfun) (m); //printMatrix(yPredDiracs ); Matrix yPredDiracsSum = sumMatrix(yPredDiracs, 2); Matrix yPred = mulScalarMatrix(w, yPredDiracsSum); // myDiracs = yPredDiracs-repmat(yPred, 1, d); tmp = fillMatrix(yPred, d); Matrix myDiracs = subMatrix(yPredDiracs, tmp); freeMatrix(tmp); //* CPred = w_opt*mxDiracs*mxDiracs'; // Matrix CPred = mulScalarMatrix( w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)) ); // Matrix CPred = *CEst; // Cxy = w_opt*mxDiracs*myDiracs'; Matrix tmp1 = transposeMatrix(myDiracs); Matrix tmp2 = mulMatrix(mxDiracs, tmp1); Matrix Cxy = mulScalarMatrix( w_opt, tmp2); freeMatrix(tmp1); freeMatrix(tmp2); // Cy = w_opt*myDiracs*myDiracs'+Cv; tmp1 = transposeMatrix(myDiracs); tmp2 = mulMatrix(myDiracs, tmp1); Matrix tmp3 = mulScalarMatrix( w_opt, tmp2); Matrix Cy = addMatrix( tmp3 , *Cv ); freeMatrix(tmp1); freeMatrix(tmp2); freeMatrix(tmp3); // K = Cxy / Cy; tmp = invertCovMatrix(Cy); Matrix K = mulMatrix( Cxy, tmp); freeMatrix(tmp); // I = y - yPred; Matrix I = subMatrix( *y, yPred ); // xEst = xPred + K*I; tmp = mulMatrix( K, I ); Matrix tmp23 = addMatrix( *xEst, tmp); appMatrix(*xEst,0,5,0,0, tmp23,0,5,0,0); freeMatrix(tmp); // CEst = CPred - K*Cy*K'; tmp1 = mulMatrix(K, Cy); tmp2 = transposeMatrix(K); tmp3 = mulMatrix( tmp1, tmp2); Matrix tmp24 = subMatrix(*CEst, tmp3); appMatrix(*CEst,0,5,0,5, tmp24,0,5,0,5); freeMatrix(tmp1); freeMatrix(tmp2); freeMatrix(tmp3); freeMatrix(tmp24); freeMatrix(m1); freeMatrix(m); freeMatrix(mxDiracs); freeMatrix(x); freeMatrix(yPredDiracs); freeMatrix(yPredDiracsSum);// freeMatrix(yPred);// freeMatrix(myDiracs); freeMatrix(Cxy); freeMatrix(Cy); freeMatrix(K);// freeMatrix(I);// freeMatrix(tmp23); }
void gaussianEstimator_Pred_decomp ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, float *dt, Matrix *m_opt) { float r; Matrix sizeMopt; Matrix xn = zeroMatrix(3,1); Matrix Cn = zeroMatrix(3,3); Matrix xl = zeroMatrix(9,1); Matrix Cl = zeroMatrix(9,9); Matrix Cnl = zeroMatrix(3,9); Matrix Cnl_T; Matrix Cn_i; Matrix CLN; Matrix sizeCn; Matrix Vec; Matrix Val; Matrix m1; Matrix m; Matrix x; Matrix A; Matrix Hi = zeroMatrix(12,9); Matrix Cy = zeroMatrix(12, 12); Matrix muy = zeroMatrix(12, 1); Matrix zeros33 = zeroMatrix(3,3); Matrix eye33 = unitMatrix(3,3); Matrix Mat; Matrix H; Matrix gi = zeroMatrix(12,1); Matrix Rot_vec = zeroMatrix(3,1); Matrix mui; Matrix muiy; Matrix Ciy; Matrix tmp; Matrix tmp1; Matrix tmp2; Matrix tmp3; Matrix tmp4; Matrix tmp5; Matrix tmp6; Matrix tmp7; Matrix tmp8; Matrix tmpHi; sizeMopt = sizeOfMatrix(*m_opt); //printf("%f\n",*dt); float D = elem(sizeMopt,0,1)+1; //printf("%f\n", D); freeMatrix(sizeMopt); float w_opt = 1/D; //printf("%f\n", w_opt); float Nx = 3; float d = Nx*(D-1) + 1; //printf("%f\n", d); float w = 1/d; //printf("%f\n", w); //xn = xEst(4:6); % Rotation vector appMatrix(xn, 0, 2, 0, 0, *xEst, 3, 5, 0, 0); //printMatrix(xn);system("PAUSE"); //Cn = CEst(4:6,4:6); appMatrix(Cn, 0, 2, 0, 2, *CEst, 3, 5, 3, 5); //printMatrix(Cn);system("PAUSE"); //xl = [xEst(1:3) ; xEst(7:12)]; % Translation, angular velocity, linear velocity appMatrix(xl, 0, 2, 0, 0, *xEst, 0, 2, 0, 0); appMatrix(xl, 3, 8, 0, 0, *xEst, 6, 11, 0, 0); //printMatrix(xl);system("PAUSE"); //Cl = [CEst(1:3,1:3) CEst(1:3,7:12); // CEst(7:12,1:3) CEst(7:12,7:12)] ; appMatrix(Cl, 0, 2, 0, 2, *CEst, 0, 2, 0, 2); appMatrix(Cl, 0, 2, 3, 8, *CEst, 0, 2, 6, 11); appMatrix(Cl, 3, 8, 0, 2, *CEst, 6, 11, 0, 2); appMatrix(Cl, 3, 8, 3, 8, *CEst, 6, 11, 6, 11); //printMatrix(Cl);system("PAUSE"); //Cnl = [CEst(4:6,1:3) CEst(4:6,7:12)]; appMatrix(Cnl, 0, 2, 0, 2, *CEst, 3, 5, 0, 2); appMatrix(Cnl, 0, 2, 3, 8, *CEst, 3, 5, 6, 11); //printMatrix(Cnl);system("PAUSE"); //CLN = Cl - Cnl'*inv(Cn)*Cnl; Cnl_T = transposeMatrix(Cnl); // printMatrix(Cn);system("PAUSE"); Cn_i = invertCovMatrix(Cn); //printMatrix(Cn_i);system("PAUSE"); tmp = mulMatrix( Cnl_T, Cn_i); tmp7 = mulMatrix(tmp, Cnl); CLN = subMatrix ( Cl, tmp7); //printMatrix(CLN);system("PAUSE"); freeMatrix(tmp); freeMatrix(tmp7); // Eigenvectors, Eigenvalues sizeCn = sizeOfMatrix(Cn); int dimC = elem ( sizeCn, 0, 0 ); freeMatrix(sizeCn); Vec = zeroMatrix(dimC, dimC); Val = zeroMatrix(dimC, dimC); eig ( &Cn, &Vec, &Val ); //printMatrix(Cn);printMatrix(Vec);printMatrix(Val);system("PAUSE"); // m1 = vec*sqrtf(val) int i; for ( i = 0; i < dimC; ++i ) setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i)))); m1 = mulMatrix(Vec, Val); //printMatrix(m1);system("PAUSE"); // rotate & scale samples: m = m1*S m = scaledSamplePoints(m1, *m_opt); //printMatrix(m);system("PAUSE"); // x = x*ones(1,d) x = fillMatrix(xn, d); // shift samples: m = m + x tmp = addMatrix(m, x); appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ); //printMatrix(m);system("PAUSE"); freeMatrix(tmp); //A = [[eye(3,3),t*eye(3,3)];[zeros(3,3),eye(3,3)]]; A = unitMatrix(6,6); setElem(A, 0, 3, *dt); setElem(A, 1, 4, *dt); setElem(A, 2, 5, *dt); //printMatrix(A);system("PAUSE"); for (i=0; i<d; i++) { //gi = [zeros(3,1); m(:,i); zeros(6,1)]; setElem(gi, 3, 0, elem(m, 0, i)); setElem(gi, 4, 0, elem(m, 1, i)); setElem(gi, 5, 0, elem(m, 2, i)); //printMatrix(gi);system("PAUSE"); //Rot_vec = m(:,i); setElem(Rot_vec, 0, 0, elem(m, 0, i)); setElem(Rot_vec, 1, 0, elem(m, 1, i)); setElem(Rot_vec, 2, 0, elem(m, 2, i)); //printMatrix(Rot_vec);system("PAUSE"); //r = norm(Rot_vec); r = sqrtf( powf((elem(Rot_vec,0,0)),2) + powf((elem(Rot_vec,1,0)),2) + powf((elem(Rot_vec,2,0)),2) ); //printf("%f\n",r); H = zeroMatrix(3,3); if (fmod(r, 2*pi) == 0) { Mat = unitMatrix(3,3); } else { // build skew symmetric Matrix setElem(H, 0, 1, -elem(Rot_vec,2,0)); setElem(H, 0, 2, elem(Rot_vec,1,0)); setElem(H, 1, 0, elem(Rot_vec,2,0)); setElem(H, 1, 2, -elem(Rot_vec,0,0)); setElem(H, 2, 0, -elem(Rot_vec,1,0)); setElem(H, 2, 1, elem(Rot_vec,0,0)); //printMatrix(H);system("PAUSE"); // Bortz equation // Mat = eye(3,3) + 0.5*H + (1- r*sin(r)/( 2*(1-cos(r))))/r^2*H*H; // already declared Mat = unitMatrix(3,3); tmp1 = mulScalarMatrix(0.5, H); tmp4 = addMatrix( eye33 , tmp1 ); tmp2 = mulMatrix(H, H); tmp3 = mulScalarMatrix( (1-(r*sin(r)/(2*(1-cos(r)))))/powf(r,2), tmp2); Mat = addMatrix( tmp4, tmp3); //printMatrix(Mat);system("PAUSE"); freeMatrix(tmp1); freeMatrix(tmp2); freeMatrix(tmp3); freeMatrix(tmp4); } //Hi = [[A(1:3,1:3) zeros(3,3) A(1:3,4:6)]; // [zeros(3,3), t*Mat, zeros(3,3)]; // [zeros(3,3), eye(3,3), zeros(3,3)]; // [A(4:6,1:3),zeros(3,3), A(4:6,4:6)]]; appMatrix( Hi, 0, 2, 0, 2, A, 0, 2, 0, 2 ); appMatrix( Hi, 0, 2, 3, 5, zeros33, 0, 2, 0, 2 ); appMatrix( Hi, 0, 2, 6, 8, A, 0, 2, 3, 5 ); appMatrix( Hi, 3, 5, 0, 2, zeros33, 0, 2, 0, 2 ); tmpHi = mulScalarMatrix(*dt, Mat); appMatrix( Hi, 3, 5, 3, 5, tmpHi, 0, 2, 0, 2 ); freeMatrix(tmpHi); appMatrix( Hi, 3, 5, 6, 8, zeros33, 0, 2, 0, 2 ); appMatrix( Hi, 6, 8, 0, 2, zeros33, 0, 2, 0, 2 ); appMatrix( Hi, 6, 8, 3, 5, eye33, 0, 2, 0, 2 ); appMatrix( Hi, 6, 8, 6, 8, zeros33, 0, 2, 0, 2 ); appMatrix( Hi, 9, 11, 0, 2, A, 3, 5, 0, 2 ); appMatrix( Hi, 9, 11, 3, 5, zeros33, 0, 2, 0, 2 ); appMatrix( Hi, 9, 11, 6, 8, A, 3, 5, 3, 5 ); //printMatrix(Hi);system("PAUSE"); // mui = xl + Cnl'*inv(Cn)*(m(:,i)-xn); //m(:,i) -> Rot_vec tmp = mulMatrix(Cnl_T, Cn_i ); tmp1 = subMatrix(Rot_vec, xn); tmp2 = mulMatrix(tmp, tmp1); mui = addMatrix(xl, tmp2); freeMatrix(tmp); freeMatrix(tmp1); freeMatrix(tmp2); //printMatrix(mui);system("PAUSE"); // muiy = gi + Hi * mui; tmp = mulMatrix(Hi, mui); muiy = addMatrix( gi, tmp); //printMatrix(muiy);system("PAUSE"); freeMatrix(tmp); // Ciy = Hi *CLN *Hi'; tmp1 = mulMatrix(Hi, CLN); tmp2 = transposeMatrix(Hi); Ciy = mulMatrix( tmp1, tmp2); //printMatrix(Ciy);system("PAUSE"); freeMatrix(tmp1); freeMatrix(tmp2); // Cy = Cy + (w*Ciy + w_opt*muiy*muiy'); tmp3 = mulScalarMatrix(w, Ciy); tmp1 = transposeMatrix(muiy); tmp2 = mulMatrix(muiy, tmp1); tmp4 = mulScalarMatrix( w_opt, tmp2 ); tmp5 = addMatrix( tmp3, tmp4 ); tmp6 = addMatrix( Cy, tmp5); appMatrix(Cy,0,Cy->height-1,0,Cy->width-1,tmp6, 0,tmp6->height-1,0,tmp6->width-1); //printMatrix(Cy);system("PAUSE"); freeMatrix(tmp1); freeMatrix(tmp2); freeMatrix(tmp3); freeMatrix(tmp4); freeMatrix(tmp5); freeMatrix(tmp6); // muy = muy + w*muiy; tmp = mulScalarMatrix( w, muiy ); tmp2 = addMatrix( muy, tmp ); appMatrix(muy,0,muy->height-1,0,muy->width-1, tmp2, 0, tmp2->height-1, 0, tmp2->width-1); //printMatrix(muy);system("PAUSE"); freeMatrix(tmp); freeMatrix(tmp2); freeMatrix(H); freeMatrix(Mat); freeMatrix(mui);// freeMatrix(muiy);// freeMatrix(Ciy); } appMatrix(*xEst, 0, 11, 0, 0, muy, 0, 11, 0, 0 ); //printMatrix(muy);system("PAUSE"); //CEst = Cy - muy*muy' * w_opt/w + Cw; tmp1 = transposeMatrix(muy); tmp2 = mulMatrix(muy, tmp1); tmp5 = mulScalarMatrix( w_opt/w, tmp2 ); tmp6 = subMatrix(Cy, tmp5); tmp8 = addMatrix( tmp6, *Cw); //printMatrix(*CEst);system("PAUSE"); appMatrix(*CEst,0,11,0,11, tmp8, 0,11,0,11 ); //printMatrix(tmp8);system("PAUSE"); freeMatrix(tmp1); freeMatrix(tmp2); freeMatrix(tmp5); freeMatrix(tmp6); freeMatrix(tmp8); freeMatrix(muy);// freeMatrix(zeros33);// freeMatrix(Vec); freeMatrix(Val); freeMatrix(Cy); freeMatrix(xn); freeMatrix(Cn); freeMatrix(xl); freeMatrix(Cl);// freeMatrix(Cnl); freeMatrix(Cnl_T); freeMatrix(Cn_i); freeMatrix(CLN);// freeMatrix(m1); freeMatrix(m);// freeMatrix(x); freeMatrix(A); freeMatrix(eye33); freeMatrix(Hi); freeMatrix(gi); freeMatrix(Rot_vec); } /* End gaussianPred_decomp */
int main (int argc, const char ** argv) { ProgramArgument program_args=ProcessArguments(argc,argv); bool print_ali=program_args.print_ali; ParseMatrix submatParser(program_args.matrixname); #ifdef DEBUG std::cout << "Print Matrix\n"; submatParser.print(); #endif SubstitutionMatrix subMatrix(submatParser.scores, submatParser.row_index); #ifdef DEBUG std::cout << "Print SubMatrix!\n"; subMatrix.print(); #endif SequenceLibrary sequences(program_args.seqlib); const size_t max_seq_size = sequences.get_max_seq_size(); PairsLibrary pairs(program_args.pairs,&sequences); GotohEight gotohAlgo(max_seq_size,&subMatrix, program_args.go, program_args.ge, program_args.mode); SequenceEight sequenceQuery(max_seq_size, subMatrix.aa2short,subMatrix.short2aa); SequenceEight sequenceTemplate(max_seq_size, subMatrix.aa2short,subMatrix.short2aa); char buffer[2097152]; std::cout.rdbuf()->pubsetbuf(buffer, 2097152); const char ** template_sequences=(const char **)memalign(16,(VEC_SIZE)*sizeof(const char *)); const char ** template_keys=(const char **)memalign(16,(VEC_SIZE)*sizeof(const char *)); std::map< std::string,std::vector<std::pair<std::string,size_t> > >::iterator it = pairs.pairs.begin(); for( ; it != pairs.pairs.end(); ++it ){ std::pair<std::string, size_t> query = sequences.get_sequence(it->first); std::string query_key = it->first.c_str(); sequenceQuery.MapOneSEQToSEQ8(query.first.c_str()); std::vector<std::pair<std::string, size_t> > t_seq = it->second; sort (t_seq.begin(), t_seq.end(), sort_seq_vector); int elem_count=0; size_t t_seq_size = t_seq.size(); for(std::vector<int>::size_type i = 0; i < t_seq_size ; i++) { std::string template_key=t_seq[i].first; template_keys[i%VEC_SIZE] = template_key.c_str(); template_sequences[i%VEC_SIZE] = sequences.get_sequence(template_key).first.c_str(); elem_count++; if((i+1)%VEC_SIZE==0){ elem_count=0; sequenceTemplate.MapSEQArrayToSEQ8(template_sequences, VEC_SIZE); GotohEight::GotohMatrix matrix= gotohAlgo.calc_matrix(&sequenceQuery, &sequenceTemplate); if(print_ali == true){ std::vector<char *> alignments=gotohAlgo.backtrace(matrix,&sequenceQuery,&sequenceTemplate); for(size_t i = 0; i < 8; i++) { if(program_args.check==true){ float check_score=gotohAlgo.calc_check_score(alignments[(i*2)],alignments[(i*2)+1]); printf("check score: %.3f\n",check_score); } print_ali_result(alignments[(i*2)],alignments[(i*2)+1],matrix.score[i],query_key,template_keys[i]); } }else{ print_score(matrix,query_key,template_keys,VEC_SIZE); } } } if(elem_count!=0){ sequenceTemplate.MapSEQArrayToSEQ8(template_sequences, elem_count); GotohEight::GotohMatrix matrix= gotohAlgo.calc_matrix(&sequenceQuery, &sequenceTemplate); if(print_ali == true){ std::vector<char *> alignments=gotohAlgo.backtrace(matrix,&sequenceQuery,&sequenceTemplate); for(size_t i = 0; i < elem_count; i++) { if(program_args.check==true){ float check_score=gotohAlgo.calc_check_score(alignments[(i*2)],alignments[(i*2)+1]); printf("check score: %.3f\n",check_score); } print_ali_result(alignments[(i*2)],alignments[(i*2)+1],matrix.score[i],query_key,template_keys[i]); } }else{ print_score(matrix,query_key,template_keys,elem_count); } } } std::cout.flush(); free(template_keys); free(template_sequences); return 0; }
FgMatrixV rowVector(uint n) const {return subMatrix(n,0,1,ncols); };
int main() { char cmd[1000]={},split[5][100]={}; char *token; int cmdSpt=0,i,returnA,returnB,returnTar,returnFunc,row,col,size; double mul; FILE *fileA=NULL,*fileB=NULL,*fileTar=NULL; matrix matA,matB,matTar; //prints welcome messages system("cls"); printf("\n\n===Matrix manipulation program===\n\n"); printf("Type <help> for command list and usage\n\n"); do { printf("MATRIX> "); //command prompt rewind(stdin); //rewind everything scanf("%[^\n]",cmd); //get command (with spaces) cmdSpt=0; //set sub command count to 0 token=strtok(cmd," "); //partition command to subcommand with spaces while(token!=NULL&&cmdSpt<=5) { strcpy(split[cmdSpt],token); //save subcommands to split[] cmdSpt++; //increase sub command count token=strtok(NULL," "); } for(i=0;i<strlen(split[0]);i++) //set command to lowercase split[0][i]=tolower(split[0][i]); if(strcmp(split[0],"show")==0&&cmdSpt==2) //show command { returnA=openFile(&fileA,split[1],0); //call openFile() to open file to show if(returnA==1) //failed to open printf("Error: file %s failed to open.\n\n",split[1]); else { matA=readMatrix(fileA); //read matrix and save to matA showMatrix(matA,split[1]); //show matrix matA } } else if(strcmp(split[0],"create")==0&&cmdSpt==4) //create command { returnA=openFile(&fileA,split[3],1); //call openFile() to create a new file { if(returnA==1) //failed to open printf("Error: file %s failed to open.\n\n",split[3]); else if(sscanf(split[1],"%d",&row)!=1||sscanf(split[2],"%d",&col)!=1) //incorrect matrix size entered printf("Error: invalid matrix size.\n\n"); else if(row>50||col>50) //size too large printf("Error: maximum matrix size is 50x50.\n\n"); else { createMatrix(row,col,fileA); //create a new matrix with specified size printf("%dx%d matrix was successfully saved to %s.\n\n",row,col,split[3]); } } } else if(strcmp(split[0],"copy")==0&&cmdSpt==3) //copy command { returnA=openFile(&fileA,split[1],0); //open source and destination files returnB=openFile(&fileTar,split[2],1); if(returnA==1) //failed to open source printf("Error: file %s failed to open.\n\n",split[1]); if(returnB==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[2]); if(returnA==0&&returnB==0) //passed { matA=readMatrix(fileA); //read source file writeMatrix(matA,fileTar); //write to destination file printf("Matrix from %s is successfully copied to %s.\n\n",split[1],split[2]); } } else if(strcmp(split[0],"add")==0&&cmdSpt==4) //add command { returnA=openFile(&fileA,split[1],0); //open source and destination files returnB=openFile(&fileB,split[2],0); returnTar=openFile(&fileTar,split[3],1); if(returnA==1) //failed to open source A printf("Error: file %s failed to open.\n\n",split[1]); if(returnB==1) //failed to open source B printf("Error: file %s failed to open.\n\n",split[2]); if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[3]); if(returnA==0&&returnB==0&&returnTar==0) //passed { matA=readMatrix(fileA); //read both sources files matB=readMatrix(fileB); returnFunc=addMatrix(matA,matB,fileTar); //add matrices and save to destination if(returnFunc==0) //success { printf("New matrix is successfully saved to %s.\n\n",split[3]); matTar=readMatrix(fileTar); printf("===Base matrix===\n"); showMatrix(matA,split[1]); printf("===Adder matrix===\n"); showMatrix(matB,split[2]); printf("===Result matrix===\n"); showMatrix(matTar,split[3]); } else //dimensions not matched printf("Error: matrix dimensions unmatched.\n\n"); } } else if((strcmp(split[0],"subtract")==0||strcmp(split[0],"sub")==0)&&cmdSpt==4) //subtract command { returnA=openFile(&fileA,split[1],0); //open source and destination files returnB=openFile(&fileB,split[2],0); returnTar=openFile(&fileTar,split[3],1); if(returnA==1) //failed to open source A printf("Error: file %s failed to open.\n\n",split[1]); if(returnB==1) //failed to open source B printf("Error: file %s failed to open.\n\n",split[2]); if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[3]); if(returnA==0&&returnB==0&&returnTar==0) //passed { matA=readMatrix(fileA); //read both sources files matB=readMatrix(fileB); returnFunc=subMatrix(matA,matB,fileTar); //subtract matrices and save to destination if(returnFunc==0) //success { printf("New matrix is successfully saved to %s.\n\n",split[3]); matTar=readMatrix(fileTar); printf("===Base matrix===\n"); showMatrix(matA,split[1]); printf("===Subtractor matrix===\n"); showMatrix(matB,split[2]); printf("===Result matrix===\n"); showMatrix(matTar,split[3]); } else //dimensions not matched printf("Error: matrix dimensions unmatched.\n\n"); } } else if((strcmp(split[0],"multiply")==0||strcmp(split[0],"mul")==0)&&cmdSpt==4) //multiply command { returnA=openFile(&fileA,split[1],0); //open source and destination files returnB=openFile(&fileB,split[2],0); returnTar=openFile(&fileTar,split[3],1); if(returnA==1) //failed to open source A printf("Error: file %s failed to open.\n\n",split[1]); if(returnB==1) //failed to open source B printf("Error: file %s failed to open.\n\n",split[2]); if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[3]); if(returnA==0&&returnB==0&&returnTar==0) //passed { matA=readMatrix(fileA); //read both sources files matB=readMatrix(fileB); returnFunc=mulMatrix(matA,matB,fileTar); //multiply matrices and save to destination if(returnFunc==0) //success { printf("New matrix is successfully saved to %s.\n\n",split[3]); matTar=readMatrix(fileTar); printf("===Base matrix===\n"); showMatrix(matA,split[1]); printf("===Multiplier matrix===\n"); showMatrix(matB,split[2]); printf("===Result matrix===\n"); showMatrix(matTar,split[3]); } else //dimensions not matched printf("Error: matrix dimensions unmatched.\n\n"); } } else if((strcmp(split[0],"mulscalar")==0||strcmp(split[0],"muls")==0)&&cmdSpt==4) //multiply scalar command { returnA=openFile(&fileA,split[2],0); //open source and destination files returnTar=openFile(&fileTar,split[3],1); returnFunc=sscanf(split[1],"%lf",&mul); //convert multiplier to double if(returnA==1) //failed to open source A printf("Error: file %s failed to open.\n\n",split[2]); if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[3]); if(returnFunc!=1) //cannot convert multiplier to double (invalid) printf("Error: invalid multiplier.\n\n"); if(returnA==0&&returnB==0&&returnFunc==1) //passed { matA=readMatrix(fileA); //read source file mulScaMatrix(matA,mul,fileTar); //multiply with scalar and save to destination printf("New matrix is successfully saved to %s.\n\n",split[3]); matTar=readMatrix(fileTar); printf("===Base matrix===\n"); showMatrix(matA,split[2]); printf("Multiply with %g\n\n",mul); printf("===Result matrix===\n"); showMatrix(matTar,split[3]); } } else if((strcmp(split[0],"transpose")==0||strcmp(split[0],"trans")==0)&&cmdSpt==3) //transpose command { returnA=openFile(&fileA,split[1],0); //open source and destination files returnTar=openFile(&fileTar,split[2],1); if(returnA==1) //failed to open source A printf("Error: file %s failed to open.\n\n",split[1]); if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[2]); if(returnA==0&&returnTar==0) //passed { matA=readMatrix(fileA); //read source file transMatrix(matA,fileTar); //transpose matrix and save to destination printf("New matrix is successfully saved to %s.\n\n",split[2]); } } else if((strcmp(split[0],"identity")==0||strcmp(split[0],"iden")==0)&&cmdSpt==3) //identity matrix command { returnTar=openFile(&fileTar,split[2],1); //open destination file returnFunc=sscanf(split[1],"%d",&size); //convert size to integer if(returnTar==1) //failed to open destination printf("Error: file %s failed to open.\n\n",split[2]); else if(returnFunc!=1) //failed to convert size to integer (invalid) printf("Error: invalid matrix size.\n\n"); else if(size>50) //size too large printf("Error: maximum matrix size is 50x50.\n\n"); else { idenMatrix(size,fileTar); //create identity matrix with specified size and save to destination printf("Identity matrix of size %d is successfully saved to %s.\n\n",size,split[2]); } } else if(strcmp(split[0],"help")==0&&cmdSpt==1) //help command { displayHelp(); //display help } else if((strcmp(split[0],"clear")==0||strcmp(split[0],"cls")==0)&&cmdSpt==1) //clear screen command { system("cls"); //clear screen } else if((strcmp(split[0],"exit")==0||strcmp(split[0],"end")==0)&&cmdSpt==1) //exit command { printf("Exit\n"); //display exit message } else //invalid command { printf("Invalid command or incorrect command syntax, type <help> for command list and usage.\n\n"); } if(fileA!=NULL) //close all file pointers in case they weren't closed by functions fclose(fileA); if(fileB!=NULL) fclose(fileB); if(fileTar!=NULL) fclose(fileTar); } while(strcmp(cmd,"exit")!=0&&strcmp(cmd,"end")!=0); //loop until exit return 0; }
double ObjectiveFunction::getDeterminant(Matrix<double> matrix) { double determinant = 0.0; int numberOfRows = matrix.getNumberOfRows(); int numberOfColumns = matrix.getNumberOfColumns(); if(numberOfRows != numberOfColumns) { std::cout << "Error: NewtonMethod class. " << "getDeterminant(Matrix<double>) method." << std::endl << "Matrix must be square" << std::endl << std::endl; exit(1); } if(numberOfRows == 0) { std::cout << "Error: NewtonMethod class. " << "getDeterminant(Matrix<double>) method." << std::endl << "Size of matrix is zero." << std::endl << std::endl; exit(1); } else if(numberOfRows == 1) { // std::cout << "Warning: NewtonMethod class. " // << "getDeterminant(Matrix<double>) method." << std::endl // << "Size of matrix is one." << std::endl; determinant = matrix[0][0]; } else if(numberOfRows == 2) { determinant = matrix[0][0]*matrix[1][1] - matrix[1][0]*matrix[0][1]; } else { for(int j1 = 0; j1 < numberOfRows; j1++) { Matrix<double> subMatrix(numberOfRows-1, numberOfColumns-1, 0.0); for(int i = 1; i < numberOfRows; i++) { int j2 = 0; for (int j = 0; j < numberOfColumns; j++) { if (j == j1) { continue; } subMatrix[i-1][j2] = matrix[i][j]; j2++; } } determinant += pow(-1.0, j1+2.0)*matrix[0][j1]*getDeterminant(subMatrix); } } return(determinant); }
float Matrix4::det() { return mat[0][0]*subMatrix(0,0).det() - mat[1][0]*subMatrix(1,0).det() + mat[2][0]*subMatrix(2,0).det() - mat[3][0]*subMatrix(3,0).det(); }
int kalman_function(sensor_struct_t* sen,float_j_t dt){ /* * * system model * omega = [ w_x w_y w_z ]^T * x = [pi theta psi]^T * used x_dot, C_inv, omega, x_k_1, x_k, dt, * * */ #define Def_x 0 #define Def_y 1 #define Def_z 2 #define Def_pi 0 #define Def_theta 1 #define Def_psi 2 //float_j_t dt; //float_j_t B[3][3]={0,}; float_j_t res[3][3]={0,}; float_j_t res2[3][3]={0,}; float_j_t omega[3][3]={0,}; omega[0][0] = sen->gyroX*DEG_TO_RAD; omega[1][0] = sen->gyroY*DEG_TO_RAD; omega[2][0] = sen->gyroZ*DEG_TO_RAD; static float_j_t x_k_1[3][3]={0,}; float_j_t x_k_p[3][3]={0,}; float_j_t pi,theta,psi; pi = x_k_1[Def_pi][0]; theta = x_k_1[Def_theta][0]; psi = x_k_1[Def_psi][0]; if(cosf(theta)==0.0){ printf("cosf error\n "); theta += 0.00001; } float_j_t C_inv[3][3]={0,}; C_inv[0][0] = 1*dt; C_inv[0][1] = sinf(pi)*tanf(theta)*dt; C_inv[0][2] = cosf(pi)*tanf(theta)*dt; C_inv[1][0] = 0; C_inv[1][1] = cosf(pi)*dt; C_inv[1][2] = -sinf(pi)*dt; C_inv[2][0] = 0; C_inv[2][1] = (sinf(pi)/cosf(theta))*dt; C_inv[2][2] = (cosf(pi)/cosf(theta))*dt; //x_k = x_k_1 + C_inv * omega * dt multiMatrix(C_inv,omega,res,3,3); sumMatrix(res,x_k_1,x_k_p,3,3); /* A_k*/ float_j_t A[3][3]={0,}; A[0][0] = 1 + (sinf(theta)*(omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt; A[0][1] = ((omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt; A[0][2] = 0; A[1][0] = (-omega[1][0]*sinf(pi)-omega[2][0]*cosf(pi))*dt; A[1][1] = 1; A[1][2] = 0; A[2][0] = ((omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt; A[2][1] = (sinf(theta)*(omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt; A[2][2] = 1; float_j_t P_k_p[3][3]={0,}; static float_j_t P_k_1[3][3]={1,0,0,0,1,0,0,0,1}; float_j_t Q[3][3]={1,0,0,0,1,0,0,0,1}; //P_k = A* P_k_1 * A^T + B * Q * B^T similarMatrix(A,P_k_1,res,3,3); similarMatrix(C_inv,Q,res2,3,3); sumMatrix(res,res2,P_k_p,3,3); //Correct term float_j_t H[3][3]={1,0,0,0,1,0,0,0,0}; float_j_t norm_a=9.8; norm_a=sqrtf(powf(sen->accX,2)+powf(sen->accY,2)+powf(sen->accZ,2)); // printf("norm_a = %f\n",norm_a); float_j_t R[3][3]={0,}; if((norm_a<10.8 )&&(norm_a>8.8)){ R[0][0]=1; R[1][1]=1; }else{ R[0][0]=1000; R[1][1]=1000; } float_j_t res3[3][3] = {0,}; similarMatrix(H,P_k_p,res,3,3); //printf("similarMatrix res\n"); //printMatrix(res,3,3); sumMatrix(res,R,res2,3,3); _2by2_inverseMatrix(res2,res,3,3); //inverseMatrix(res2,res,3,3); //result inverse matrix is res //so kalman gain K = Pk* H^T* res float_j_t K[3][3]={0,}; transMatrix(H,res3,3,3); multiMatrix(P_k_p,res3,res2,3,3); res2[0][2]=0; res2[1][2]=0; res2[2][0]=0; res2[2][1]=0; res2[2][2]=0; // printf("res2 gain\n"); // printMatrix(res2,3,3); // printf("res gain\n"); // printMatrix(res,3,3); multiMatrix(res2,res,K,3,3); // printf("kalman gain\n"); // printMatrix(K,3,3); /* state variable, covaliance mat update*/ float_j_t x_k[3][3]={0,}; //calculate z_k roll, ptich, with gravitational euler angle calc. float_j_t z_k[3][3] ={0,}; #if 1 z_k[0][0] = atan2f(sen->accY,sen->accZ); z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) )); #else if(sen->accZ>=0){ z_k[0][0] = atan2f(sen->accY,sen->accZ); z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) )); }else if(sen->accZ<0){ omega[1][0] = -omega[1][0]; z_k[1][0] = -atan2f(sen->accX,sen->accZ); z_k[0][0] = atan2f(-sen->accY, sqrtf( powf(sen->accX,2)+powf(sen->accZ,2) )); } #endif //GravityToEuler(sen->accX, sen->accY, sen->accZ, z_k); //printf("r %f p %f ",z_k[0][0]*180/3.14,z_k[1][0]*180/3.14); multiMatrix(H,x_k_p,res,3,3); subMatrix(z_k,res,res2,3,3); multiMatrix(K,res2,res,3,3); sumMatrix(x_k_p,res,x_k,3,3); float_j_t P_k[3][3]={0,}; multiMatrix(K,H,res,3,3); multiMatrix(res,P_k_p,res2,3,3); subMatrix(P_k_p,res2,P_k,3,3); /*insert prev value*/ insertMatrix(x_k, x_k_1,3,3); insertMatrix(P_k, P_k_1,3,3); /*Debug print*/ //printf("roll = %f , pitch = %f \n",x_k[0][0]*180.0/3.14,x_k[1][0]*180.0/3.14); sen->roll=x_k[0][0]*RAD_TO_DEG; sen->pitch=x_k[1][0]*RAD_TO_DEG; return 0; }