/* (memo: mpartの大きさでrows,colsを指定するのもアリ…だがとりあえず分かり易さ重視) */ mat_t* robolib_matrix_copy_part(mat_t* m, int irow, int icol, int rows, int cols, mat_t* mpart) { if (robolib_is_null_matrix(m) || m->rows < irow+rows || m->cols < icol+cols ) return NULL; mat_t* mhat; if (robolib_matrix_is_null(mpart)){ mpart = robolib_matrix_alloc(rows, cols); } else if (robolib_matrix_is_empty(mpart) || mpart->rows != rows || mpart->cols != cols) { // robolib_matrix_free(mpart); robolib_matrix_realloc(mpart,rows, cols); } mhat = robolib_matrix_alloc(rows, cols); int i,j; for (i=0;i<rows;i++) { for (j=0;j<cols;j++) { robolib_matrix_set(mhat, i, j, robolib_matrix_get(m, irow+i, icol+j)); } } robolib_matrix_copy(mhat, mpart); robolib_matrix_free(mhat); return mpart; }
// 注意 // よくわかんないけどNULL渡す方がよいかも mat_t* robolib_matrix_arr_average(mat_t** arrm, int dim, mat_t* mave) { if (robolib_is_null_matrix(arrm[0])) return NULL; //このifをつけるとcopy(mhat, mave)がうまくいかなくなるっぽい? // if (robolib_is_null_matrix(mave)) { //robolib_matrix_new_zero(arrm[0]->rows,arrm[0]->cols,mave); // いや原因は↑か? mave = robolib_matrix_new_zero(arrm[0]->rows,arrm[0]->cols,NULL); // } mat_t* mhat; mhat = robolib_matrix_new_zero(arrm[0]->rows,arrm[0]->cols,NULL); int i,j; for (i=1;i<dim;i++) { if (!robolib_matrix_is_same_size(arrm[0],arrm[i])) return NULL; } for (j=0;j<(mhat->rows * mhat->cols);j++){ for (i=0;i<dim;i++) { mhat->val[j] += arrm[i]->val[j]; } mhat->val[j] /= dim; } robolib_matrix_copy(mhat, mave); robolib_matrix_free(mhat); return mave; }
mat_t* robolib_matrix_linear_comb(double a, mat_t* ma, double b, mat_t* mb, mat_t* mc) { if (robolib_is_null_matrix(ma)||robolib_is_null_matrix(mb)){ return NULL; } if (!robolib_matrix_is_same_size(ma,mb)) return NULL; if (robolib_matrix_is_null(mc)){ mc = robolib_matrix_alloc(ma->rows,ma->cols); } else if (!robolib_matrix_is_empty(mc) && robolib_matrix_is_same_size(ma,mc)) { } else { mc = robolib_matrix_realloc(mc,ma->rows,ma->cols); } mat_t* mhat; mhat = robolib_matrix_alloc(ma->rows,ma->cols); int i; for (i=0; i< ma->rows*ma->cols; i++) { mhat->val[i] = a * ma->val[i] + b * mb->val[i]; } robolib_matrix_copy(mhat, mc); robolib_matrix_free(mhat); return mc; }
mat_t* robolib_matrix_new_unit(int nrc) { if (nrc<=0) return NULL; // if (robolib_is_null_matrix(m) || m->rows!=m->cols) { // m = robolib_matrix_alloc(nrc,nrc); // } else if (nrc!=m->rows || nrc!=m->cols) { // if (robolib_is_null_matrix(m)) // これチェックするとセグフォ起こることがある。 // freeした後で0xffffffffになってた場合。 // robolib_matrix_free(m); // freeも駄目だった。これはminvを消すしかない. mat_t* m; m = robolib_matrix_alloc(nrc,nrc); // } mat_t* mhat; mhat = robolib_matrix_alloc(nrc, nrc); int i; for (i=0; i<nrc*nrc; i++) { if (i/nrc == i%nrc) robolib_matrix_set(mhat,i/nrc,i%nrc,1.0); else robolib_matrix_set(mhat,i/nrc,i%nrc,0.0); } robolib_matrix_copy(mhat, m); robolib_matrix_free(mhat); return m; }
mat_t* robolib_matrix_new_zero(int rows, int cols, mat_t* m){ if (rows<=0||cols<=0) return NULL; if (robolib_matrix_is_null(m)){ robolib_matrix_free(m); m = robolib_matrix_alloc(rows,cols); } mat_t* mhat; mhat = robolib_matrix_alloc(rows,cols); #ifdef predDEBUG if (robolib_matrix_is_null(mhat)) printf("matrix allocation error"); #endif int i; for (i=0;i<rows*cols;i++) { robolib_matrix_set(mhat,i/cols,i%cols,0); } robolib_matrix_copy(mhat, m); robolib_matrix_free(mhat); return m; }
int kfsys_init(kfsys_t* kfsys, int sdim, int mdim, int cdim, double time, double dt, const mat_t* H, const mat_t* initX, const mat_t* initP, const mat_t* Q) { kfsys->sdim = sdim; kfsys->mdim = mdim; kfsys->cdim = cdim; kfsys->time = time; kfsys->dt = dt; kfsys->X = robolib_column_vector_alloc(sdim); kfsys->X = robolib_matrix_copy(initX, kfsys->X); kfsys->P = robolib_matrix_alloc(sdim, sdim); kfsys->P = robolib_matrix_copy(initP, kfsys->P); kfsys->Q = robolib_matrix_alloc(sdim, sdim); kfsys->Q = robolib_matrix_copy(Q, kfsys->Q); kfsys->H = robolib_matrix_alloc(mdim, mdim); kfsys->H = robolib_matrix_copy(H, kfsys->H); kfsys->preX = robolib_column_vector_alloc(kfsys->sdim); kfsys->preP = robolib_matrix_alloc(kfsys->sdim, kfsys->sdim); kfsys->K = robolib_matrix_alloc(kfsys->sdim, kfsys->mdim); return 0; }
mat_t* robolib_matrix_inverse(mat_t* m, mat_t* minv) { if (robolib_is_null_matrix(m)||m->rows!=m->cols) return NULL; int i,j; int maxi; int nrc; nrc = m->rows; mat_t* mtmp = robolib_matrix_alloc(nrc, nrc); mat_t* vtmp = robolib_vector_alloc(nrc); double tmp; robolib_matrix_copy(m, mtmp); minv = robolib_matrix_new_unit(nrc); if (mtmp==NULL || minv==NULL) return NULL; for (i=0; i<nrc; i++) { // maxi = 0; あれっこれ違… maxi = i; for (j=i+1; j<nrc; j++) { if(myABS(robolib_matrix_get(mtmp,maxi,i)) < myABS(robolib_matrix_get(mtmp,j,i))) maxi = j; } if (robolib_matrix_get(mtmp,maxi,i) == 0.0) { printf("input matrix is invalid!\n"); break; } // 行入れ替え robolib_matrix_copy_row(mtmp, i, vtmp, 0); robolib_matrix_copy_row(mtmp, maxi, mtmp, i); // robolib_matrix_copy_row(vtmp, 0, mtmp, i); robolib_matrix_copy_row(vtmp, 0, mtmp, maxi); // 行入れ替え robolib_matrix_copy_row(minv, i, vtmp, 0); robolib_matrix_copy_row(minv, maxi, minv, i); // robolib_matrix_copy_row(vtmp, 0, minv, i); robolib_matrix_copy_row(vtmp, 0, minv, maxi); tmp = robolib_matrix_get(mtmp,i,i); for (j=0; j<nrc; j++) { // j:列番号 //*(mtmp->val+i*mtmp->cols+j) /= tmp; //*(minv->val+i*mtmp->cols+j) /= tmp; robolib_matrix_set(mtmp, i, j, robolib_matrix_get(mtmp, i, j)/tmp); robolib_matrix_set(minv, i, j, robolib_matrix_get(minv, i, j)/tmp); // printf("%s : %d\n",__FILE__,__LINE__); // robolib_matrix_print(mtmp); } for (j=0; j<nrc; j++) { // j:行番号 if (i!=j) { // tmp = robolib_matrix_get(m,j,i); tmp = robolib_matrix_get(mtmp,j,i); robolib_matrix_row_linear_comb(-tmp, mtmp, i, 1.0, mtmp, j, mtmp, j); robolib_matrix_row_linear_comb(-tmp, minv, i, 1.0, minv, j, minv, j); // printf("%s : %d\n",__FILE__,__LINE__); // robolib_matrix_print(mtmp); } } } robolib_vector_free(vtmp); robolib_matrix_free(mtmp); return minv; }