Exemple #1
0
 /*
 * helper function to allocate all the memory necessary to respresent the responsabilities.
 * @param N_meas number of points
 * @param N_comp number of clusters
 */
 void allocate(int N_meas, int N_comp)
 {
     R.assign(N_meas,ColumnVector(N_comp));
     responsibleCluster.assign(N_meas,-1);
     sumR.resize(N_meas);
     N.resize(N_comp);   
 }
Exemple #2
0
//---------------------------------------------------------------------------
void TEstimation::preparePosteriorDensityPoints(){
	 //prepare vector for points at which the density (also prior!!!) is estimated
	   theta=ColumnVector(posteriorDensityPoints);
	   //float step=1.2/(posteriorDensityPoints-1);
	   //for(int j=1; j<=posteriorDensityPoints; ++j) theta(j)=-0.1+(j-1)*step;
	   //since the parameters are standardized between 0 and 1, we can easy calculate the posterior between 0 and 1
	   step=1.0/(posteriorDensityPoints-1.0);
	   for(int j=0; j<posteriorDensityPoints; ++j) theta(j+1)=(j)*step;
}
Exemple #3
0
void CParam::init_pi() {
  nu_short = ColumnVector(K - 1);
  nu_short = 0.1 ;    // WAS 0.1 starting value

  logpi = ColumnVector(K);
  pi= ColumnVector(K);

  double Sum_logOneMinusVg = 0.0 ;
  for (int i= 1; i<= (K-1); i++) {
    double nu_k = nu_short(i) ;
    logpi(i) = log(nu_k) + Sum_logOneMinusVg ;
    pi(i) = exp(logpi(i)) ;

    double one_minus_V = ( 1.0 - nu_short(i) ) ;
    Sum_logOneMinusVg = Sum_logOneMinusVg + log(one_minus_V) ;
  }
  logpi(K) = Sum_logOneMinusVg ;
  pi(K) = exp(Sum_logOneMinusVg) ;
}
Exemple #4
0
void CParam::init_sigmamu(CData &Data) {
  mu_bar = ColumnVector(n_var_independent);
  ColumnVector log_obs_mean(n_var), log_obs_sd(n_var) ;
  Matrix logD_editpassing(n_sample-Data.n_faulty,n_var) ;
  for (int i_sample=1, count = 1; i_sample<=n_sample; i_sample++){
    if (Data.is_case(i_sample,0)) {
      logD_editpassing.row(count++) = Data.log_D_Observed.row(i_sample);
    }
  }

  for (int i_var=1; i_var<=n_var; i_var++){
    ColumnVector temp_col = logD_editpassing.column(i_var) ;
    log_obs_mean(i_var) = 1.0/(n_sample-Data.n_faulty)*(temp_col.sum()) ;
    ColumnVector temp_mean(n_sample-Data.n_faulty) ; temp_mean = log_obs_mean(i_var) ;
    Matrix SumSq = (temp_col-temp_mean).t()*(temp_col-temp_mean) ;
    log_obs_sd(i_var) = sqrt( 1.0/(n_sample-Data.n_faulty-1)*SumSq(1,1) ) ;
  }

  Data.UpdateCompactVector(mu_bar,log_obs_mean);
  DiagonalMatrix LSigma_temp(n_var_independent);
  for (int i=1; i<= n_var_independent; i++){
    LSigma_temp(i) = log_obs_sd(i);
  }
  DiagonalMatrix LSigma_temp_i = LSigma_temp.i();

  LSIGMA = vector<LowerTriangularMatrix>(K);
  LSIGMA_i = vector<LowerTriangularMatrix>(K);
  SIGMA = vector<SymmetricMatrix>(K);
  for (int k=0 ; k<K; k++) {
    LSIGMA[k] = LSigma_temp;
    LSIGMA_i[k] = LSigma_temp_i;
    SIGMA[k] << LSigma_temp * LSigma_temp; //lazy
  }
  logdet_and_more = ColumnVector(K); logdet_and_more = -0.5*n_var* LOG_2_PI + logdet(LSIGMA_i[0]);

  Mu = Matrix((n_var_independent),K);      // Note that mu_k is Mu.column(k)
  for (int k=1; k<=K; k++){
    Mu.column(k) = mu_bar ;  // starting value
  }
}
Exemple #5
0
void CParam::init_logUnif_y_tilde(CData &Data,CFeasibilityMap &FM, Uniform &randUnif, int n_simul) {
  logUnif_y_tilde = ColumnVector(Data.n_faulty);
  for (int i = 1; i <= Data.n_faulty; i++) {
    ColumnVector s_i = S_Mat.column(i);
    int i_original = Data.Faulty2Original[i-1];
    // double log_f_y_tilde_q = 0;  Commented by HANG on 5/16/2015
    if (FM.useMap && Data.is_case(i_original,2)) {
      logUnif_y_tilde(i) = FM.Simulate_logUnif_case2(CFeasibilityMap::s_to_tau_fn(s_i),i_original,n_simul,randUnif);
    } else {
      logUnif_y_tilde(i) = Data.get_logUnif_y_tilde(s_i, CFeasibilityMap::s_to_tau_fn(s_i),i_original); 
    } 
    //logUnif_y_tilde(i) = Data.get_logUnif_y_tilde(s_i, CFeasibilityMap::s_to_tau_fn(s_i),i_original);
  }
}
bool CFeasibilityMap::SolveLP(Matrix &A, ColumnVector &b, ColumnVector &x) {
  lprec *lp ;
  int n_row = A.nrows(); int n_col = A.ncols();
  x = ColumnVector(n_col); x = 0;
  lp = make_lp(0,n_col) ; 
  
  double *input_row = new double[1+n_col];
  for (int i_row=1; i_row<=n_row; i_row++){
      input_row[0] = 0 ; // The first zero is for matrix form
      for (int j=1; j<=n_col; j++){
          input_row[j] = A(i_row,j) ;
      }
      add_constraint(lp, input_row, LE, b(i_row)) ;
  }
  delete [] input_row;
  
  double *input_obj = new double[1+n_col];    // The first zero is for matrix form
  input_obj[0] = 0 ;
  for (int j=1; j<=n_col; j++){
      input_obj[j] = 1 ;
  }
  set_obj_fn(lp, input_obj) ;
  delete [] input_obj;
  set_verbose(lp, IMPORTANT); // NEUTRAL (0), IMPORTANT (3), NORMAL (4), FULL (6)
  bool is_feasible = (solve(lp)==0); // 0: feasible solution found,  2: not found
                                     // solution for minimizing objective function                               
  double* x_min = new double[n_col];
  double* x_max = new double[n_col];                      
  if (is_feasible) {
    get_variables(lp, x_min);
    set_maxim(lp);
    is_feasible = (solve(lp)==0); // 0: feasible solution found,  2: not found
    if (is_feasible) {
      get_variables(lp, x_max);
      for (int i = 0; i < n_col; i++) {
        x(i+1) = (x_min[i] + x_max[i]) / 2.0;
      }
    }
  }
  
  delete [] x_min;
  delete [] x_max;
                                     
  delete_lp(lp);
  return is_feasible;
}
Exemple #7
0
 /*
 * constructor.
 * @param N_comp number of clusters
 * @param N_comp_eff number of effective clusters
 * @param D dimension
 * @param Beta_init beta
 * @param Nu_init nu
 * @param Alfa_init alfa
 * @param W_init W
 * @param Sigma_init sigma
 * @param Pi_init pi
 */
 Cluster_parameters(int N_comp=1, int N_comp_eff=0, int D=1, double Beta_init = 0.05, double Nu_init = 10.0, double Alfa_init = 1.0 ,double W_init = 10.0, double Sigma_init = 1.0 , double Pi_init = 0.0)
    : N_comp_effective(N_comp_eff), Beta(N_comp,Beta_init), Nu(N_comp,Nu_init), Alfa(N_comp,Alfa_init), Pi(N_comp)
 {
     // allocate 
     Pi = Pi_init;
     M.assign(N_comp,ColumnVector(D));
     Matrix W_el(D,D);
     W_el = 0.0;
     Matrix Sigma_el(D,D);
     Sigma_el = 0.0;
     for (int d=1; d<=D; d++)
     {
         W_el(d,d) = W_init;
         Sigma_el(d,d) = Sigma_init;
     }
     W.assign(N_comp,W_el);
     Sigma.assign(N_comp,Sigma_el);
 }
