void estimate(KalmanFilter f) { /* Calculate innovation */ multiply_matrix(f.observation_model, f.predicted_state, f.innovation); subtract_matrix(f.observation, f.innovation, f.innovation); /* Calculate innovation covariance */ multiply_by_transpose_matrix(f.predicted_estimate_covariance, f.observation_model, f.vertical_scratch); multiply_matrix(f.observation_model, f.vertical_scratch, f.innovation_covariance); add_matrix(f.innovation_covariance, f.observation_noise_covariance, f.innovation_covariance); /* Invert the innovation covariance. Note: this destroys the innovation covariance. TODO: handle inversion failure intelligently. */ destructive_invert_matrix(f.innovation_covariance, f.inverse_innovation_covariance); /* Calculate the optimal Kalman gain. Note we still have a useful partial product in vertical scratch from the innovation covariance. */ multiply_matrix(f.vertical_scratch, f.inverse_innovation_covariance, f.optimal_gain); /* Estimate the state */ multiply_matrix(f.optimal_gain, f.innovation, f.state_estimate); add_matrix(f.state_estimate, f.predicted_state, f.state_estimate); /* Estimate the state covariance */ multiply_matrix(f.optimal_gain, f.observation_model, f.big_square_scratch); subtract_from_identity_matrix(f.big_square_scratch); multiply_matrix(f.big_square_scratch, f.predicted_estimate_covariance, f.estimate_covariance); }
static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims) { double **src_mat = NULL; double **src_mat_T = NULL; double **dest_mat = NULL; double **dest_mat_T = NULL; double **src_dest_mat = NULL; double *S_vec = NULL; double **R_mat = NULL; double **R_mat_T = NULL; double **mat_mn1 = NULL; double **mat_mn2 = NULL; double **mat_nm1 = NULL; double **mat_nm2 = NULL; double **mat_nn1 = NULL; double **E_mat = NULL; double **P_mat = NULL; double **Q_mat = NULL; double *D_vec = NULL; double *one_vec = NULL; double trace1 = 0.0; double trace2 = 0.0; int numactive; /* NUMBER OF ACTIVE CONTROL POINTS */ int m, n, i, j; int status; /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */ for (i = numactive = 0; i < cp->count; i++) { if (cp->status[i] > 0) numactive++; } m = numactive; n = ndims; src_mat = G_alloc_matrix(m, n); dest_mat = G_alloc_matrix(m, n); for (i = numactive = 0; i < cp->count; i++) { if (cp->status[i] > 0) { src_mat[numactive][0] = cp->e1[i]; src_mat[numactive][1] = cp->n1[i]; src_mat[numactive][2] = cp->z1[i]; dest_mat[numactive][0] = cp->e2[i]; dest_mat[numactive][1] = cp->n2[i]; dest_mat[numactive][2] = cp->z2[i]; numactive++; } } D_vec = G_alloc_vector(ndims); src_mat_T = G_alloc_matrix(n, m); dest_mat_T = G_alloc_matrix(n, m); src_dest_mat = G_alloc_matrix(n, n); R_mat = G_alloc_matrix(n, n); R_mat_T = G_alloc_matrix(n, n); mat_mn1 = G_alloc_matrix(m, n); mat_mn2 = G_alloc_matrix(m, n); mat_nm1 = G_alloc_matrix(n, m); mat_nm2 = G_alloc_matrix(n, m); mat_nn1 = G_alloc_matrix(n, n); E_mat = G_alloc_matrix(m, m); P_mat = G_alloc_matrix(ndims, ndims); Q_mat = G_alloc_matrix(ndims, ndims); transpose_matrix(m, n, dest_mat, dest_mat_T); for (i = 0; i < m; i++) { for (j = 0; j < m; j++) { if (i != j) { E_mat[i][j] = -1.0 / (double)m; } else{ E_mat[i][j] = 1.0 - 1.0 / (double)m; } } } matmult(n, m, m, dest_mat_T, E_mat, mat_nm1); matmult(n, m, n, mat_nm1, src_mat, src_dest_mat); copy_matrix(n, n, src_dest_mat, P_mat); copy_matrix(n, n, src_dest_mat, mat_nn1); status = G_math_svduv(D_vec, mat_nn1, P_mat, n, Q_mat, n); if (status == 0) status = MSUCCESS; transpose_matrix(n, n, P_mat, mat_nn1); /* rotation matrix */ matmult(n, n, n, Q_mat, mat_nn1, R_mat_T); transpose_matrix(n, n, R_mat_T, R_mat); /* scale */ matmult(n, n, n, src_dest_mat, R_mat_T, mat_nn1); trace1 = trace(n, n, mat_nn1); transpose_matrix(m, n, src_mat, src_mat_T); matmult(n, m, m, src_mat_T, E_mat, mat_nm1); matmult(n, m, n, mat_nm1, src_mat, mat_nn1); trace2 = trace(n, n, mat_nn1); OR[14] = trace1 / trace2; /* shifts */ matmult(m, n, n, src_mat, R_mat_T, mat_mn1); scale_matrix(m, n, OR[14], mat_mn1, mat_mn2); subtract_matrix(m, n, dest_mat, mat_mn2, mat_mn1); scale_matrix(m, n, 1.0 / m, mat_mn1, mat_mn2); transpose_matrix(m, n, mat_mn2, mat_nm1); S_vec = G_alloc_vector(n); one_vec = G_alloc_vector(m); for (i = 0; i < m; i++){ one_vec[i] = 1.0; } matrix_multiply(n, m, mat_nm1, one_vec, S_vec); /* matrix to vector */ for (i = 0; i < ndims; i++) { for (j = 0; j < ndims; j++) { OR[i * ndims + j] = R_mat[i][j]; } } G_free_matrix(src_mat); G_free_matrix(src_mat_T); G_free_matrix(dest_mat); G_free_matrix(dest_mat_T); G_free_matrix(src_dest_mat); G_free_vector(D_vec); G_free_matrix(E_mat); G_free_matrix(P_mat); G_free_matrix(Q_mat); G_free_matrix(R_mat); G_free_matrix(R_mat_T); G_free_matrix(mat_mn1); G_free_matrix(mat_mn2); G_free_matrix(mat_nm1); G_free_matrix(mat_nm2); G_free_matrix(mat_nn1); G_free_vector(S_vec); G_free_vector(one_vec); return status; }
int transform(const char * source, const char * destination, const char * output){ char src_pts_name[256]; char dest_pts_name[256]; char out_param_name[256]; int n=3; int m=0; int m2=0; int k,l; double **src_mat=NULL; double **dest_mat=NULL; double **dest_mat_T=NULL; double **src_mat_T=NULL; double **E_mat=NULL; double **C_mat=NULL; double **C_mat_interm=NULL; double **D_mat_interm=NULL; double **P_mat=NULL; double *D_vec=NULL; double *T_vec=NULL; double *one_vec=NULL; double **D_mat=NULL; double **Q_mat=NULL; double **P_mat_T=NULL; double **R_mat=NULL; double trace1=0.0; double trace2=0.0; double scal=0.0; double ppm=0.0; FILE *outfile; printf("\n*******************************\n"); printf( "* helmparms3d v%1.2f *\n",VERS); printf( "* (c) U. Niethammer 2011 *\n"); printf( "* http://helmparms3d.sf.net *\n"); printf( "*******************************\n"); memset(src_pts_name,0,sizeof(src_pts_name)); memset(dest_pts_name,0,sizeof(dest_pts_name)); memset(out_param_name,0,sizeof(out_param_name)); strcpy(src_pts_name, source); strcpy(dest_pts_name, destination); strcpy(out_param_name, output); m=get_m_size(src_pts_name); m2=get_m_size(dest_pts_name); if(m2!=m){ printf("Error, number of source and destination points is not equal!\n"); } else { src_mat=matrix(m,m, src_mat); dest_mat=matrix(m,m, dest_mat); read_points(src_pts_name, src_mat); read_points(dest_pts_name, dest_mat); D_vec=vector(n, D_vec); E_mat=matrix(m, m, E_mat); P_mat=matrix(m, m, P_mat); D_mat=matrix(m, m, D_mat); Q_mat=matrix(m, m, Q_mat); P_mat_T=matrix(m, m, P_mat_T); R_mat=matrix(m, m, R_mat); dest_mat_T=matrix(m, m, dest_mat_T); C_mat=matrix(m, m, C_mat); C_mat_interm=matrix(m, m, C_mat_interm); src_mat_T=matrix(m, m, src_mat_T); D_mat_interm=matrix(m, m, D_mat_interm); transpose_matrix(m, m, dest_mat, dest_mat_T); if(debug)printf("%s_T:\n",dest_pts_name); if(debug)plot_matrix(stdout, n, m, dest_mat_T); for(k=0;k<m;k++){ for(l=0;l<m;l++){ if(k!=l){ E_mat[k][l]=-1.0/(double)m; } else{ E_mat[k][l]=1.0-1.0/(double)m; } } } if(debug)printf("E:\n"); if(debug)plot_matrix(stdout, m, m, E_mat); if(debug)printf("dest_mat_T:\n"); if(debug)plot_matrix(stdout, n, m, dest_mat_T); matmult(dest_mat_T, m, m, E_mat, m, m, C_mat_interm, m, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, m, C_mat_interm); matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n); if(debug)printf("C:\n"); if(debug)plot_matrix(stdout, n, n, C_mat); copy_matrix(n,n,C_mat,P_mat); if(debug)printf("P:\n"); if(debug)plot_matrix(stdout, n, n, P_mat); //Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n]. svd(n, n, C_mat, P_mat, D_vec, Q_mat); transpose_matrix(n, n, P_mat, P_mat_T); if(debug)printf("P\n"); if(debug)plot_matrix(stdout, n, n, P_mat); if(debug)printf("P_T\n"); if(debug)plot_matrix(stdout, n, n, P_mat_T); if(debug)printf("D_vec\n"); if(debug)plot_vector(stdout, n, D_vec); for(k=0;k<n;k++){ for(l=0;l<n;l++){ D_mat[k][l]=0.0; D_mat[l][l]=D_vec[l]; } } if(debug)printf("D\n"); if(debug)plot_matrix(stdout, n, n, D_mat); matmult(Q_mat, n, n, P_mat_T, n, n, R_mat, n, n); if(debug)printf("R_trans:\n"); if(debug)plot_matrix(stdout, n, n, R_mat); matmult(C_mat, m, n, R_mat, n, m, C_mat_interm, m, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, n, C_mat_interm); trace1=trace(n,n,C_mat_interm); if(debug)printf("\ntra=%lf\n\n",trace1); transpose_matrix(m, m, src_mat, src_mat_T); if(debug)printf("%s_T:\n",src_pts_name); if(debug)plot_matrix(stdout, n, m, src_mat_T); init_matrix(m,m,C_mat); init_matrix(m,m,C_mat_interm); matmult(src_mat_T, m, m, E_mat, m, m, C_mat_interm, n, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, m, C_mat_interm); matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n); if(debug)printf("C:\n"); if(debug)plot_matrix(stdout, n, n, C_mat); trace2=trace(n,n,C_mat); if(debug)printf("\ntra=%lf\n\n",trace2); scal=trace1/trace2; ppm=scal-1.0; if(debug)printf("\nscal = %10.10lf\nscal = %10.10lf ppm\n\n",scal, ppm); init_matrix(m,m,C_mat); init_matrix(m,m,C_mat_interm); matmult(src_mat, m, n, R_mat, n,m, D_mat_interm, m, n); if(debug)printf("C_mat_interm:\n"); if(debug)plot_matrix(stdout, m, n, D_mat_interm); scal_matrix(m, n, scal, D_mat_interm, C_mat_interm); if(debug)printf("C_mat_interm:\n"); if(debug)plot_matrix(stdout, m, n, C_mat_interm); subtract_matrix(m, n, dest_mat, C_mat_interm, D_mat_interm); if(debug)plot_matrix(stdout, m, n, D_mat_interm); scal_matrix(m, n, 1.0/m, D_mat_interm, C_mat_interm); if(debug)plot_matrix(stdout, m, n, C_mat_interm); init_matrix(m,m,src_mat_T); transpose_matrix(m, m, C_mat_interm, src_mat_T); if(debug)plot_matrix(stdout, n, m, src_mat_T); T_vec=vector(m, T_vec); one_vec=vector(m, one_vec); for(k=0;k<m;k++){ one_vec[k]=1.0; } matrix_multiply(n, m, src_mat_T, one_vec, T_vec); if(debug)printf("T:\n"); if(debug)plot_vector(stdout, 3, T_vec); outfile = fopen(out_param_name, "w"); if(outfile == NULL){ printf("Error writing %s\r\n",out_param_name); exit(-1); } init_matrix(m,m,src_mat_T); transpose_matrix(m, m, R_mat, src_mat_T); plot_matrix(outfile, n, n, src_mat_T); printf("R =\n");fflush(stdout); plot_matrix(stdout, n, n, src_mat_T); printf("\n");fflush(stdout); plot_vector(outfile, 3, T_vec); printf("T =\n");fflush(stdout); plot_vector(stdout, 3, T_vec); printf("\n");fflush(stdout); fprintf(outfile, "%10.10lf\n", scal); printf("s = %10.10lf (= %10.10lf ppm)\n\n",scal, ppm);fflush(stdout); fclose(outfile); freevector(D_vec); freevector(T_vec); freevector(one_vec); freematrix(m, src_mat); freematrix(m, dest_mat); freematrix(m, E_mat); freematrix(m, P_mat); freematrix(m, D_mat); freematrix(m, Q_mat); freematrix(m, P_mat_T); freematrix(m, R_mat); freematrix(m, dest_mat_T); freematrix(m, C_mat); freematrix(m, C_mat_interm); freematrix(m, src_mat_T); freematrix(m, D_mat_interm); printf("\n...done\n"); } }