double dmvt(const unsigned int n, const gsl_vector *x, const gsl_vector *location, const gsl_matrix *scale, const unsigned int dof)
{
int s;
double ax,ay,az=0.5*(dof + n);
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, scale );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, location );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
ay = pow((1+ay/dof),-az)*gsl_sf_gamma(az)/(gsl_sf_gamma(0.5*dof)*sqrt( pow((dof*M_PI),double(n))*ax ));
return ay;
}
/// Subtract a vector
/// @param v :: The other vector
GSLVector &GSLVector::operator-=(const GSLVector &v) {
if (size() != v.size()) {
throw std::runtime_error("GSLVectors have different sizes.");
}
gsl_vector_sub(gsl(), v.gsl());
return *this;
}
double ran_mv_normal_pdf(const gsl_vector *x, const gsl_vector *mu,
const gsl_matrix *Sigma)
{
const int k = x->size;
int s;
double det, den;
gsl_vector *y = gsl_vector_alloc(k);
gsl_vector *work_k = gsl_vector_alloc(k);
gsl_matrix *work_k_k = gsl_matrix_alloc(k, k);
gsl_matrix *Sigmainv = gsl_matrix_alloc(k, k);
gsl_permutation *p = gsl_permutation_alloc(k);
gsl_vector_memcpy(y, x);
gsl_vector_sub(y, mu);
gsl_matrix_memcpy(work_k_k, Sigma);
gsl_linalg_LU_decomp(work_k_k, p, &s);
gsl_linalg_LU_invert(work_k_k, p, Sigmainv);
det = gsl_linalg_LU_det(work_k_k, s);
gsl_blas_dgemv(CblasNoTrans, 1.0, Sigmainv, y, 0.0, work_k);
gsl_blas_ddot(y, work_k, &den);
den = exp(-0.5*den) / sqrt(pow((2*M_PI), k)*det);
gsl_vector_free(y);
gsl_vector_free(work_k);
gsl_matrix_free(work_k_k);
gsl_matrix_free(Sigmainv);
gsl_permutation_free(p);
return den;
}
/**
* Adapted from: Multivariate Normal density function and random
* number generator Using GSL from Ralph dos Santos Silva
* Copyright (C) 2006
*
* multivariate normal density function
*
* @param n dimension of the random vetor
* @param mean vector of means of size n
* @param var variance matrix of dimension n x n
*/
double ssm_dmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var, double sd_fac)
{
int s;
double ax,ay;
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, var );
//scale var with sd_fac^2
gsl_matrix_scale(work, sd_fac*sd_fac);
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );
return ay;
}
double overlap_gto(const GTO* g1, const gsl_vector* A, const GTO* g2, const gsl_vector* B, int debug)
{
double K, gamma, Ix, Iy, Iz, result = 0;
double normal1, normal2;
double coeff1, coeff2;
gsl_vector *PA, *PB, *P;
PA = gsl_vector_alloc(3);
PB = gsl_vector_alloc(3);
normal1 = g1->norm;
normal2 = g2->norm;
coeff1 = g1->coeff;
coeff2 = g2->coeff;
// compute the coordination of P. 《量子化学》中册, P77
P = gaussian_product_center(g1->alpha, A, g2->alpha, B, debug);
gsl_vector_memcpy(PA, P);
gsl_vector_memcpy(PB, P);
gsl_vector_sub(PA, A);
gsl_vector_sub(PB, B);
gamma = g1->alpha + g2->alpha;
Ix = I_xyz(g1->l, PA->data[0], g2->l, PB->data[0], gamma, debug);
Iy = I_xyz(g1->m, PA->data[1], g2->m, PB->data[1], gamma, debug);
Iz = I_xyz(g1->n, PA->data[2], g2->n, PB->data[2], gamma, debug);
K = gauss_K(g1->alpha, A, g2->alpha, B);
result = pow(M_PI/gamma, 1.5) * K * Ix * Iy * Iz * normal1 * normal2 * coeff1 * coeff2;
if (debug == 2) {
printf("--------------------------------------------\n");
vector_output(PA, 3, "PA:");
vector_output(PB, 3, "PB:");
gto_output(g1, 1, "g1:");
gto_output(g2, 1, "g2:");
printf("%14.8lf%14.8lf%14.8lf%14.8lf%14.8lf\n",
K, Ix, Iy, Iz, result);
}
gsl_vector_free(P);
gsl_vector_free(PA);
gsl_vector_free(PB);
return result;
}
GslVector&
GslVector::operator-=(const GslVector& rhs)
{
int iRC;
iRC = gsl_vector_sub(m_vec,rhs.m_vec);
queso_require_msg(!(iRC), "failed");
return *this;
}
/** Subtraction operator (vector) */
vector<double> vector<double>::operator-(const vector<double>& v)
{
vector<double> v1(_vector);
if (gsl_vector_sub(v1.as_gsl_type_ptr(), v.as_gsl_type_ptr()))
{
std::cout << "\n Error in vector<double> -" << std::endl;
exit(EXIT_FAILURE);
}
return v1;
}
double objective(gsl_matrix *A, gsl_vector *b, double lambda, gsl_vector *z) {
double obj = 0;
gsl_vector *Azb = gsl_vector_calloc(A->size1);
gsl_blas_dgemv(CblasNoTrans, 1, A, z, 0, Azb);
gsl_vector_sub(Azb, b);
double Azb_nrm2;
gsl_blas_ddot(Azb, Azb, &Azb_nrm2);
obj = 0.5 * Azb_nrm2 + lambda * gsl_blas_dasum(z);
gsl_vector_free(Azb);
return obj;
}
void SimplexFltr::pivot(int jj, int ii) {
gsl_vector_scale(gjs[jj],1.0/gsl_vector_get(gjs[jj],ii));
for(int j=0; j<d+1; j++) {
if(j==jj) j++;
double scale = gsl_vector_get(gjs[j],ii);
if(std::abs(scale)>=signtol) { //need to subtract off this row
gsl_vector_scale(gjs[jj],scale);
gsl_vector_sub(gjs[j],gjs[jj]);
gsl_vector_scale(gjs[jj],1.0/scale);
}
}
}
GslVector&
GslVector::operator-=(const GslVector& rhs)
{
int iRC;
iRC = gsl_vector_sub(m_vec,rhs.m_vec);
UQ_FATAL_RC_MACRO(iRC,
m_env.worldRank(),
"GslVector::operator-=()",
"failed");
return *this;
}
double gaussian_pdf_log(const gaussian_t* dist,
const gsl_vector* x) {
double r = 0.0;
double logdet = 0.0;
if (gaussian_isdiagonal(dist)) {
size_t i;
double dx, dd;
for (i = 0; i < dist->dim; i++) {
dx = gsl_vector_get(x, i) - gsl_vector_get(dist->mean, i);
dd = gsl_vector_get(dist->diag, i);
r += dx * dx / dd;
logdet += DEBUG_LOG(dd);
}
}
else {
int signum;
gsl_vector* w1 = gsl_vector_alloc(dist->dim);
gsl_vector* w2 = gsl_vector_alloc(dist->dim);
gsl_vector_memcpy(w1, x);
gsl_vector_sub(w1, dist->mean);
gsl_matrix* v = gsl_matrix_alloc(dist->dim, dist->dim);
gsl_matrix_memcpy(v, dist->cov);
gsl_permutation* p = gsl_permutation_alloc(dist->dim);
gsl_linalg_LU_decomp(v, p, &signum);
gsl_linalg_LU_solve(v, p, w1, w2);
gsl_blas_ddot(w1, w2, &r);
logdet = gsl_linalg_LU_lndet(v);
assert(gsl_linalg_LU_sgndet(v, signum) == 1.0);
gsl_vector_free(w1);
gsl_vector_free(w2);
gsl_matrix_free(v);
gsl_permutation_free(p);
}
/* Use log to avoid underflow !
here
r = (x - mean)^T * cov^-1 * (x - mean)
logdet = log(det(cov))
then
logpdf = -.5 * (k * log(2*pi) + logdet + r);
*/
r = r + dist->dim * DEBUG_LOG(2 * M_PI) + logdet;
r = -0.5 * r;
assert(!isnan(r));
return r;
}
double ldmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var){
/* log-multivariate normal density function */
/*
* n dimension of the random vetor
* mean vector of means of size n
* var variance matrix of dimension n x n
*/
int s;
double ax,ay;
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
#ifdef PRINTSTIFF
/* Print Stiffness indicator S=max(eigen)/min(eigen)*/
gsl_vector *eval = gsl_vector_alloc (n);
gsl_matrix *evec = gsl_matrix_alloc (n, n);
gsl_matrix_memcpy(work,var);
gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(n);
gsl_eigen_symmv (work, eval, evec, w);
gsl_eigen_symmv_free (w);
gsl_eigen_symmv_sort (eval, evec,
GSL_EIGEN_SORT_ABS_ASC);
printf("%f ",
gsl_vector_get(eval,n-1) / gsl_vector_get(eval,0));
gsl_vector_free(eval);
gsl_matrix_free(evec);
#endif
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, var );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
/*
* ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );
*/
ay = -0.5*( ay + n*log(2*M_PI) + log(ax) );
return ay;
}
int resudial_itegral_k(const gsl_vector *in, void * p, gsl_vector *out){
struct mu_data_fit * mu = (struct mu_data_fit *)p;
struct fit_params fp = {in->data[0], in->data[1], in->data[2], \
in->data[3], in->data[4], in->data[5]} ;
gsl_vector_set_zero(out);
compute_itegral(mu->k, &fp, out);
/*
for (int i =0; i< in->size; i++){
printf("%10.5f", gsl_vector_get (out, i)) ;
}
printf("\n") ;*/
gsl_vector_set (out, 0, 0.0) ;
gsl_vector_sub(out, mu->mu);
return GSL_SUCCESS;}
bool move_unit_towards(unit *subj, gsl_vector *dest, PLAYERS *players) {
// set the z coordinate because it gets set incorrectly when
// placing the unit in the beginning
gsl_vector_set(subj->position, 2, height_at(x(subj->position), y(subj->position)));
gsl_vector *go_to = gsl_vector_alloc(3);
gsl_vector_memcpy(go_to, dest);
gsl_vector_sub(go_to, subj->position);
// check if we're already there
bool there = abs(lround(x(go_to))) < subj->attributes.speed && abs(lround(y(go_to))) < subj->attributes.speed;
if(there) {
gsl_vector_free(go_to);
return true;
}
double norm = gsl_blas_dnrm2(go_to);
gsl_vector_scale(go_to, 1 / norm);
//delta_height_scale(go_to, subj->position);
gsl_vector_add(subj->velocity, go_to);
//bool unit_at = check_for_unit_near(go_to, players, subj, false, false) != NULL;
//if(!unit_at) {
// XXX find a way around?
//gsl_vector_memcpy(subj->position, go_to);
//}
// re-check if we're there yet
gsl_vector_memcpy(go_to, dest);
gsl_vector_sub(go_to, subj->position);
there = abs(lround(x(go_to))) < subj->attributes.speed && abs(lround(y(go_to))) < subj->attributes.speed;
gsl_vector_free(go_to);
return there;
}
gsl_vector* gaussian_product_center(const double a, const gsl_vector *A,
const double b, const gsl_vector *B, int flags)
{
// Gaussian函数乘积定理计算双中心
// 关于此部分不甚明白
int i;
double gamma = a + b;
//double x1, x2, tmp;
gsl_vector *center = gsl_vector_alloc(3);
for (i = 0; i < 3; i++)
center->data[i] = (a * A->data[i] + b * B->data[i]) / gamma;
// FOR DEBUG
if (flags == 1) {
gsl_vector * test = gsl_vector_alloc(3);
gsl_vector * test2 = gsl_vector_alloc(3);
gsl_vector_set(test, 0, 0);
gsl_vector_set(test, 1, 0);
gsl_vector_set(test, 2, 2.175);
gsl_vector_memcpy(test2, test);
gsl_vector_sub(test, B);
gsl_vector_sub(test2, A);
if (gsl_vector_max(test) < 1.0E-10 || gsl_vector_max(test2) < 1.0E-10) {
//printf("----------------------------------------\n");
vector_output(A, 3, "用于计算中心的第一个坐标:");
vector_output(B, 3, "用于计算中心的第二个坐标:");
printf("alpha1 =%10.6lf\talpha2 =%10.6lf\n", a, b);
vector_output(center, 3, "中心为:");
}
gsl_vector_free(test);
gsl_vector_free(test2);
}
return center;
}
static inline double mean_distance(const mtrx* in, int n){
double out = 0.0;
vect* wrk = gsl_vector_alloc(n);
for(int i = 0; i< in->size1; i++){
gsl_vector_view v1 = gsl_vector_view_array(in->data+i*in->tda,n);
for(int j = i; j < in->size1; j++){
gsl_vector_view v2 = gsl_vector_view_array(in->data+j*in->tda,n);
gsl_vector_memcpy(wrk, &v2.vector);
gsl_vector_sub(wrk, &v1.vector);
out += gsl_blas_dnrm2(wrk);
}
}
gsl_vector_free(wrk);
return out/(in->size1*in->size1-1);
}
double gauss_K(double a, const gsl_vector *A, double b, const gsl_vector *B)
{
// 归一化系数 《量子化学》中册 P58 (10.4.1b)
// A, B 为坐标
double result, norm_2;
gsl_vector* v = gsl_vector_alloc(3);
gsl_vector_memcpy(v, A);
gsl_vector_sub(v, B);
norm_2 = gsl_pow_2(gsl_blas_dnrm2(v));
result = exp(-a * b * norm_2 / (a + b));
return result;
}
static CVECTOR *_sub(CVECTOR *a, CVECTOR *b, bool invert)
{
CVECTOR *v = VECTOR_make(a);
if (COMPLEX(v) || COMPLEX(b))
{
VECTOR_ensure_complex(v);
VECTOR_ensure_complex(b);
gsl_vector_complex_sub(CVEC(v), CVEC(b));
}
else
gsl_vector_sub(VEC(v), VEC(b));
return v;
}
double uniformPdf(gsl_vector *alpha,gsl_vector *beta,const double *x){
double r=1.0;
int i;
for(i=0;i<alpha->size;i++){
if(x[i] > gsl_vector_get(alpha,i)) return 0;
if(x[i] < gsl_vector_get(beta,i)) return 0;
}
gsl_vector *rages=gsl_vector_clone(alpha);
gsl_vector_sub(rages,beta);
for(i=0;i<rages->size;i++){
r*=gsl_vector_get(rages,i);
}
return 1.0/r;
}
/** Rotates point b around point a using the rotation matrix R. */
void rotate(bool transpose, const gsl_matrix *R, const gsl_vector *a, gsl_vector *b)
{
declare_stack_allocated_vector(v, 3);
gsl_vector_memcpy(v, b);
gsl_vector_sub(v, a);
/* Rotate end vector. */
declare_stack_allocated_vector(w, 3);
gsl_blas_dgemv(transpose == false ? CblasNoTrans : CblasTrans, 1.0, R, v, 0.0, w);
/* Update position. */
gsl_vector_memcpy(b, a);
gsl_vector_add(b, w);
assert(gsl_fcmp(gsl_blas_dnrm2(v), gsl_blas_dnrm2(w), 1e-15) == 0);
}
double gsl_vector_minkowski_dist(const gsl_vector* v1,
const gsl_vector* v2, const double p)
{
gsl_vector* diff = gsl_vector_alloc(v1->size);
gsl_vector_memcpy(diff, v1);
gsl_vector_sub(diff, v2);
double retval = 0.;
for (size_t i = 0; i < diff->size; ++i)
{
retval += pow(abs(gsl_vector_get(diff, i)), p);
}
retval = pow(retval, 1./p);
gsl_vector_free(diff);
return retval;
}
void GbA::compute_likelihood(double *data, int nstats, int nbands, int nsim,
double mean[2], double cov[2][2]){
int i, j, k, l, idx;
gsl_vector *lsq = gsl_vector_alloc (_td->get_noftraces());
gsl_vector *input = gsl_vector_alloc (nbands);
gsl_vector *trace = gsl_vector_alloc (nbands);
double *mags, *dists;
double misfit_l2;
size_t *indices = new size_t[nsim];
mags = new double[nsim];
dists = new double[nsim];
for (i=0; i<nstats; i++){
for (j=0; j<nbands; j++){
gsl_vector_set(input, j, std::log10(data[i*nbands+j]));
}
for (k=0; k<_td->get_noftraces(); k++){
gsl_matrix_get_row(trace, _td->_tdata_gsl, k);
gsl_vector_sub(trace,input);
misfit_l2 = gsl_blas_dnrm2(trace);
gsl_vector_set(lsq, k, misfit_l2*misfit_l2);
}
gsl_sort_vector_smallest_index(indices, nsim,lsq);
mean[0] = 0;
mean[1] = 0;
for(l=0; l<nsim; l++){
dists[l] = std::log10(gsl_vector_get(_td->_r_gsl, indices[l]));
mags[l] = gsl_vector_get(_td->_m_gsl, indices[l]);
//std::cout << gsl_vector_get(_td->_m_gsl,indices[l]) << std::endl;
}
mean[0] = gsl_stats_mean(dists,1,nsim);
mean[1] = gsl_stats_mean(mags,1,nsim);
cov[0][0] = gsl_stats_variance(dists, 1, nsim);
cov[1][1] = gsl_stats_variance(mags, 1, nsim);
cov[0][1] = gsl_stats_covariance_m(dists,1,mags,1,nsim,mean[0],mean[1]);
cov[1][0] = cov[0][1];
std::cout << "Distance: " << mean[0] << "; Magnitude: " << mean[1] << std::endl;
std::cout << "Covariance matrix:" << std::endl;
for(i=0; i<2; i++){
for(j=0;j<2;j++){
printf("%d, %d, %g\n",i,j,cov[i][j]);
}
}
}
}
double execute_chi2_t(chi2_t *chichi)
{
//-- Let Delta X = X_model - X_obs,
//-- L = 1 / sqrt[(2 pi)^d * det(Cov)] * exp[-0.5 *(Delta X)^T * Cov^-1 * (Delta X)]
//-- -2 ln L = -2 * [ -0.5 * ln (2 pi)^d - 0.5 * ln det(Cov) - 0.5 * (Delta X)^T * Cov^-1 * (Delta X) ]
//-- = cst + ln det(Cov) + (Delta X)^T * Cov^-1 * (Delta X)
//-- We set chi2 = ln det(Cov) + (Delta X)^T * Cov^-1 * (Delta X)
//-- data should be N*d matrix
int N = chichi->N;
int d = chichi->d;
gsl_vector *X_model = chichi->X_model;
double value;
gsl_vector_sub(X_model, chichi->X_obs); //-- X_model -= X_obs
gsl_blas_dsymv(CblasUpper, 1.0, chichi->invCov, X_model, 0.0, chichi->intermediate); //-- intermediate = invCov * (X_model - X_obs)
gsl_blas_ddot(X_model, chichi->intermediate, &value);
value += gsl_linalg_LU_lndet(chichi->cov);
return value;
}
void gatherErrors(gsl_vector *x_bar, gsl_vector *x, gsl_vector **x_error,
double *max_error)
{
double elem;
size_t i;
*x_error = gsl_vector_alloc(x_bar->size);
gsl_vector_memcpy(*x_error, x);
gsl_vector_sub(*x_error, x_bar);
*max_error = fabs(gsl_vector_get(*x_error, 0));
for (i = 1; i < (*x_error)->size; ++i) {
elem = fabs(gsl_vector_get(*x_error, i));
if (*max_error < elem) {
*max_error = elem;
}
}
}
/** Give me a data set and a model, and I'll give you the jackknifed covariance matrix of the model parameters.
The basic algorithm for the jackknife (glossing over the details): create a sequence of data
sets, each with exactly one observation removed, and then produce a new set of parameter estimates
using that slightly shortened data set. Then, find the covariance matrix of the derived parameters.
\li Jackknife or bootstrap? As a broad rule of thumb, the jackknife works best on models
that are closer to linear. The worse a linear approximation does (at the given data),
the worse the jackknife approximates the variance.
\param in The data set. An \ref apop_data set where each row is a single data point.
\param model An \ref apop_model, that will be used internally by \ref apop_estimate.
\exception out->error=='n' \c NULL input data.
\return An \c apop_data set whose matrix element is the estimated covariance matrix of the parameters.
\see apop_bootstrap_cov
For example:
\include jack.c
*/
apop_data * apop_jackknife_cov(apop_data *in, apop_model *model){
Apop_stopif(!in, apop_return_data_error(n), 0, "The data input can't be NULL.");
Get_vmsizes(in); //msize1, msize2, vsize
apop_model *e = apop_model_copy(model);
int i, n = GSL_MAX(msize1, GSL_MAX(vsize, in->textsize[0]));
apop_model *overall_est = e->parameters ? e : apop_estimate(in, e);//if not estimated, do so
gsl_vector *overall_params = apop_data_pack(overall_est->parameters);
gsl_vector_scale(overall_params, n); //do it just once.
gsl_vector *pseudoval = gsl_vector_alloc(overall_params->size);
//Copy the original, minus the first row.
apop_data *subset = apop_data_copy(Apop_rs(in, 1, n-1));
apop_name *tmpnames = in->names;
in->names = NULL; //save on some copying below.
apop_data *array_of_boots = apop_data_alloc(n, overall_params->size);
for(i = -1; i< n-1; i++){
//Get a view of row i, and copy it to position i-1 in the short matrix.
if (i >= 0) apop_data_memcpy(Apop_r(subset, i), Apop_r(in, i));
apop_model *est = apop_estimate(subset, e);
gsl_vector *estp = apop_data_pack(est->parameters);
gsl_vector_memcpy(pseudoval, overall_params);// *n above.
gsl_vector_scale(estp, n-1);
gsl_vector_sub(pseudoval, estp);
gsl_matrix_set_row(array_of_boots->matrix, i+1, pseudoval);
apop_model_free(est);
gsl_vector_free(estp);
}
in->names = tmpnames;
apop_data *out = apop_data_covariance(array_of_boots);
gsl_matrix_scale(out->matrix, 1./(n-1.));
apop_data_free(subset);
gsl_vector_free(pseudoval);
apop_data_free(array_of_boots);
if (e!=overall_est)
apop_model_free(overall_est);
apop_model_free(e);
gsl_vector_free(overall_params);
return out;
}
double ran_mv_t_pdf(const gsl_vector *x, const gsl_vector *mu,
const gsl_matrix *Sigma, const double nu)
{
const int k = x->size;
int s;
double det,temp, den;
gsl_vector *y = gsl_vector_alloc(k);
gsl_vector *work_k = gsl_vector_alloc(k);
gsl_matrix *work_k_k = gsl_matrix_alloc(k, k);
gsl_matrix *Sigmainv = gsl_matrix_alloc(k, k);
gsl_permutation *p = gsl_permutation_alloc(k);
gsl_vector_memcpy(y, x);
gsl_vector_sub(y, mu);
gsl_matrix_memcpy(work_k_k, Sigma);
gsl_linalg_LU_decomp(work_k_k, p, &s);
gsl_linalg_LU_invert(work_k_k, p, Sigmainv);
det = gsl_linalg_LU_det(work_k_k, s);
gsl_blas_dgemv(CblasNoTrans, 1.0/k, Sigmainv, y, 0.0, work_k);
gsl_blas_ddot(y, work_k, &temp);
temp = pow((1+temp), (nu+ (double) k)/2 );
temp *= gsl_sf_gamma(nu/2) * pow(nu, k/2) * pow(M_PI, k/2) * sqrt(det);
den = gsl_sf_gamma((nu+ (double) k)/2);
den /= temp;
gsl_vector_free(y);
gsl_vector_free(work_k);
gsl_matrix_free(work_k_k);
gsl_matrix_free(Sigmainv);
gsl_permutation_free(p);
return den;
}
void GetMVNpdf(const gsl_matrix * mat, const double * mu, const gsl_matrix * sigmaInv, const gsl_matrix * sigmaChol, const size_t nPoints, const size_t nDim, double * returnVal)
{
double normConst = - log(2*M_PI)*nDim/2.0;
for(size_t j = 0; j < nDim; j++)
normConst -= log(gsl_matrix_get(sigmaChol, j, j));
gsl_vector_const_view vecMu = gsl_vector_const_view_array(mu, nDim);
#pragma omp parallel for
for(size_t i = 0; i < nPoints; i++){
gsl_vector * x1 = gsl_vector_alloc(nDim); // Note: allocating and freeing these every loop is not ideal, but needed for threadsafe. There might be a better way.
gsl_vector * x2 = gsl_vector_alloc(nDim);
gsl_matrix_get_row(x1, mat, i);
gsl_vector_sub(x1, &vecMu.vector);
gsl_blas_dsymv(CblasUpper, 1.0, sigmaInv, x1, 0.0, x2);
gsl_blas_ddot(x1, x2, &returnVal[i]);
returnVal[i] = exp(normConst - 0.5*returnVal[i]);
gsl_vector_free(x1);
gsl_vector_free(x2);
}
return;
}
/* build orthonormal basis matrix
Q = Y;
for j=1:k
vj = Q(:,j);
for i=1:(j-1)
vi = Q(:,i);
vj = vj - project_vec(vj,vi);
end
vj = vj/norm(vj);
Q(:,j) = vj;
end
*/
void build_orthonormal_basis_from_mat(gsl_matrix *A, gsl_matrix *Q){
int m,n,i,j,ind,num_ortos=2;
double vec_norm;
gsl_vector *vi,*vj,*p;
m = A->size1;
n = A->size2;
vi = gsl_vector_calloc(m);
vj = gsl_vector_calloc(m);
p = gsl_vector_calloc(m);
gsl_matrix_memcpy(Q, A);
for(ind=0; ind<num_ortos; ind++){
for(j=0; j<n; j++){
gsl_matrix_get_col(vj, Q, j);
for(i=0; i<j; i++){
gsl_matrix_get_col(vi, Q, i);
project_vector(vj, vi, p);
gsl_vector_sub(vj, p);
}
vec_norm = gsl_blas_dnrm2(vj);
gsl_vector_scale(vj, 1.0/vec_norm);
gsl_matrix_set_col (Q, j, vj);
}
}
}
void GbA::process(double *data, int nbands, float time, int cmpnt){
assert(nbands == _nbands);
pthread_mutex_lock(&_process_lock);
int i, j, k, timeidx=0;
double timemin=std::numeric_limits<double>::max();
gsl_vector *lsq = gsl_vector_alloc (_td->get_noftraces());
gsl_vector *input = gsl_vector_alloc (nbands);
gsl_vector *trace = gsl_vector_alloc (nbands);
gsl_matrix *tdata;
double misfit_l2, terror;
size_t *indices = new size_t[_nsim];
TData::Component _c = static_cast<TData::Component>(cmpnt);
_status[_c] = true;
// Find time index
for(i=0;i<_td->get_noftimes();i++){
terror = gsl_vector_get(_td->get_times(),i) - time;
if(abs(terror)< timemin){
timeidx = i;
timemin = abs(terror);
}
}
tdata = _td->get_amps(timeidx,_c);
for (i=0; i<nbands; i++){
gsl_vector_set(input, i, std::log10(data[i]));
}
for (j=0; j<_td->get_noftraces(); j++){
gsl_matrix_get_row(trace, tdata, j);
gsl_vector_sub(trace,input);
misfit_l2 = gsl_blas_dnrm2(trace);
gsl_vector_set(lsq, j, misfit_l2*misfit_l2);
}
gsl_sort_vector_smallest_index(indices, _nsim,lsq);
for(k=0; k<_nsim; k++){
gsl_vector_set(&_r[_c].vector,k, std::log10(gsl_vector_get(_td->get_dist(), indices[k])));
gsl_vector_set(&_m[_c].vector,k, gsl_vector_get(_td->get_mag(), indices[k]));
}
if (_status[TData::vertical] && _status[TData::horizontal]){
_mv = gsl_vector_subvector(_mags,0,_mags->size);
_rv = gsl_vector_subvector(_dists,0,_dists->size);
} else if(_status[TData::vertical]){
_mv = gsl_vector_subvector(_mags,0,_nsim);
_rv = gsl_vector_subvector(_dists,0,_nsim);
} else if(_status[TData::horizontal]){
_mv = gsl_vector_subvector(_mags,_nsim,_nsim);
_rv = gsl_vector_subvector(_dists,_nsim,_nsim);
}
_mn[0] = gsl_stats_mean(_rv.vector.data,1,_rv.vector.size);
_mn[1] = gsl_stats_mean(_mv.vector.data,1,_mv.vector.size);
_cov[0][0] = gsl_stats_variance(_rv.vector.data, 1, _rv.vector.size);
_cov[1][1] = gsl_stats_variance(_mv.vector.data, 1, _mv.vector.size);
_cov[0][1] = gsl_stats_covariance_m(_rv.vector.data,1,_mv.vector.data,1,
_rv.vector.size,_mn[0],_mn[1]);
_cov[1][0] = _cov[0][1];
delete[] indices;
pthread_mutex_unlock(&_process_lock);
}
// measurement update (correction)
bool DiscreteKalmanFilter::updateMeasurement(const size_t step, const gsl_vector *actualMeasurement, const gsl_vector *Du) // Du(k) = Dd(k) * u(k)
{
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_matrix *Cd = system_.getOutputMatrix(step, x_hat_);
#if 0
const gsl_matrix *V = system_.getMeasurementNoiseCouplingMatrix(step);
const gsl_matrix *R = system_.getMeasurementNoiseCovarianceMatrix(step); // R(k)
#else
const gsl_matrix *Rd = system_.getMeasurementNoiseCovarianceMatrix(step); // Rd(k) = V(k) * R(k) * V(k)^T
#endif
//const gsl_vector *Du = system_.getMeasurementInput(step, x_hat_); // Du(k) = Dd(k) * u(k)
//const gsl_vector *actualMeasurement = system_.getMeasurement(step, x_hat_); // actual measurement
if (!Cd || !Rd || !Du || !actualMeasurement) return false;
// 1. calculate Kalman gain: K(k) = P-(k) * Cd(k)^T * (Cd(k) * P-(k) * Cd(k)^T + Rd(k))^-1 where Rd(k) = V(k) * R(k) * V(k)^T
// inverse of matrix using LU decomposition
gsl_matrix_memcpy(RR_, Rd);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, P_, Cd, 0.0, PCt_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Cd, PCt_, 1.0, RR_))
return false;
int signum;
if (GSL_SUCCESS != gsl_linalg_LU_decomp(RR_, permutation_, &signum) ||
GSL_SUCCESS != gsl_linalg_LU_invert(RR_, permutation_, invRR_))
return false;
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, PCt_, invRR_, 0.0, K_)) // calculate Kalman gain
return false;
// 2. update measurement: x(k) = x-(k) + K(k) * (y_tilde(k) - y_hat(k)) where y_hat(k) = Cd(k) * x-(k) + Dd(k) * u(k)
#if 0
// save an estimated measurement, y_hat
gsl_vector_memcpy(y_hat_, Du);
if (GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, 1.0, Cd, x_hat_, 1.0, y_hat_)) // calcuate y_hat(k)
return false;
gsl_vector_memcpy(residual_, y_hat_);
if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#else
gsl_vector_memcpy(residual_, Du);
if (GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, 1.0, Cd, x_hat_, 1.0, residual_) || // calcuate y_hat(k)
GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#endif
// 3. update covariance: P(k) = (I - K(k) * Cd(k)) * P-(k)
#if 0
// not working
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, P_))
return false;
#else
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, M2_))
return false;
gsl_matrix_memcpy(P_, M2_);
#endif
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
}