/* (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;
}
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;
}
/*
* mc=ma*mb
*/
mat_t* robolib_matrix_product(mat_t* ma, mat_t* mb, mat_t* mc)
{
if (robolib_is_null_matrix(ma) || robolib_is_null_matrix(mb) || ma->cols != mb->rows) {
return NULL;
}
if (robolib_matrix_is_null(mc)) {
mc = robolib_matrix_alloc(ma->rows,mb->cols);
} else if (!robolib_matrix_is_empty(mc) && mc->rows == ma->rows && mc->cols == mb->cols) {
} else {
mc = robolib_matrix_realloc(mc,ma->rows,mb->cols);
}
int i,j,k;
for(i=0;i<ma->rows;i++){
for(j=0;j<mb->cols;j++){
double tmp = 0;
for (k=0; k<ma->cols; k++) {
tmp += robolib_matrix_get(ma,i,k) * robolib_matrix_get(mb,k,j);
}
robolib_matrix_set(mc,i,j,tmp);
}
}
return mc;
}
mat_t* robolib_matrix_trans(mat_t* m, mat_t* mt)
{
if (robolib_is_null_matrix(m)) {
return NULL;
}
if (mt == NULL){
mt = robolib_matrix_alloc(m->cols,m->rows);
} else if (!robolib_matrix_is_empty(mt) && m->rows==mt->cols && m->cols==mt->rows) {
} else {
// 行列のサイズがおかしい
mt = robolib_matrix_realloc(mt,m->cols,m->rows);
}
int i,j;
// mat_t* mhat;
// mhat = robolib_matrix_alloc(m->rows,m->cols);
for(i=0;i<m->rows;i++){
for(j=0;j<m->cols;j++){
robolib_matrix_set(mt,j,i,robolib_matrix_get(m,i,j));
}
}
// robolib_matrix_copy(mhat,mt);
// robolib_matrix_free(mhat);
return mt;
}
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_copy(mat_t* ma, mat_t* mb)
{
if (robolib_is_null_matrix(ma)) {
return NULL;
}
if (robolib_matrix_is_null(mb)){
mb = robolib_matrix_alloc(ma->rows,ma->cols);
} else if (robolib_matrix_is_empty(mb)||!robolib_matrix_is_same_size(ma,mb)) {
robolib_matrix_realloc(mb, ma->rows, ma->cols);
}
memcpy(mb->val, ma->val, ma->rows * ma->cols * sizeof *ma->val);
return mb;
}
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;
}
