/* covariance matrix is linearized */ double ighmm_rand_binormal_density(const double *x, double *mean, double *cov) { # define CUR_PROC "ighmm_rand_binormal_density" double rho; #ifndef DO_WITH_GSL double numerator,part1,part2,part3; #endif if (cov[0] <= 0.0 || cov[2 + 1] <= 0.0) { GHMM_LOG(LCONVERTED, "variance <= 0.0 not allowed\n"); goto STOP; } rho = cov[1] / ( sqrt (cov[0]) * sqrt (cov[2 + 1]) ); /* The denominator is possibly < EPS??? Check that ? */ #ifdef DO_WITH_GSL /* double gsl_ran_bivariate_gaussian_pdf (double x, double y, double sigma_x, double sigma_y, double rho) */ return gsl_ran_bivariate_gaussian_pdf (x[0], x[1], sqrt (cov[0]), sqrt (cov[2 + 1]), rho); #else part1 = (x[0] - mean[0]) / sqrt (cov[0]); part2 = (x[1] - mean[1]) / sqrt (cov[2 + 1]); part3 = m_sqr (part1) - 2 * part1 * part2 + m_sqr (part2); numerator = exp ( -1 * (part3) / ( 2 * (1 - m_sqr(rho)) ) ); return (numerator / ( 2 * PI * sqrt(1 - m_sqr(rho)) )); #endif STOP: return (-1.0); # undef CUR_PROC } /* double ighmm_rand_binormal_density */
void test_bivariate_gaussian(void){ double sigma_x = 3.5, sigma_y = 4.5, rho = .7, x, y, prob; gsl_ran_bivariate_gaussian(rng, sigma_x, sigma_y, rho, &x, &y); printf("gsl_ran_bivariate_gaussian\t[%.12f,%.12f]\t%.12f\t[%.12f,%.12f]\n", sigma_x, sigma_y, rho, x, y); prob = gsl_ran_bivariate_gaussian_pdf(x, y, sigma_x, sigma_y, rho); printf("gsl_ran_bivariate_gaussian_pdf\t[%.12f,%.12f]\t[%.12f,%.12f]\t%.12f\t%.12f\n", x, y, sigma_x, sigma_y, rho, prob); }
double test_bivariate_gaussian2_pdf (double y) { int i, n = 10 ; double sum = 0 ; double a = -10, b = 10, dx = (b - a)/n ; for (i = 0; i < n ; i++) { double x = a + i * dx ; sum += gsl_ran_bivariate_gaussian_pdf (x, y, 3.0, 2.0, 0.3) * dx ; } return sum ; }
/** * @brief mhtTrackingModule::calculateCosts Calculates the costs of connections for every edge possible in a specific hypothesis. * @param hyp Hypothesis in analysis */ void mhtTrackingModule::calculateCosts(hypothesis& hyp) { double N, ux, uy, S; QPointF x; // The space must be reserved int** costs = (int**) malloc((measures_t.size()+1)*sizeof(int*)); for (uint i = 0; i < measures_t.size()+1; i++) { costs[i] = (int*) malloc((hyp.stimation_t_1.size()+1)*sizeof(int)); } // For every measure, and every object, calculate the cost of connection // according to: -2log(N(z:Hx,S)) -> normal distribution -> Hx mean, using Kalman matrix, // and S variance of the stimation and variance of the distribution. for(uint i=0; i<measures_t.size()+1; i++) { if(i<measures_t.size()) { for(uint j=0; j<hyp.stimation_t_1.size(); j++) { x = fixMeasure(hyp.stimation_t_1.at(j), measures_t.at(i)); S = hyp.covariance_t_1.at(j).at(j); ux = hyp.stimation_t_1.at(j).cx; uy = hyp.stimation_t_1.at(j).cy; N = gsl_ran_bivariate_gaussian_pdf(x.x()-ux,x.y()-uy,S,S,0.0); costs[i][j] = -2*log(N); } costs[i][hyp.stimation_t_1.size()] = CN; } else { for(uint j=0; j<hyp.stimation_t_1.size(); j++) costs[i][j] = CD; costs[i][hyp.stimation_t_1.size()] = 0; } } delete hyp.costs; hyp.costs = costs; }
void GbA::get_pdf(double *pdf, int nm, int nr, double *mmarg, int nms, double *rmarg, int nrs){ assert(nms==nm && nrs==nr); assert(nr > 1 && nm > 1); assert(nms == _nms && nrs == _nrs); pthread_mutex_lock(&_process_lock); double dm, dr, evd; int i,j; double *_m_tmp, *_r_tmp, *_r1, *_m1; _m_tmp = new double[nm]; _r_tmp = new double[nr]; _r1 = new double[nr]; _m1 = new double[nm]; memcpy(_r1,_rsamples,nr*sizeof(double)); memcpy(_m1,_msamples,nm*sizeof(double)); // assume regular samples dr = _rsamples[1] - _rsamples[0]; dm = _msamples[1] - _msamples[0]; // Compute pdf for(i=0; i<nm; i++){ for(j=0; j<nr; j++){ pdf[i*nr+j] = gsl_ran_bivariate_gaussian_pdf(_msamples[i]-_mn[1], _rsamples[j]-_mn[0], std::sqrt(_cov[1][1]), std::sqrt(_cov[0][0]), _cov[0][1]/(std::sqrt(_cov[0][0])*std::sqrt(_cov[1][1]))); } } for(i=0;i<nm;i++){ _m_tmp[i] = 0; for(j=0; j<nr-1; j++){ _m_tmp[i] = _m_tmp[i] + (pdf[i*nr+j] + pdf[i*nr+j+1])/2.*dr; } } evd = 0; for(i=0;i<nm-1;i++) evd = evd + (_m_tmp[i] + _m_tmp[i+1])/2.*dm; for(i=0;i<nm;i++) _m_tmp[i] = _m_tmp[i] / evd; for(j=0;j<nr;j++){ _r_tmp[j] = 0; for(i=0; i<nm-1; i++){ _r_tmp[j] = _r_tmp[j] + (pdf[i*nr+j] + pdf[(i+1)*nr+j])/2.*dm; } } evd=0; for(i=0;i<nr-1;i++) evd = evd + (_r_tmp[i] + _r_tmp[i+1])/2.*dr; for(i=0;i<nr;i++) _r_tmp[i] = _r_tmp[i] / evd; memcpy(mmarg,_m_tmp,nm*sizeof(double)); memcpy(rmarg,_r_tmp,nr*sizeof(double)); delete[] _r1; delete[] _m1; delete[] _m_tmp; delete[] _r_tmp; pthread_mutex_unlock(&_process_lock); }