Exemple #8
0
// Assumes that there is either no exogenous variables or there is a single
// exogenous variable that is constant and equal to one.  The unconditional
// mean is obtained from the reduced form companion matrix.
TDenseVector UnconditionalMean(const TDenseMatrix &A0, const TDenseMatrix &Aplus, bool IsConstant)
{
  int n_lags=NumberLags(A0,Aplus,IsConstant), n_vars=A0.cols;
  if (!IsConstant) return TDenseVector(n_vars,0.0);
  TDenseMatrix B=ReducedForm(A0,Aplus);
  if (n_lags == 0) return ColumnVector(B,0);
  TDenseMatrix C=CompanionMatrix(B,n_lags);
  TDenseVector b(n_vars*n_lags,0.0);
  b.InsertColumnVector(0,B,n_vars*n_lags,0,n_vars-1);
  try
    {
      return SubVector(InverseMultiply(Identity(n_vars*n_lags) - C,b),0,n_vars-1);
    }
  catch (dw_exception &e)
    {
      throw dw_exception("UnconditionalMean(): Unconditional mean does not exist");
    }
}
Exemple #9
0
void CParam::init_z_in() {
  z_in = ColumnVector(n_sample) ;
  for (int i_sample=1; i_sample<=n_sample; i_sample++) {
    // calculate pi_tilde_in
    ColumnVector logN_unnorm(K);
    ColumnVector y_compact = Y_in_compact.row(i_sample).t() ;
    for (int k=1; k<=K; k++) {
      ColumnVector mu_k = Mu.column(k);
      logN_unnorm(k) = log_MVN_fn( y_compact, mu_k, LSIGMA_i[k-1],logdet_and_more(k));
    }
    double max_logN_unnorm = logN_unnorm.maximum();

    ColumnVector pi_tilde_in_unnorm(K); pi_tilde_in_unnorm = 0.0 ;
    for (int k=1; k<=K; k++){
      pi_tilde_in_unnorm(k) = pi(k) * exp(logN_unnorm(k)-max_logN_unnorm);
    }
    ColumnVector pi_tilde_in = (1.0/pi_tilde_in_unnorm.sum()) * pi_tilde_in_unnorm ;

    z_in(i_sample) = rdiscrete_fn(pi_tilde_in);
  }
}
Exemple #10
0
 /*
 * helper function to allocate all the memory necessary to respresent the cluster parameters.
 * @param N_comp number of clusters
 * @param N_comp_eff number of effective components
 * @param D dimension
 * @param Beta_init beta
 * @param Nu_init nu
 * @param Alfa_init alfa
 * @param W_init W
 * @param Sigma_init sigma
 * @param Pi_init pi
 */
 void allocate(int N_comp, int N_comp_eff=0, int D=1, double Beta_init = 0.05, double Nu_init = 10.0, double Alfa_init = 1.0 ,double W_init = 10.0, double Sigma_init = 1.0 , double Pi_init = 0.0)
 {
     N_comp_effective = N_comp_eff;
     Beta.assign(N_comp,Beta_init);
     Nu.assign(N_comp,Nu_init);
     Alfa.assign(N_comp,Alfa_init);
     Pi.resize(N_comp);
     Pi= Pi_init;
     M.assign(N_comp,ColumnVector(D));
     Matrix W_el(D,D);
     W_el = 0.0;
     Matrix Sigma_el(D,D);
     Sigma_el = 0.0;
     for (int d=1; d<=D; d++)
     {
         W_el(d,d) = W_init;
         Sigma_el(d,d) = Sigma_init;
     }
     W.assign(N_comp,W_el);
     Sigma.assign(N_comp,Sigma_el);
 }
