/* (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;
}
