void nested_mpi::take_step(){ for (int i=0;i<numProcs;++i){ int ii=pre_log_posterior.min_x(); if (cpu_log_posterior.x_data(i) > min_log_like){ for(int j=0;j<nb_var;++j) {param_vector[j]=pre_chain[j].x_data(ii);} for(int j=0;j<nb_var;++j) {chains[j].set(param_vector[j]);} log_posterior.set(pre_log_posterior.x_data(ii)); weights.set(1.0); for(int j=0;j<nb_var;++j) pre_chain[j].set(ii,cpu_chain[j].x_data(i)); pre_log_posterior.set(ii,cpu_log_posterior.x_data(i)); min_log_like=pre_log_posterior.min(); size_chain++;dump_nb_mpi++;conv_nb_mpi++;cov_nb_mpi=0;steps_taken++; } else {cov_nb_mpi++;} } for(int j=0;j<nb_var;++j) cpu_chain[j].clear(); cpu_log_posterior.clear(); if (cov_nb_mpi >= cov_lenght) { cov_nb_mpi = 0; calc_covariance_pre(); update_covariance(); } }
uint8_t kalman_update( kalman_robot_handle_t * handle, const position_t * measurement, float delta_t, robot_pos_t * dest) { if(handle == NULL || dest == NULL || delta_t < 0.0f) { return 0; } os_mutex_take(&(handle->_mutex)); // predict new state predict_state(&(handle->_state), delta_t, &(handle->_state)); // predict new covariance covariance_t proc_noise_cov; process_noise_covariance(handle, delta_t, &proc_noise_cov); predict_covariance( &(handle->_state_covariance), &proc_noise_cov, delta_t, &(handle->_state_covariance)); // if there is a measurement make kalman update if(measurement != NULL) { // compute kalman gain kalman_gain_t gain; kalman_gain( &(handle->_state_covariance), &(handle->_measurement_covariance), &gain); // compute difference between prediction and measurement vec2d_t residual; residual._x = measurement->x - handle->_state._x; residual._y = measurement->y - handle->_state._y; // estimate new state considering measurement update_state(&(handle->_state), &gain, &residual, &(handle->_state)); update_covariance( &(handle->_state_covariance), &gain, &(handle->_state_covariance)); } // write resulting position (and associated variances) to dest dest->x = handle->_state._x; dest->y = handle->_state._y; dest->var_x = handle->_state_covariance._cov_a._a; dest->var_y = handle->_state_covariance._cov_a._d; dest->cov_xy = handle->_state_covariance._cov_a._b; os_mutex_release(&(handle->_mutex)); return 1; }
void nested_mpi::make_pre_chain(){ choose_cpu_pre_step(); param_log_like=pre_log_posterior.min(); calc_covariance_pre(); update_covariance(); if (myid == 0){ std::string name_here; name_here=tag_name+"_pre_chain"; save_pre(name_here); name_here=tag_name+"_pre_cov"; save_cov(name_here); } int ii=pre_log_posterior.max_x(); for (int i=0;i<nb_var;++i) param_max[i]=pre_chain[i].x_data(ii); max_log_like=pre_log_posterior.x_data(ii); min_log_like=pre_log_posterior.min(); }
/* INPUTS : imgx Spatial gradients under x. imgy Spatial gradients under y. imgt Temporal gradient (DFD): imgt = I(pi+depl,t+1) - I(pi,t) zone_val Estimation support. etiq Support value to take into consderation. fen Work window. ima_pond Ponderation. OUTPUT : d_param Estimated motion model in the polynomial form. DESCRIPTION : Compute a robust estimation of the motion model. RETURN : The number of pixels used for the computation. */ bool estimate_QUA_COMPLET(TImageFloat *imgx, TImageFloat *imgy, TImageFloat *imgt, TImageShort *zone_val, int etiq, TWindow win, TImageFloat *ima_pond, Para *d_param) { int N; // Number of parameters N = d_param->nb_para; if (d_param->var_light) N++; int cnt = 0; // Pixel counter int nb_pts_utiles = 0; // Used pixel counter double YTWY = 0.0; double YTWXTheta = 0.0; double *A_ = new double [N*N]; double **A = new double* [N]; double *B = new double [N]; double *X = new double [N]; double *phi = new double [N]; double li_c = d_param->li_c; double co_c = d_param->co_c; int success = 0; // return value int x; // column int y; // row int i, j; for (i=0; i < N; i ++) A[i] = A_ + i*N; d_param->sigma2res = 0.0; set_double (X, 0.0, N); set_double (B, 0.0, N); set_double (&A[0][0], 0.0, N * N); // Diagonal pertubation, to assume that the matrix would be inversible for (i = 0; i < N; i++) A[i][i] = PERTURBATION; if (d_param->var_light) phi[N-1] = 1.0; // For the global illumination variation parameter for (y = win.dli; y <win.fli; y++) { size_t off = MIJ(0, y, 0, ima_pond->nbco); short *pval = &zone_val->ad[off]; float *pond = &ima_pond->ad[off]; float *pgx = &imgx->ad[off]; float *pgy = &imgy->ad[off]; float *pgt = &imgt->ad[off]; double dy = (double) (y - li_c); for(x = win.dco; x < win.fco; x++) { double dpond, dpgt; // temporary values double dx; if ((pval[x]==etiq) && (((pgx[x]!=0.0)||(pgy[x]!=0.0))||(pgt[x]!=0.0))){ nb_pts_utiles++; if (pond[x] > POND_MINI) { cnt++; dpond = (double) pond[x]; dpgt = (double) pgt[x]; dx = (double) (x - co_c); phi[0] = pgx[x]; phi[1] = pgy[x]; phi[2] = phi[0] * dx; phi[3] = phi[0] * dy; phi[4] = phi[1] * dx; phi[5] = phi[1] * dy; phi[6] = phi[2] * dx; phi[7] = phi[2] * dy; phi[8] = phi[3] * dy; phi[9] = phi[4] * dx; phi[10] = phi[4] * dy; phi[11] = phi[5] * dy; // For the residual variance computation if (d_param->compute_sigma2res) YTWY += dpond * dpgt * dpgt; for (i = 0; i < N; i++) { double d = (double) phi[i] * dpond; B[i] -= d * dpgt; // Cumulation on a triangle matrix for (j = 0; j <= i; j++) A[i][j] += phi[j] * d; } } } } } // Fill the upper triangle of the matrix for (i = 0; i < N; i++) for (j = i + 1; j < N; j++) A[i][j] = A[j][i]; d_param->tx_pts = ((double)cnt)/nb_pts_utiles; if (cnt <= N * 2) { // Not enough pixels delete [] A_; delete [] A; delete [] B; delete [] X; delete [] phi; return false; } // Solve AX = B by exploiting the symetry of the matrix success = resoud_mat_sym (&A[0][0], B, X, N, N); if (! success) { set_double (X, 0.0, N); return false; } // Convert the compact form to the polynomial form if (d_param->var_light) { for (i=0; i < N-1; i ++) d_param->thet[i] = X[i]; d_param->thet[12] = X[N-1]; // Illumination variation } else { for (i=0; i < N; i ++) d_param->thet[i] = X[i]; } if ( ! d_param->compute_sigma2res) { delete [] A_; delete [] A; delete [] B; delete [] X; delete [] phi; return true; } // Compute the residual variance YTWXTheta = dot_double (B, X, N); d_param->sigma2res = (YTWY - YTWXTheta) / (double) cnt; if ( ! d_param->compute_covariance) { delete [] A_; delete [] A; delete [] B; delete [] X; delete [] phi; return true; } // Compute the covariance matrix double *InvA_ = new double [N*N]; double **InvA = new double* [N]; for (i=0; i < N; i ++) InvA[i] = InvA_ + i*N; success = inverse_mat_sym (&A[0][0], &InvA[0][0], N); if (! success) { return false; } update_covariance(&InvA[0][0], d_param); delete [] A_; delete [] A; delete [] B; delete [] X; delete [] phi; delete [] InvA_; delete [] InvA; return true; }