Exemple #11
0
void CParam::initizalize(CData &Data, int nComp, CFeasibilityMap &FM, Uniform &randUnif, int n_simul){
  // Set the minimum value of (no. of correct records)/(no. of all records)
  toolarge_nout = (1-min_Prob_A)/min_Prob_A * Data.n_sample ;

  n_var = Data.n_var;
  n_var_independent = n_var - Data.n_balance_edit;
  K = nComp;
  n_sample = Data.n_sample;

  alpha = 1.0 ;
  Phi = IdentityMatrix(n_var_independent) * 5.0;
  init_pi();
  init_sigmamu(Data);
  init_Y_in(Data);
  init_z_in();

  S_Mat = Data.initial_S_Mat.t();
  init_logUnif_y_tilde(Data,FM,randUnif,n_simul);

  n_z = ColumnVector(K); n_z = 0 ;
  Sigma_k_inv_ll = Matrix(K,n_var_independent);  Sigma_k_inv_ll=0.0;
  X_bar= Matrix(n_var_independent,K); X_bar = 0.0 ;
  CalculateInitProbA(Data);
}
Exemple #12
0
RBFInterpolator::RBFInterpolator(vector<real> x, vector<real> y, vector<real> z, vector<real> f)
{
	successfullyInitialized = false; // default value for if we end init prematurely.

	M = f.size();

	// all four input vectors must have the same length.
	if ( x.size() != M || y.size() != M || z.size() != M )
		return;	

	ColumnVector F = ColumnVector(M + 4);
	P = Matrix(M, 3);

	Matrix G(M + 4,M + 4);

	// copy function values
	for (int i = 1; i <= M; i++)
		F(i) = f[i-1];
	
	F(M+1) = 0;  F(M+2) = 0;  F(M+3) = 0;  F(M+4) = 0;

	// fill xyz coordinates into P 
	for (int i = 1; i <= M; i++)
	{
		P(i,1) = x[i-1];
		P(i,2) = y[i-1];
		P(i,3) = z[i-1];
	}

	// the matrix below is symmetric, so I could save some calculations Hmmm. must be a todo
	for (int i = 1; i <= M; i++)
	for (int j = 1; j <= M; j++)
	{
		real dx = x[i-1] - x[j-1];
		real dy = y[i-1] - y[j-1];
		real dz = z[i-1] - z[j-1];

		real distance_squared = dx*dx + dy*dy + dz*dz;

		G(i,j) = g(distance_squared);
	}

	//Set last 4 columns of G
	for (int i = 1; i <= M; i++)
	{
		G( i, M+1 ) = 1;
		G( i, M+2 ) = x[i-1];
		G( i, M+3 ) = y[i-1];
		G( i, M+4 ) = z[i-1];
	}

	for (int i = M+1; i <= M+4; i++)
	for (int j = M+1; j <= M+4; j++)
		G( i, j ) = 0;

	//Set last 4 rows of G
	for (int j = 1; j <= M; j++)
	{
		G( M+1, j ) = 1;
		G( M+2, j ) = x[j-1];
		G( M+3, j ) = y[j-1];
		G( M+4, j ) = z[j-1];
	}

	Try 
	{ 
		Ginv = G.i(); 

		A = Ginv*F;
		successfullyInitialized = true;
	}
    CatchAll { cout << BaseException::what() << endl; }

}
Exemple #13
0
ColumnVector SDThread::calcSD( int id )
{
    return ColumnVector( 20 );
}
Exemple #14
0
 /*
 * constructor.
 * @param N_meas number of points
 * @param N_comp number of clusters
 * @param N_meas_eff number of effective points
 * @param N_comp_eff number of effective clusters
 */
 Respons_struct(int N_meas = 181, int N_comp = 1, int N_meas_eff = 0, int N_comp_eff = 0)
     : R(N_meas,ColumnVector(N_comp)), sumR(N_meas), N(N_comp), N_meas_effective(N_meas_eff), N_comp_effective(N_comp_eff)
 {
 }
