void ld_linearize(LDP ld) {
int i;
for(i=0;i<ld->nrays;i++) {
if(-1 == ld->cluster[i]) continue;
int this_cluster = ld->cluster[i];
DYNAMIC_ALLOCATE(int, indexes, ld->nrays);
int nindexes = 0;
int j;
for(j=i;j<ld->nrays;j++)
if(ld->cluster[j]==this_cluster)
indexes[nindexes++] = j;
DYNAMIC_ALLOCATE(double, alpha, nindexes);
DYNAMIC_ALLOCATE(double, alpha_weight, nindexes);
for(j=0;j<nindexes;j++) {
alpha[j] = ld->alpha[indexes[j]];
alpha_weight[j] = 1 / ld->cov_alpha[indexes[j]];
}
double est_alpha = weighted_mean(alpha, alpha_weight, nindexes);
DYNAMIC_ALLOCATE(double, rho, nindexes);
DYNAMIC_ALLOCATE(double, rho_weight, nindexes);
for(j=0;j<nindexes;j++) {
int i = indexes[j];
double theta = ld->theta[i];
double x = cos(theta) * ld->readings[i];
double y = sin(theta) * ld->readings[i];
rho[j] = cos(est_alpha) * x + sin(est_alpha) * y;
rho_weight[j] = 1 / ( cos(est_alpha) * cos(theta)
+ sin(est_alpha) * sin(theta) );
}
double est_rho = weighted_mean(rho, rho_weight, nindexes);
for(j=0;j<nindexes;j++) {
int i = indexes[j];
double theta = ld->theta[i];
ld->readings[i] = est_rho / (cos(est_alpha) * cos(theta)
+ sin(est_alpha) * sin(theta));
}
i = indexes[nindexes-1];
CLEAN_MEMORY(rho_weight);
CLEAN_MEMORY(rho);
CLEAN_MEMORY(alpha_weight);
CLEAN_MEMORY(alpha);
CLEAN_MEMORY(indexes);
}
}
result_type result(Args const &args) const
{
return numeric::average(
weighted_moment<3>(args)
- 3. * weighted_moment<2>(args) * weighted_mean(args)
+ 2. * weighted_mean(args) * weighted_mean(args) * weighted_mean(args)
, ( weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
* std::sqrt( weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
);
}
//----------------------------------------------------------------------
void anisotropic_kernel(){
for(int k=0;k<PARTICLES;k++){
/*
このままだと粒子近傍の有効範囲が正方形になってしまう。
*/
int x_min = int(x[k] * GRID_SIZE_X - DISTANCE * GRID_SIZE_X);
int y_min = int(y[k] * GRID_SIZE_Y - DISTANCE * GRID_SIZE_Y);
int x_max = int(x[k] * GRID_SIZE_X + DISTANCE * GRID_SIZE_X);
int y_max = int(y[k] * GRID_SIZE_Y + DISTANCE * GRID_SIZE_Y);
if(x_min < 0)
x_min =0;
if(y_min < 0)
y_min =0;
if(x_max > GRID_SIZE_X)
x_max = GRID_SIZE_X;
if(y_max > GRID_SIZE_Y)
x_max = GRID_SIZE_Y;
int i = x_min;
int j = y_min;
double sigma = 1.0;//スケーリング定数
double h = DISTANCE;//有効範囲
double h_square = h * h;//h^2
weighted_mean();
calc_covariance_matrix(k);
calc_svd();
calc_matrix_G();
double determinant = matrix_G[0][0] * matrix_G[1][1] - matrix_G[1][0] * matrix_G[0][1];
double Gr = sqrt( pow(matrix_G[0][0] * (x[k] - x_smoothing[k]) + matrix_G[1][0] * (y[k] - y_smoothing[k]),2)
+ pow(matrix_G[0][1] * (x[k] - x_smoothing[k]) + matrix_G[1][1] * (y[k] - y_smoothing[k]),2) );
//ここで固有値などを調整する必要がある。
for(i =x_min;i<x_max;i++){
int l=0;
for(j=y_min;j<y_max;j++){
double distance = sqrt( pow(fabs((double)j / GRID_SIZE_X - y[k]),2) + pow(fabs( (double)i/ GRID_SIZE_Y - x[k]),2) );
grid[i][j] = grid[i][j] + sigma * determinant * function_p(Gr);
l++;
}
particle_number[i][j] = l;
}
}
}
void Metropolis::prepare_deviance()
{
sampleDeviance.push_back(DBar);
DBar = weighted_mean(sampleDeviance);
sampleDeviance.clear();
// compute the mean of the current parameter samples
std::pair<STM::ParMap, int> parMeans;
for(const auto & p : parameters.names())
parMeans.first[p] = parMeans.second = 0;
for(const auto sample : currentSamples)
{
for(const auto & p : parameters.names())
parMeans.first.at(p) += sample.at(p);
}
parMeans.second = currentSamples.size();
for(const auto & p : parameters.names())
parMeans.first.at(p) /= parMeans.second;
thetaBar = weighted_mean(thetaBar, parMeans);
}
void operator ()(Args const &args)
{
std::size_t cnt = count(args);
if (cnt > 1)
{
extractor<tag::weighted_mean_of_variates<VariateType, VariateTag> > const some_weighted_mean_of_variates = {};
this->cov_ = this->cov_ * (sum_of_weights(args) - args[weight]) / sum_of_weights(args)
+ numeric::outer_product(
some_weighted_mean_of_variates(args) - args[parameter::keyword<VariateTag>::get()]
, weighted_mean(args) - args[sample]
) * args[weight] / (sum_of_weights(args) - args[weight]);
}
}
result_type result(Args const &args) const
{
return numeric::fdiv(
accumulators::weighted_moment<3>(args)
- 3. * accumulators::weighted_moment<2>(args) * weighted_mean(args)
+ 2. * weighted_mean(args) * weighted_mean(args) * weighted_mean(args)
, ( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
* std::sqrt( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
);
}
/**
* @brief Main wrapper for fitting a trendfilter model.
* Takes as input either a sequence of lambda tuning parameters, or the number
* of desired lambda values. In the latter case the function will also calculate
* a lambda sequence. The user must supply allocated memory to store the output,
* with the function itself returning only @c void. For default values, and an
* example of how to call the function, see the function tf_admm_default.
*
* @param x a vector of data locations; must be in increasing order
* @param y a vector of responses
* @param w a vector of sample weights
* @param n the length of x, y, and w
* @param k polynomial degree of the fitted trend; i.e., k=1 for linear
* @param family family code for the type of fit; family=0 for OLS
* @param max_iter maximum number of ADMM interations; ignored for k=0
* @param beta0 initialization value of beta for first lambda; ignored if NULL
* @param lam_flag 0/1 flag for whether lambda sequence needs to be estimated
* @param lambda either a sequence of lambda when lam_flag=0, or empty
* allocated space if lam_flag=1
* @param nlambda number of lambda values; need for both lam_flag=0 and 1
* @param lambda_min_ratio minimum ratio between min and max lambda; ignored for lam_flag=0
* @param df allocated space of nlambda to store the output df values
* @param beta allocated space of size n*nlambda to store the output coefficents
* @param obj allocated space of size max_iter*nlambda to store the objective
* @param iter allocated space of size nlambda to store the number of iterations
* @param status allocated space of size nlambda to store the status of each run
* @param rho tuning parameter for the ADMM algorithm
* @param obj_tol stopping criteria tolerance
* @param obj_tol_newton for family != 0, stopping criteria tolerance for prox Newton
* @param alpha_ls for family != 0, line search tuning parameter
* @param gamma_ls for family != 0, line search tuning parameter
* @param max_iter_ls for family != 0, max number of iterations in line search
* @param max_iter_newton for family != 0, max number of iterations in inner ADMM
* @param verbose 0/1 flag for printing progress
* @return void
* @see tf_admm_default
*/
void tf_admm ( double * x, double * y, double * w, int n, int k, int family,
int max_iter, double * beta0, int lam_flag, double * lambda,
int nlambda, double lambda_min_ratio, int tridiag, int * df,
double * beta, double * obj, int * iter, int * status,
double rho, double obj_tol, double obj_tol_newton, double alpha_ls, double gamma_ls,
int max_iter_ls, int max_iter_newton, int verbose)
{
int i;
int j;
int numDualVars;
double max_lam;
double min_lam;
double * temp_n;
double * beta_max;
double * alpha;
double * u;
double * A0;
double * A1;
double * v;
cs * D;
cs * Dt;
cs * Dk;
cs * Dkt;
cs * DktDk;
gqr * Dt_qr;
gqr * Dkt_qr;
beta_max = (double *) malloc(n * sizeof(double));
temp_n = (double *) malloc(n * sizeof(double));
v = (double *) malloc(n * sizeof(double));
numDualVars = tridiag ? k : 1;
/* we use extra buffer below (n vs n-k) */
alpha = (double *) malloc(n * numDualVars * sizeof(double));
u = (double *) malloc(n * numDualVars * sizeof(double));
/* Assume w does not have zeros */
for (i = 0; i < n; i++) temp_n[i] = 1/sqrt(w[i]);
D = tf_calc_dk(n, k+1, x);
Dk = tf_calc_dktil(n, k, x);
Dt = cs_transpose(D, 1);
diag_times_sparse(Dt, temp_n); /* Dt = W^{-1/2} Dt */
Dkt = cs_transpose(Dk, 1);
Dt_qr = glmgen_qr(Dt);
Dkt_qr = glmgen_qr(Dkt);
DktDk = cs_multiply(Dkt,Dk);
/* Determine the maximum lambda in the path */
max_lam = tf_maxlam(n, y, Dt_qr, w);
/* and if it is too small, return a trivial solution for Gaussian case */
if (family == FAMILY_GAUSSIAN) {
if (max_lam < 1e-12) {
for (i=0; i<nlambda; i++) {
for (j=0; j<n; j++) beta[i*n+j] = y[j];
obj[i*(max_iter+1)] = 0;
df[i] = n;
}
cs_spfree(D);
cs_spfree(Dt);
cs_spfree(Dk);
cs_spfree(Dkt);
cs_spfree(DktDk);
glmgen_gqr_free(Dt_qr);
glmgen_gqr_free(Dkt_qr);
free(temp_n);
free(beta_max);
free(alpha);
free(u);
return;
}
}
else {
max_lam += 1;
}
/* Initiate the path if needed using the input lambda_min_ratio and
* equally spaced points in log space. */
if (!lam_flag) seq_logspace(max_lam,lambda_min_ratio,nlambda,lambda);
/* Augmented Lagrangian parameter */
rho = rho * pow((x[n-1] - x[0])/(double)(n-1), (double)k);
/* Initiate alpha and u for a warm start */
if (lambda[0] < max_lam * 1e-5)
for (i = 0; i < n - k; i++) alpha[i] = u[i] = 0;
else {
/* beta_max */
if (beta0 == NULL)
calc_beta_max(y,w,n,Dt_qr,Dt,temp_n,beta_max);
else
memcpy(beta_max, beta0, n*sizeof(double));
/* Check if beta = weighted mean(y) is better than beta */
double yc = weighted_mean(y,w,n);
for (i = 0; i < n; i++) temp_n[i] = yc;
double obj1 = tf_obj(x,y,w,n,k,max_lam,family,beta_max,v);
double obj2 = tf_obj(x,y,w,n,k,max_lam,family,temp_n,v);
if(obj2 < obj1) memcpy(beta_max, temp_n, n*sizeof(double));
/* alpha_max */
if (tridiag && k>0)
{
tf_dx1(x, n, 1, beta_max, alpha + (n*k-n));
for (j=k-1; j >= 1; j--)
tf_dx1(x, n, k-j+1, alpha + (n*j), alpha + (n*j-n));
}
else if (k>0)
tf_dxtil(x, n, k, beta_max, alpha);
/* u_max */
if (tridiag)
for (j=0; j<k; j++) memset(u + (n*j), 0, (n-k+j) * sizeof(double));
else {
for (i = 0; i < n; i++)
u[i] = w[i] * (beta_max[i] - y[i]) / (rho * lambda[0]);
if(family == FAMILY_LOGISTIC)
for (i = 0; i < n; i++) u[i] *= logi_b2(beta_max[i]);
else if(family == FAMILY_POISSON)
for (i = 0; i < n; i++) u[i] *= pois_b2(beta_max[i]);
glmgen_qrsol (Dkt_qr, u);
// for (i = 0; i < n-k; i++) u[i] = 0;
}
}
if (tridiag && k>0)
{
/* Setup tridiagonal systems */
A0 = (double*) malloc(n*k*sizeof(double));
A1 = (double*) malloc(n*k*sizeof(double));
for (j=2; j <= k; j++)
{
form_tridiag(x, n, k-j+2, 1, 1, A0+(n*j-n), A1+(n*j-n));
}
}
/* Iterate lower level functions over all lambda values;
* the alpha and u vectors get used each time of subsequent
* warm starts */
for (i = 0; i < nlambda; i++)
{
/* warm start */
double *beta_init = (i == 0) ? beta_max : beta + (i-1)*n;
for(j = 0; j < n; j++) beta[i*n + j] = beta_init[j];
if (tridiag)
{
form_tridiag(x, n, 1, rho * lambda[i], 0, A0, A1);
for (j=0; j < n; j++) A0[j] = A0[j] + w[j];
}
switch (family) {
case FAMILY_GAUSSIAN:
if (tridiag)
tf_admm_gauss_tri(x, y, w, n, k, max_iter, lambda[i], df+i, beta+i*n,
alpha, u, obj+i*(1+max_iter), iter+i, rho * lambda[i],
obj_tol, A0, A1, verbose);
else
tf_admm_gauss(x, y, w, n, k, max_iter, lambda[i], df+i, beta+i*n,
alpha, u, obj+i*(1+max_iter), iter+i, rho * lambda[i],
obj_tol, DktDk, verbose);
break;
case FAMILY_LOGISTIC:
tf_admm_glm(x, y, w, n, k, max_iter, lambda[i], tridiag, df+i, beta+i*n,
alpha, u, obj+i*(1+max_iter_newton), iter+i, rho * lambda[i], obj_tol,
obj_tol_newton, alpha_ls, gamma_ls, max_iter_ls, max_iter_newton,
DktDk, A0, A1, &logi_b, &logi_b1, &logi_b2, verbose);
break;
case FAMILY_POISSON:
tf_admm_glm(x, y, w, n, k, max_iter, lambda[i], tridiag, df+i, beta+i*n,
alpha, u, obj+i*(1+max_iter_newton), iter+i, rho * lambda[i], obj_tol,
obj_tol_newton, alpha_ls, gamma_ls, max_iter_ls, max_iter_newton,
DktDk, A0, A1, &pois_b, &pois_b1, &pois_b2, verbose);
break;
default:
printf("Unknown family, stopping calculation.\n");
status[i] = 2;
}
/* If there any NaNs in beta: reset beta, alpha, u */
if (has_nan(beta + i*n, n))
{
double yc = weighted_mean(y,w,n);
switch(family) {
case FAMILY_POISSON:
yc = (yc > 0) ? log(yc) : -DBL_MAX;
break;
case FAMILY_LOGISTIC:
yc = (yc > 0) ? ( yc < 1 ? log(yc/(1-yc)) : DBL_MAX) : -DBL_MAX;
break;
default: break;
}
for (j = 0; j < n; j++) beta[i*n + j] = yc;
for (j = 0; j < n-k; j++) alpha[j] = 0;
for (j = 0; j < n; j++) u[j] = w[j] * (beta[i*n+j] - y[j]) / (rho * lambda[i]);
glmgen_qrsol (Dkt_qr, u);
if (tridiag) for (j = 0; j < n*k; j++) alpha[j] = u[j] = 0;
status[i] = 1;
}
}
cs_spfree(D);
cs_spfree(Dt);
cs_spfree(Dk);
cs_spfree(Dkt);
cs_spfree(DktDk);
glmgen_gqr_free(Dt_qr);
glmgen_gqr_free(Dkt_qr);
free(beta_max);
free(temp_n);
free(alpha);
free(u);
free(v);
if (tridiag && k>0)
{
free(A0);
free(A1);
}
}
void FloatAuto::adjust_tangents()
// recalculates tangents if current mode
// implies automatic adjustment of tangents
{
if(!autos) return;
if(tangent_mode == SMOOTH)
{
// normally, one would use the slope of chord between the neighbours.
// but this could cause the curve to overshot extremal automation nodes.
// (e.g when setting a fade node at zero, the curve could go negative)
// we can interpret the slope of chord as a weighted mean value, where
// the length of the interval is used as weight; we just use other
// weights: intervall length /and/ reciprocal of slope. So, if the
// connection to one of the neighbours has very low slope this will
// dominate the calculated tangent slope at this automation node.
// if the slope goes beyond the zero line, e.g if left connection
// has positive and right connection has negative slope, then
// we force the calculated tangent to be horizontal.
float s, dxl, dxr, sl, sr;
calculate_slope((FloatAuto*) previous, this, sl, dxl);
calculate_slope(this, (FloatAuto*) next, sr, dxr);
if(0 < sgn(sl) * sgn(sr))
{
float wl = fabs(dxl) * (fabs(1.0/sl) + 1);
float wr = fabs(dxr) * (fabs(1.0/sr) + 1);
s = weighted_mean(sl, sr, wl, wr);
}
else s = 0; // fixed hoizontal tangent
control_in_value = s * control_in_position;
control_out_value = s * control_out_position;
}
else
if(tangent_mode == LINEAR)
{
float g, dx;
if(previous)
{
calculate_slope(this, (FloatAuto*)previous, g, dx);
control_in_value = g * dx / 3;
}
if(next)
{
calculate_slope(this, (FloatAuto*)next, g, dx);
control_out_value = g * dx / 3;
} }
else
if(tangent_mode == TFREE && control_in_position && control_out_position)
{
float gl = control_in_value / control_in_position;
float gr = control_out_value / control_out_position;
float wl = fabs(control_in_value);
float wr = fabs(control_out_value);
float g = weighted_mean(gl, gr, wl, wr);
control_in_value = g * control_in_position;
control_out_value = g * control_out_position;
}
}
