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;
}
/* (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;
}
int kfsys_free(kfsys_t* kfsys)
{
robolib_matrix_free(kfsys->X);
robolib_matrix_free(kfsys->P);
robolib_matrix_free(kfsys->Q);
robolib_matrix_free(kfsys->H);
robolib_matrix_free(kfsys->K);
robolib_matrix_free(kfsys->preX);
robolib_matrix_free(kfsys->preP);
return 0;
}
int kfsys_revise(kfsys_t* kfsys, const mat_t* A, const mat_t* B, const mat_t* U, const mat_t* Z, const mat_t* R)
{
int i, j;
mat_t* tmp = robolib_matrix_alloc(0, 0);
mat_t* tmp2 = robolib_matrix_alloc(0, 0);
// Project the state ahead
tmp = robolib_matrix_product(A, kfsys->X, tmp);
// fprintf(stderr, "1 tmp: %p\n", tmp);
tmp2 = robolib_matrix_product(B, U, tmp2);
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
kfsys->preX = robolib_matrix_linear_comb(1.0, tmp, 1.0, tmp2, kfsys->preX);
// fprintf(stderr, "2 preX: %p\n", kfsys->preX);
// Project the error covariance ahead
tmp = robolib_matrix_trans(A, tmp);
// fprintf(stderr, "1 tmp: %p\n", tmp);
tmp2 = robolib_matrix_product(kfsys->P, tmp, tmp2);
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
tmp = robolib_matrix_product(A, tmp2, tmp);
// fprintf(stderr, "1 tmp: %p\n", tmp);
kfsys->preP = robolib_matrix_linear_comb(1.0, tmp, 1.0, kfsys->Q, kfsys->preP);
// fprintf(stderr, "2 preX: %p\n", kfsys->preX);
// Compute the Karman gain
mat_t* Ht = robolib_matrix_alloc(kfsys->sdim, kfsys->mdim);
Ht = robolib_matrix_trans(kfsys->H, Ht);
tmp2 = robolib_matrix_product(kfsys->preP, Ht, tmp2); // prePHt
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
tmp = robolib_matrix_product(kfsys->H, tmp2, tmp); // HprePHt
// fprintf(stderr, "1 tmp: %p\n", tmp);
tmp2 = robolib_matrix_linear_comb(1.0, tmp, 1.0, R, tmp2); // HprePHt+R
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
tmp = robolib_matrix_inverse(tmp2, tmp); // inv(HprePHt+R)
// fprintf(stderr, "1 tmp: %p\n", tmp);
tmp2 = robolib_matrix_product(Ht, tmp, tmp2); // Htinv(HprePHt+R)
robolib_matrix_print(Ht);
robolib_matrix_print(tmp);
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
kfsys->K = robolib_matrix_product(kfsys->preP, tmp2, kfsys->K); // prePHtinv(HprePHt+R)
// fprintf(stderr, "2 K: %p\n", kfsys->K);
robolib_matrix_free(Ht);
// Update estimate with measurement Z
tmp = robolib_matrix_product(kfsys->H, kfsys->preX, tmp);
// fprintf(stderr, "2 tmp: %p\n", tmp);
tmp2 = robolib_matrix_linear_comb(1.0, Z, -1.0, tmp, tmp2);
// fprintf(stderr, "2 tmp2: %p\n", tmp2);
tmp = robolib_matrix_product(kfsys->K, tmp2, tmp);
// fprintf(stderr, "2 tmp: %p\n", tmp);
kfsys->X = robolib_matrix_linear_comb(1.0, tmp, 1.0, kfsys->preX, kfsys->X);
// fprintf(stderr, "3 kfsys->X: %p\n", kfsys->X);
// Update the error covariance
mat_t* I = robolib_matrix_new_unit(kfsys->sdim);
tmp = robolib_matrix_product(kfsys->K, kfsys->H, tmp);
// fprintf(stderr, "1 tmp: %p\n", tmp);
tmp2 = robolib_matrix_linear_comb(1.0, I, -1.0, tmp, tmp2);
kfsys->P = robolib_matrix_product(tmp2, kfsys->preP, kfsys->P);
robolib_matrix_free(I);
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;
}