Exemple #15
0
void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
                     const ColumnVector & dqp,
                     ColumnVector & ltorque, ColumnVector & dtorque)
{
   int i;
   ColumnVector z0(3);
   Matrix Rt, temp;
   Matrix Q(3,3);
   ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
   ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
   if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
   if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
   ltorque = ColumnVector(dof);
   dtorque = ColumnVector(dof);
   set_q(q);
   w = new ColumnVector[dof+1];
   wp = new ColumnVector[dof+1];
   vp = new ColumnVector[dof+1];
   a = new ColumnVector[dof+1];
   f = new ColumnVector[dof+1];
   n = new ColumnVector[dof+1];
   F = new ColumnVector[dof+1];
   N = new ColumnVector[dof+1];
   p = new ColumnVector[dof+1];
   dw = new ColumnVector[dof+1];
   dwp = new ColumnVector[dof+1];
   dvp = new ColumnVector[dof+1];
   da = new ColumnVector[dof+1];
   df = new ColumnVector[dof+1];
   dn = new ColumnVector[dof+1];
   dF = new ColumnVector[dof+1];
   dN = new ColumnVector[dof+1];
   dp = new ColumnVector[dof+1];
   w[0] = ColumnVector(3);
   wp[0] = ColumnVector(3);
   vp[0] = gravity;
   dw[0] = ColumnVector(3);
   dwp[0] = ColumnVector(3);
   dvp[0] = ColumnVector(3);
   z0 = 0.0;
   Q = 0.0;
   Q(1,2) = -1.0;
   Q(2,1) = 1.0;
   z0(3) = 1.0;
   w[0] = 0.0;
   wp[0] = 0.0;
   dw[0] = 0.0;
   dwp[0] = 0.0;
   dvp[0] = 0.0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      p[i] = ColumnVector(3);
      p[i](1) = links[i].get_a();
      p[i](2) = links[i].get_d() * Rt(2,3);
      p[i](3) = links[i].get_d() * Rt(3,3);
      if(links[i].get_joint_type() != 0) {
         dp[i] = ColumnVector(3);
         dp[i](1) = 0.0;
         dp[i](2) = Rt(2,3);
         dp[i](3) = Rt(3,3);
      }
      if(links[i].get_joint_type() == 0) {
         w[i] = Rt*(w[i-1] + z0*qp(i));
         dw[i] = Rt*(dw[i-1] + z0*dqp(i));
         wp[i] = Rt*(wp[i-1] + vec_x_prod(w[i-1],z0*qp(i)));
         dwp[i] = Rt*(dwp[i-1]
               + vec_x_prod(dw[i-1],z0*qp(i))
               + vec_x_prod(w[i-1],z0*dqp(i))
               );
         vp[i] = vec_x_prod(wp[i],p[i])
               + vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
               + Rt*(vp[i-1]);
         dvp[i] = vec_x_prod(dwp[i],p[i])
               + vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
               + vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
               + Rt*dvp[i-1];
      } else {
         w[i] = Rt*w[i-1];
         dw[i] = Rt*dw[i-1];
         wp[i] = Rt*wp[i-1];
         dwp[i] = Rt*dwp[i-1];
         vp[i] = Rt*(vp[i-1]
               + vec_x_prod(w[i],z0*qp(i))) * 2.0
               + vec_x_prod(wp[i],p[i])
               + vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
         dvp[i] = Rt*(dvp[i-1]
               + (vec_x_prod(dw[i],z0*qp(i)) * 2.0
               + vec_x_prod(w[i],z0*dqp(i))))
               + vec_x_prod(dwp[i],p[i])
               + vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
               + vec_x_prod(w[i],vec_x_prod(dw[i],p[i]));
      }
      a[i] = vec_x_prod(wp[i],links[i].r)
            + vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
            + vp[i];
      da[i] = vec_x_prod(dwp[i],links[i].r)
            + vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
            + vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
            + dvp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] = a[i] * links[i].m;
      N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
      dF[i] = da[i] * links[i].m;
      dN[i] = links[i].I*dwp[i] + vec_x_prod(dw[i],links[i].I*w[i])
            + vec_x_prod(w[i],links[i].I*dw[i]);
      if(i == dof) {
         f[i] = F[i];
         n[i] = vec_x_prod(p[i],f[i])
               + vec_x_prod(links[i].r,F[i]) + N[i];
         df[i] = dF[i];
         dn[i] = vec_x_prod(p[i],df[i])
               + vec_x_prod(links[i].r,dF[i]) + dN[i];
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
               + vec_x_prod(links[i].r,F[i]) + N[i];
         df[i] = links[i+1].R*df[i+1] + dF[i];
         dn[i] = links[i+1].R*dn[i+1] + vec_x_prod(p[i],df[i])
               + vec_x_prod(links[i].r,dF[i]) + dN[i];
      }
      if(links[i].get_joint_type() == 0) {
         temp = ((z0.t()*links[i].R)*n[i]);
         ltorque(i) = temp(1,1);
         temp = ((z0.t()*links[i].R)*dn[i]);
         dtorque(i) = temp(1,1);
      } else {
         temp = ((z0.t()*links[i].R)*f[i]);
         ltorque(i) = temp(1,1);
         temp = ((z0.t()*links[i].R)*df[i]);
         dtorque(i) = temp(1,1);
      }
   }

   delete []dp;
   delete []dN;
   delete []dF;
   delete []dn;
   delete []df;
   delete []da;
   delete []dvp;
   delete []dwp;
   delete []dw;
   delete []p;
   delete []N;
   delete []F;
   delete []n;
   delete []f;
   delete []a;
   delete []vp;
   delete []wp;
   delete []w;
}