Example #1
0
/* (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;
}
Example #2
0
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;
}
Example #3
0
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;
}
Example #4
0
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;
}
Example #5
0
/* 
 * 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;
}
Example #6
0
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;
}
Example #7
0
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;
}
Example #8
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;
}
Example #9
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;
}
Example #10
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;
	
}