Example #1
0
//------------------------------------------------------------------------------
void HamiltonMatrix::computeMatrixElements()
{
    cout << "Setting up the Hamilton matrix " << endl;
    int phase;
    int nStates = slaterDeterminants.size();

    bitset<BITS> newState;
    int i, j, k, l;
    double interactionElement;
    H = zeros(nStates, nStates);
    Ht = zeros(nStates, nStates);

    for (int st = 0; st < nStates; st++) {
        for (int b = 0; b < (int)interactions.n_rows; b++) {
            newState = slaterDeterminants[st];
            i = interactions(b, 0);
            j = interactions(b, 1);
            k = interactions(b, 2);
            l = interactions(b, 3);

            // Using the two body operator. Mathematics: a+(i)a+(j)a-(l)a-(k)|psi>
            phase = 1;

            newState = removeParticle(k, newState);
            phase *= sign(k, newState);

            newState = removeParticle(l, newState);
            phase *= sign(l, newState);

            newState = addParticle(j, newState);
            phase *= sign(j, newState);

            newState = addParticle(i, newState);
            phase *= sign(i, newState);

            if (newState[BITS - 1] != 1) {
                interactionElement = interactions(b, 4);
                // Searching for the new state to compute the matrix elements.
                for (int st2 = 0; st2 < (int)slaterDeterminants.size(); st2++) {
                    if (newState == slaterDeterminants[st2]) {
                        H(st, st2) += interactionElement * phase;
                        break;
                    }
                }
            }
        }

        // One body energies
        H(st, st) += spsEnergy(st);
    }

#if DEBUG
    cout << H << endl;

    //        // Symmetrizing the Hamilton matrix
    //        for (int i = 0; i < H.n_rows; i++) {
    //            for (int j = 0; j < i; j++) {
    //                H(j, i) = H(i, j);
    //            }
    //        }
    cout << "Completed generating the Hamilton matrix" << endl;
#endif
}
Example #2
0
 vertex_data() {
   pvec = zeros(D);
   weight = zeros(D);
   bias = 0;
 }
Example #3
0
int main(int argc, char *argv[]) {
    benchmark_t benchmarks[] = {
        (benchmark_t){"sht", setup_sht, execute_sht, finish_sht, 1, 0.0},
#if HAS_PSHT
        (benchmark_t){"psht", setup_psht, execute_psht, finish_psht, 1, 0.0},
#endif
        {NULL, NULL, NULL, NULL, 0, 0.0}
    };

    benchmark_t *sht_benchmark = &benchmarks[0];
    benchmark_t *psht_benchmark = &benchmarks[1];

    double t0, t1, dtmin, tit0, tit1, dt;
    int n, i, j, should_run;
    benchmark_t *pbench;
#ifdef HAS_PPROF
    char profilefile[MAXPATH];
#endif
    char *resourcename;
    char timestr1[MAXTIME], timestr2[MAXTIME];

    char *stats_filename = NULL;
    char *stats_mode;

    int c;
    int got_threads, miniter;
    double mintime;
    char *resource_path;

    /* Parse options */
    sht_resourcefile = NULL;
    Nside = -1;
    miniter = 1;
    mintime = 0;
    opterr = 0;
    sht_nmaps = 1;
    do_ffts = -1;
    sht_flags = WAVEMOTH_MEASURE;

    while ((c = getopt (argc, argv, "r:N:j:n:t:S:k:a:o:FEC")) != -1) {
        switch (c) {
        case 'r':
            sht_resourcefile = optarg;
            break;
        case 'N':
            Nside = atoi(optarg);
            break;
        case 'j':
            N_threads = atoi(optarg);
            break;
        case 'n':
            miniter = atoi(optarg);
            break;
        case 't':
            mintime = atof(optarg);
            break;
        case 'E':
            sht_flags &= ~WAVEMOTH_MEASURE;
            break;
        case 'C':
            sht_flags |= WAVEMOTH_NO_RESOURCE_COPY;
            break;
        case 'a':
            stats_filename = optarg;
            stats_mode = "a";
            break;
        case 'o':
            stats_filename = optarg;
            stats_mode = "w";
            break;
        case 'S':
            sht_m_stride = atoi(optarg);
            do_ffts = 0;
            break;
        case 'F':
            do_ffts = 0;
            break;
        case 'k':
            sht_nmaps = atoi(optarg);
            break;
        }
    }
    argv += optind;
    argc -= optind;

    for (pbench = benchmarks; pbench->execute; ++pbench) {
        pbench->should_run = (argc == 0);
    }
    for (j = 0; j != argc; ++j) {
        for (pbench = benchmarks; pbench->execute; ++pbench) {
            if (strcmp(argv[j], pbench->name) == 0) {
                pbench->should_run = 1;
            }
        }
    }

    if (do_ffts == -1) do_ffts = 1;

    /* Resource configuration */
    resource_path = getenv("SHTRESOURCES");
    if (sht_resourcefile != NULL) {
        wavemoth_configure("");
        wavemoth_query_resourcefile(sht_resourcefile, &Nside, &lmax);
    } else {
        check(resource_path != NULL, "Please define SHTRESOURCES or use -r switch");
        wavemoth_configure(resource_path);
        lmax = 2 * Nside;
    }

    fprintf(stderr, "Using %d threads, %d maps, %s, m-thinning %d\n", N_threads, sht_nmaps,
            do_ffts ? "with FFTs" : "without FFTs", sht_m_stride);


    /* Set up input and output */
    size_t npix = 12 * Nside * Nside;
    sht_input = gauss_array(((lmax + 1) * (lmax + 2)) / 2 * 2 * sht_nmaps);
    sht_output = zeros(npix * sht_nmaps);
    psht_output = zeros(npix * sht_nmaps);

    /* Wavemoth benchmark */
    for (pbench = benchmarks; pbench->execute; ++pbench) {
        if (!pbench->should_run) continue;

        printf("%s:\n", pbench->name);
        pbench->setup();
        printf("  Warming up\n");
        pbench->execute(NULL);
        printf("  Executing\n");

#ifdef HAS_PPROF
        snprintf(profilefile, MAXPATH, "profiles/%s.prof", pbench->name);
        profilefile[MAXPATH - 1] = '\0';
        ProfilerStart(profilefile);
#endif

        printf("  ");
        pbench->min_time = benchmark(pbench->name, pbench->execute, NULL,
                                     miniter, mintime);

#ifdef HAS_PPROF
        ProfilerStop();
#endif

        pbench->finish();
    }

    printf("Runtime sht/psht: %f\n", sht_benchmark->min_time / psht_benchmark->min_time);
    printf("Speedup psht/sht: %f\n", psht_benchmark->min_time / sht_benchmark->min_time);

    double rho = 0;
    for (j = 0; j != sht_nmaps; ++j) {
        double z = relative_error(npix, psht_output + j * npix, 1, sht_output + j, sht_nmaps);
        rho = fmax(z, rho);
    }
    printf("Relative error: %e\n", rho);

    if (stats_filename != NULL) {
        /* Write result to file as a line in the format
           nside lmax nmaps nthreads wavemoth_full_time wavemoth_legendre_time psht_time relative_error
        */
        FILE *f = fopen(stats_filename, stats_mode);
        if (!f) {
            fprintf(stderr, "Could not open %s in mode %s\n", stats_filename, stats_mode);
        } else {
            fprintf(f, "%d %d %d %d %.15e %.15e %.15e %.15e\n",
                    Nside, lmax, sht_nmaps, N_threads,
                    sht_benchmark->min_time, min_legendre_dt, psht_benchmark->min_time,
                    rho);
            fclose(f);
        }
    }


    free(psht_output);
    free(sht_output);
    free(sht_input);
}
Example #4
0
Matrix cgs( Matrix& A, Matrix& b, Matrix& x0, double tol )
{

	//Check matrix and right hand side vector inputs have appropriate sizes
	int m = size( A, 1 );                         //number of rows of A
	int n = size( A, 2 );                         //number of columns of A
	
	if( m != n )                                  //if A is not square
	{
		exit(1);
	}
	if( size( b, 1 ) != n || size( b, 2) != 1 )   //if b does not match A or is not a column vector
	{
		exit(1);
	}
	if( size( x0, 1 ) != m || size( x0, 2 ) !=  1)//if x0 does not match A or is not a column vector
	{
		exit(1);
	}
	
	if( !iscolumn( b ) )
	{
		exit(1);
	}
	//  Assign default values to unspecified parameters
	//minimum tolerance is 1e-6
	if( tol >  1e-6 )
	{
		tol = 1e-6;
	}
	
	int maxit = n < 20 ? n:20;              //maximum iterations are 20
	Matrix x = x0;                          //initial guess x0
	
	//  Check for all zero right hand side vector => all zero solution
	double n2b = norm(b);                   //  Norm of rhs vector, b
	if( fabs( n2b ) <= DBL_EPSILON )		//  if rhs vector is all zeros
	{
		x = zeros( n, 1 );                  //  then  solution is all zeros
		return x;				            
	}
	
	//  Set up for the method
	int flag = 1;
	Matrix xmin = x;                        //  Iterate which has minimal residual so far
	int imin = 0;                           //  Iteration at which xmin was computed
	double tolb = tol * n2b;                //  Relative tolerance
	Matrix r = b - A*x;                     //  Residual vector
	double normr = norm(r);                 //  Norm of residual
	double normr_act = normr;

	// other column vectors needed
	Matrix u;
	Matrix p;
	Matrix q;
	Matrix vh;
	Matrix rtvh;
	Matrix uh;
	Matrix qh;
	
	if( normr <= tolb )                     //  Initial guess is a good enough solution
	{
		return x;
	}
	
	Matrix rt = r;                          //  Shadow residual
	Matrix resvec = zeros(maxit+1,1);       //  Preallocate vector for norms of residuals
	resvec(1) = normr;                      //  resvec(1) = norm(b-A*x0)
	double normrmin = normr;                //  Norm of residual from xmin
	Matrix rho;                             //double rho = 1;
	rho = 1.0;
	Matrix rho1;
	double alpha;
	double beta;
	int stag = 0;                           //  stagnation of the method
	int moresteps = 0;
	int maxmsteps = (n/50) < 5 ? (n/50):5;  //  maxmsteps is min( floor(n/50), 5, n-maxit )
	maxmsteps = maxmsteps < n-maxit ? maxmsteps:n-maxit;
	int maxstagsteps = 3;

	//  loop over maxit iterations (unless convergence or failure)
	for( int i = 1; i <= maxit; i++ )
	{
		rho1 = rho;
		rho = tr( rt )*r;
		if( fabs( rho(1) ) <= DBL_EPSILON || fabs( rho(1) ) >= DBL_MAX )
		{
			flag = 4;
			break;
		}
		
		if(  i == 1 )
		{
			u = r;
			p = u;
		}
		else
		{
			beta = rho(1) / rho1(1);
			if( fabs( beta ) < DBL_EPSILON || fabs( beta ) > DBL_MAX  )
			{
				flag = 4;
				break;
			}
			u = r + beta * q;
			p = u + beta * (q + beta * p);
		}

		vh = A*p;
		rtvh = tr( rt ) * vh;
		if( fabs( rtvh( 1 ) )  <= DBL_EPSILON )
		{
			flag = 4;
			break;
		}
		else
		{
			alpha = rho(1) / rtvh(1);
		}
		
		if( fabs( alpha ) >= DBL_MAX )
		{
			flag = 4;
			break;
		}
		q = u - alpha * vh;
		uh = u+q;
	    
		//  Check for stagnation of the method
		if( fabs(alpha)*norm(uh) < DBL_EPSILON*norm(x) )
		{
			stag = stag + 1;
		}
		else
		{
			stag = 0;
		}
	    
		x = x + alpha * uh;            //  form the new iterate
		qh = A*uh;
		r = r - alpha * qh;
		normr = norm(r);
		normr_act = normr;
		resvec(i+1) = normr;
	    
		//  check for convergence
		if( normr <= tolb || stag >= maxstagsteps || moresteps )
		{
			r = b - A*x;
			normr_act = norm(r);
			resvec(i+1,1) = normr_act;
			if( normr_act <= tolb )
			{
				flag = 0;
				break;
			}
			else
			{
				if( stag >= maxstagsteps && moresteps == 0 )
				{
					stag = 0;
				}
				moresteps = moresteps + 1;
			}	
		}
	    
		if( normr_act < normrmin )         //  update minimal norm quantities
		{
			normrmin = normr_act;
			xmin = x;
		}
	}                                      //  for i = 1 : maxit
	
	if( flag == 0 )
	{
		//tolerance achieved
		return x;
	}
	else
	{
		if( flag == 4)
		{
			std::cerr<<"too small or too large a number encountered."<<std::endl;
		}
		r = b - A*xmin;
		if( norm(r) <= normr_act )
		{
			x = xmin;
		}
		return x;
	}
}
Example #5
0
	vertex_data() {
		pvec = zeros(D);
	}
  int init(PhantomState *s)
  {
    if(!s)
    {
      ROS_FATAL("Internal error. PhantomState is NULL.");
      return -1;
    }

    node_ = ros::NodeHandlePtr(new ros::NodeHandle);
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));

    // Vertical displacement from base_link to link_0. Defaults to Omni offset.
    pnode_->param(std::string("table_offset"), table_offset_, .135);

    // Force feedback damping coefficient
    pnode_->param(std::string("damping_k"), damping_k_, 0.001);

    // On startup device will generate forces to hold end-effector at origin.
    pnode_->param(std::string("locked"), locked_, false);

    // Check calibration status on start up and calibrate if necessary.
    pnode_->param(std::string("calibrate"), calibrate_, false);

    //Frame attached to the base of the phantom (NAME/base_link)
    base_link_name_ = "base_link";

    //Publish on NAME/pose
    std::string pose_topic_name = "pose";
    pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);

    //Publish button state on NAME/button
    std::string button_topic = "button";
    button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);

    //Subscribe to NAME/force_feedback
    std::string force_feedback_topic = "force_feedback";
    wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);

    //Frame of force feedback (NAME/sensable_origin)
    sensable_frame_name_ = "sensable_origin";

    for (int i = 0; i < 7; i++)
    {
      std::ostringstream stream1;
      stream1 << "link_" << i;
      link_names_[i] = std::string(stream1.str());
    }

    state_ = s;
    state_->buttons[0] = 0;
    state_->buttons[1] = 0;
    state_->buttons_prev[0] = 0;
    state_->buttons_prev[1] = 0;
    hduVector3Dd zeros(0, 0, 0);
    state_->velocity = zeros;
    state_->inp_vel1 = zeros; //3x1 history of velocity
    state_->inp_vel2 = zeros; //3x1 history of velocity
    state_->inp_vel3 = zeros; //3x1 history of velocity
    state_->out_vel1 = zeros; //3x1 history of velocity
    state_->out_vel2 = zeros; //3x1 history of velocity
    state_->out_vel3 = zeros; //3x1 history of velocity
    state_->pos_hist1 = zeros; //3x1 history of position
    state_->pos_hist2 = zeros; //3x1 history of position
    state_->lock = locked_;
    state_->lock_pos = zeros;
    state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);

    return 0;
  }
Example #7
0
void AvlIntTree::make_balance(void)
{
	if (balance == 2)
	{
		if (right->balance >= 0)
		{
			AvlKey tKey = key;
			uint tValue = value;
			key = right->key;
			value = right->value;
			right->key = tKey;
			right->value = tValue;
			AvlIntTree *tNode=right->right;
			right->right = right->left;
			right->left = left;
			left = right;
			right = tNode;
			balance = left->balance - 1;
			left->balance = 1 - left->balance;
			/* correct sums (values) */
			//value += left->value + (left->key.max - left->key.min + 1);
			if (left) { left->value = (left->right) ? left->value - value - zeros() : 0; }
		}
		else
		{
			AvlKey tKey = key;
			uint tValue = value;
			key = right->left->key;
			value = right->left->value;
			right->left->key = tKey;
			right->left->value = tValue;
			AvlIntTree *tNode = right->left->right;
			right->left->right = right->left->left;
			right->left->left = left;
			left = right->left;
			right->left = tNode;
			balance = 0;
			right->balance = (left->balance == -1) ? (1) : (0);
			left->balance = (left->balance == 1) ? (-1) : (0);
			/* correct sums (value) */
			//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
			//value = left->value + (left->key.max - left->key.min + 1);
			int tmp;
			tmp = left->value - right->value - right->zeros() - value - zeros();		// a
			value = right->value + right->zeros() + value;								// c
			left->value = tmp;
		}
	}
	else
	{
		if (balance == -2)
		{
			if (left->balance <= 0)
			{
				AvlKey tKey = key;
				uint tValue = value;
				key = left->key;
				value = left->value;
				left->key = tKey;
				left->value = tValue;
				AvlIntTree *tNode = left->left;
				left->left = left->right;
				left->right = right;
				right = left;
				left = tNode;
				balance = 1 + right->balance;
				right->balance = -1 - right->balance;
				/* correct sums (values) */
				//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
				if (right) { right->value = (right->right) ? value - right->value - right->zeros() : 0; }
			}
			else
			{
				AvlKey tKey = key;
				uint tValue = value;
				key = left->right->key;
				value = left->right->value;
				left->right->key = tKey;
				left->right->value = tValue;
				AvlIntTree *tNode = left->right->left;
				left->right->left = left->right->right;
				left->right->right = right;
				right = left->right;
				left->right = tNode;
				balance = 0;
				left->balance = (right->balance == 1) ? (-1) : (0);
				right->balance = (right->balance == -1) ? (1) : (0);
				/* correct sums (values) */
				//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
				//value = left->value + (left->key.max - left->key.min + 1);
				int tmp = left->value - value - zeros();
				value = right->zeros() + right->value + value;
				left->value = tmp;
			}
		}
	}
	//value = (right) ? right->value + right->zeros() : 0;
}
Example #8
0
// Given a vector of steer values, calculate the lean values associated with
// static equilibrium.  Also, return the indices of the steer/lean vectors
// which most nearly cause the u5^2 coefficient to go to zero.
static int staticEq(gsl_vector * lean, gsl_vector * pitch,
             const gsl_vector * steer, Whipple * bike)
{
  int i, N = lean->size, iter, iter_max = ITER_MAX, status;

  double ftol = FTOL;
  gsl_vector * x = gsl_vector_alloc(2);         // vector to store the solution
  gsl_vector * u5s_coefs = zeros(steer->size);
  const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
  gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
  gsl_multiroot_function_fdf f = {&static_f, &static_df, &static_fdf, 2, bike};
  bike->q1 = bike->q3 = 0.0;
  bike->calcPitch();
  gsl_vector_set(x, 0, bike->q1);
  gsl_vector_set(x, 1, bike->q2);
  gsl_multiroot_fdfsolver_set(s, &f, x);

  // for loop to loop over all values of steer
  for (i = 0; i < N; ++i) {
    bike->q3 = gsl_vector_get(steer, i);  // steer as a parameter
    iter = 0;
    do
    {
      ++iter;
      status = gsl_multiroot_fdfsolver_iterate(s);
      if (status)
        iterateError(status, "staticEq()", gsl_vector_get(steer, i));
      status = gsl_multiroot_test_residual(s->f, ftol);
    } while (status == GSL_CONTINUE && iter < iter_max);

    // Increase the tolerance by an order of magnitude to improve convergence
    if (iter == iter_max) {
      gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
      gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
      gsl_multiroot_fdfsolver_set(s, &f, x);
      increaseftol(&ftol, &i, iter_max, "staticEq()", bike->q3);
      continue;
    } // if

    // Store the lean into the lean vector
    gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
    gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));

    // Store the square of the coefficient of the u5^2 term;
    gsl_vector_set(u5s_coefs, i, bike->F[10] * bike->F[10]);
    ftol = FTOL;  // reset the error tolerance
  } // for

  // Assign a large value to the u5s_coefs vector near steer = 0 and steer = PI
  // This ensure the minimum will be near PI/2 where the two boudary curves
  // cross
  for (i = 0; i < 5; ++i) {
    gsl_vector_set(u5s_coefs, i, 10000.0);
    gsl_vector_set(u5s_coefs, u5s_coefs->size - 1 - i, 10000.0);
  }

  // Free dynamically allocated variables
  gsl_multiroot_fdfsolver_free(s);
  gsl_vector_free(x);
  i = gsl_vector_min_index(u5s_coefs);
  gsl_vector_free(u5s_coefs);
  return i;
} // staticEq()
Example #9
0
  /**
   *  Vertex update function - computes the least square step
   */
  void update(graphchi_vertex<VertexDataType, EdgeDataType> &vertex, graphchi_context &gcontext) {

    //compute only for user nodes
    if (vertex.id() >= std::min(M,(uint)end_user) || vertex.id() < (uint)start_user)
      return;

    vertex_data & vdata = latent_factors_inmem[vertex.id()];
    int howmany = (int)(N*knn_sample_percent);
    assert(howmany > 0 );
    if (vertex.num_outedges() == 0){
       mymutex.lock();
       users_without_ratings++;
       mymutex.unlock();
    }

    vec distances = zeros(howmany);
    ivec indices = ivec::Zero(howmany);
    for (int i=0; i< howmany; i++){
      indices[i]= -1;
    }
    std::vector<bool> curratings;
    curratings.resize(N);
    for(int e=0; e < vertex.num_edges(); e++) {
      //no need to calculate this rating since it is given in the training data reference
      assert(vertex.edge(e)->vertex_id() - M >= 0 && vertex.edge(e)->vertex_id() - M < N);
      curratings[vertex.edge(e)->vertex_id() - M] = true;
    }
    if (knn_sample_percent == 1.0){
      for (uint i=M; i< M+N; i++){
        if (curratings[i-M])
          continue;
        vertex_data & other = latent_factors_inmem[i];
        double dist;
        if (algo == SVDPP)
          svdpp_predict(vdata, other, 0, dist); 
        else if (algo == BIASSGD) 
	  biassgd_predict(vdata, other, 0, dist);
        else if (algo == RBM)
          rbm_predict(vdata, other, 0, dist);
        else assert(false);
        indices[i-M] = i-M;
        distances[i-M] = dist + 1e-10;
      }
    }
    else for (int i=0; i<howmany; i++){
      int random_other = ::randi(M, M+N-1);
      vertex_data & other = latent_factors_inmem[random_other];
      double dist;
      if (algo == SVDPP)
        svdpp_predict(vdata, other, 0, dist); 
      else if (algo == BIASSGD)
        biassgd_predict(vdata, other, 0, dist);
      else if (algo == RBM)
        rbm_predict(vdata, other, 0, dist);
      else assert(false);
        
      indices[i] = random_other-M;
      distances[i] = dist;
    }

    vec out_dist(num_ratings);
    ivec indices_sorted = reverse_sort_index2(distances, indices, out_dist, num_ratings);
    assert(indices_sorted.size() <= num_ratings);
    assert(out_dist.size() <= num_ratings);
    vdata.ids = indices_sorted;
    vdata.ratings = out_dist;
    if (debug)
      printf("Closest is: %d with distance %g\n", (int)vdata.ids[0], vdata.ratings[0]);

    if (vertex.id() % 1000 == 0)
      printf("Computing recommendations for user %d at time: %g\n", vertex.id()+1, mytimer.current_time());
  }
Example #10
0
// [[Rcpp::export]]
arma::rowvec euler_hat_ds_alt( 
                  arma::rowvec exog, arma::rowvec endog, arma::rowvec cont,
                  arma::mat exog_innov_integ, double betta, 
                  double gamma, arma::mat coeffs_cont, 
                  int n_exog, int n_endog, int n_cont, int n_fwd,
                  arma::rowvec rho, int n_integ, int N, arma::rowvec upper, 
                  arma::rowvec lower, bool cheby, arma::rowvec weights,
                  bool print_rhs=false ){
// Computes the single-period error on the Euler equations
  
//  double betta = params["betta"] ;
//  double gamma = params["gamma"] ;
  double rho_pref = - std::log( betta ) ;
      // Extract coefficients
  double NFA = endog( 0 ) ;
  double z1 = endog( 1 ) ;
  double z2 = endog( 2 ) ;
      // Extract the endogenous states
  double c1 = cont(0) ;
  double c2 = cont(1) ;
  double q = cont(15) ;
  double af1 = cont(16) ;
      // Extract controls
  
  mat exog_lead = zeros( n_integ, n_exog ) ;
      // Initalize the draws of the exogenous variables in the next period
  exog_lead = ones(n_integ) * ( rho % exog ) + exog_innov_integ ;
        // Multiply the most recent exogenous draw by the appropriate rho and
        // add the innovation
  
  rowvec integral = zeros<rowvec>( n_fwd ) ;
  mat integrand = zeros( n_integ, n_fwd ) ;
      // Initialize the right hand side.
      // Add the extra two columns for the extra equations for the prices 
      // r1 and r2
      
  for( int i = 0 ; i < n_integ ; i++ ){
    integrand.row(i) = integrand_ds( endog, exog_lead.row(i), gamma, 
                                      coeffs_cont, n_exog, n_endog, 
                                      n_cont, N, upper, lower, cheby ) ;
  }   // Compute the integral

  integral = weights * integrand ;
  
  if( print_rhs ){
    Rcout << "err: \n" << integrand << std::endl ;
    Rcout << "weights:" << weights << std::endl ;
    Rcout << "integral: " << integral << std::endl ;
    Rcout << "integral(0) - 1 = " << integral(0) - 1 << std::endl ;
    
  }
  
  rowvec out(4) ;
  out(0) = z1  + ( gamma * c1 - rho_pref + std::log( integral(0) ) ) ;
  out(1) = z2  - ( gamma * c1 - rho_pref + std::log( integral(1) ) ) ;
  out(2) = af1 + ( gamma * c2 - rho_pref + std::log( integral(2) ) ) ;
  out(3) = q   + ( gamma * c2 - rho_pref + std::log( integral(3) ) ) ;
      // The predictors.  Set up z1, Z_2 s.t. if current consumption is too high
      // for the Euler equation to hold then z1 decreases.  This will pull down 
      // on consumption in the contemporaneous block later.  Similarly, for af1:
      // if the expected return on asset 2 is too high then the investment share
      // in asset 1 declines.  Finally, if q(+1) is too high
  return out ;
      // Return predictors for (B11, B22, r1, r2)
  
}
Example #11
0
void Controller::run()
{
    mutexRun.lock();
    string event="none";

    // verify if any saccade is still underway
    mutexCtrl.lock();
    if (commData->get_isSaccadeUnderway() && (Time::now()-saccadeStartTime>=Ts))
    {
        if (!tiltDone)
            posHead->checkMotionDone(eyesJoints[0],&tiltDone);

        if (!panDone)
            posHead->checkMotionDone(eyesJoints[1],&panDone);

        if (!verDone)
            posHead->checkMotionDone(eyesJoints[2],&verDone);

        commData->get_isSaccadeUnderway()=!(tiltDone&&panDone&&verDone);
        if (!commData->get_isSaccadeUnderway())
            notifyEvent("saccade-done");
    }
    mutexCtrl.unlock();
    
    // get data
    double x_stamp;
    Vector xd=commData->get_xd();
    Vector x=commData->get_x(x_stamp);
    Vector new_qd=commData->get_qd();

    // read feedbacks
    q_stamp=Time::now();
    if (Robotable)
    {
        if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
        {
            printf("\nCommunication timeout detected!\n\n");
            notifyEvent("comm-timeout");
            suspend();

            mutexRun.unlock();
            return;
        }

        IntState->reset(fbHead);
    }

    fbNeck=fbHead.subVector(0,2);
    fbEyes=fbHead.subVector(3,5);

    double errNeck=norm(new_qd.subVector(0,2)-fbNeck);
    double errEyes=norm(new_qd.subVector(3,new_qd.length()-1)-fbEyes);
    bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLETIME) ? false :
                   (!commData->get_isSaccadeUnderway() &&
                   (errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
                   (errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));

    // verify control switching conditions
    if (commData->get_isCtrlActive())
    {
        // switch-off condition
        if (swOffCond)
        {
            event="motion-done";

            mutexCtrl.lock();
            stopLimb(false);
            mutexCtrl.unlock();
        }
        // manage new target while controller is active
        else if (port_xd->get_new())
        {
            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();
        }

        port_xd->get_new()=false;
    }
    else
    {
        // inhibition is cleared upon new target arrival
        if (ctrlInhibited)
            ctrlInhibited=!port_xd->get_new();

        // switch-on condition
        commData->get_isCtrlActive()=port_xd->get_new() ||
                                     (!ctrlInhibited &&
                                     (new_qd[0]!=qd[0]) || (new_qd[1]!=qd[1]) || (new_qd[2]!=qd[2]) ||
                                     (!commData->get_canCtrlBeDisabled() && (norm(port_xd->get_xd()-x)>GAZECTRL_MOTIONSTART_XTHRES)));

        // reset controllers
        if (commData->get_isCtrlActive())
        {
            ctrlActiveRisingEdgeTime=Time::now();
            port_xd->get_new()=false;

            Vector zeros(3,0.0);
            mjCtrlNeck->reset(zeros);
            mjCtrlEyes->reset(zeros);
            IntPlan->reset(fbNeck);

            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();
        }
    }

    if (event=="motion-onset")
        q0deg=CTRL_RAD2DEG*fbHead;

    qd=new_qd;
    qdNeck=qd.subVector(0,2);
    qdEyes=qd.subVector(3,5);
    
    if (commData->get_isCtrlActive())
    {
        // control loop
        vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
        IntPlan->integrate(vNeck);

        if (unplugCtrlEyes)
        {
            if (Time::now()-saccadeStartTime>=Ts)
                vEyes=commData->get_counterv();
        }
        else
            vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();
    }
    else
    {
        vNeck=0.0;
        vEyes=0.0;
    }

    v.setSubvector(0,vNeck);
    v.setSubvector(3,vEyes);

    // apply bang-bang just in case to compensate
    // for unachievable low velocities
    if (Robotable)
    {
        for (size_t i=0; i<v.length(); i++)
            if ((v[i]!=0.0) && (v[i]>-minAbsVel) && (v[i]<minAbsVel))
                v[i]=iCub::ctrl::sign(qd[i]-fbHead[i])*minAbsVel;
    }

    // convert to degrees
    mutexData.lock();
    qddeg=CTRL_RAD2DEG*qd;
    qdeg =CTRL_RAD2DEG*fbHead;
    vdeg =CTRL_RAD2DEG*v;
    mutexData.unlock();

    // send commands to the robot
    if (Robotable && commData->get_isCtrlActive())
    {
        mutexCtrl.lock();

        if (neckPosCtrlOn)
        {
            Vector posdeg=(CTRL_RAD2DEG)*IntPlan->get();
            posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
            velEyes->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
        }
        else
            velHead->velocityMove(vdeg.data());

        mutexCtrl.unlock();
    }

    // print info
    printIter(xd,x,qddeg,qdeg,vdeg,1.0);

    // send x,q through YARP ports
    Vector q(nJointsTorso+nJointsHead);
    int j;
    for (j=0; j<nJointsTorso; j++)
        q[j]=CTRL_RAD2DEG*fbTorso[j];
    for (; (size_t)j<q.length(); j++)
        q[j]=qdeg[j-nJointsTorso];

    txInfo_x.update(x_stamp);
    if (port_x.getOutputCount()>0)
    {        
        port_x.setEnvelope(txInfo_x);
        port_x.write(x);
    }

    txInfo_q.update(q_stamp);
    if (port_q.getOutputCount()>0)
    {        
        port_q.setEnvelope(txInfo_q);
        port_q.write(q);
    }

    // update pose information
    mutexChain.lock();

    for (int i=0; i<nJointsTorso; i++)
    {
        chainNeck->setAng(i,fbTorso[i]);
        chainEyeL->setAng(i,fbTorso[i]);
        chainEyeR->setAng(i,fbTorso[i]);
    }
    for (int i=0; i<3; i++)
    {
        chainNeck->setAng(nJointsTorso+i,fbHead[i]);
        chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
        chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
    }
    chainEyeL->setAng(nJointsTorso+3,fbHead[3]);               chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
    chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);

    txInfo_pose.update(q_stamp);

    mutexChain.unlock();

    if (event=="motion-onset")
        notifyEvent(event);

    motionOngoingEventsHandling();

    if (event=="motion-done")
    {
        motionOngoingEventsFlush();
        notifyEvent(event);
    }

    // update joints angles
    fbHead=IntState->integrate(v);
    commData->set_q(fbHead);
    commData->set_torso(fbTorso);
    commData->set_v(v);

    mutexRun.unlock();
}
Example #12
0
void matrix::block_mult(const matrix & B, matrix & C, int BlockX, int BlockY, int BlockZ) const {
  int i, j, k, ii, jj, kk, Ax, Ay, Bx, By, Cx, Cy; i = 0; j = 0; k = 0; ii = 0; jj = 0; kk = 0;
  Ax = 0; Ay = 0; Bx = 0; By = 0; Cx = 0; Cy = 0;
  int Nx, Ny, Nz; Nx = 0; Ny = 0; Nz = 0;
  fpp temp; temp = 0.0;

  submatrix SubA, SubB, SubC;
  matrix CacheA, CacheB, CacheC;
  //std::cout << "Matrix mult called!" <<std::endl;

  Ax = this->get_rows(); Ay = this->get_cols();
  Bx = B.get_rows(); By = B.get_cols();
  Cx = C.get_rows(); Cy = C.get_cols();

  if ((Ay != Bx) || (By != Cy) || (Ax != Cx)){
    std::cerr << "Matrix multiplication failed! Dimension incompatibility. A(" << Ax << "," << Ay << "), B(" << Bx << "," << By << "), C(" << Cx << "," << Cy << ")." << std::endl;
    exit(-1);
  }

  Nx = Cx/BlockX; Ny = Cy/BlockY; Nz = Ay/BlockZ;

  int x_fudge, y_fudge, z_fudge; x_fudge = 1; y_fudge = 1; z_fudge = 1;

  if (Cx % BlockX == 0){
    x_fudge = 0;
  }

  if (Cy % BlockY == 0){
    y_fudge = 0;
  }

  if (Ay % BlockZ == 0){
    z_fudge = 0;
  }

  for (i = 0; i < Nx + x_fudge; i++){
    for (j = 0; j < Ny + y_fudge; j++){

      // Set Up  Submatrix C
      if((i+1)*BlockX < Cx){
	if((j+1)*BlockY < Cy){
	  SubC.subcreate(C, i*BlockX, j*BlockY, BlockX, BlockY);
	} else {
	  SubC.subcreate(C, i*BlockX, j*BlockY, BlockX, Cy - j*BlockY);
	}
      } else {
	if((j+1)*BlockY < Cy){
	  SubC.subcreate(C, i*BlockX, j*BlockY, Cx - i*BlockX, BlockY);
	} else {
	  SubC.subcreate(C, i*BlockX, j*BlockY, Cx - i*BlockX, Cy - j*BlockY);
	}
      }

      CacheC = SubC;
      zeros(CacheC);

      for(k = 0; k < Nz + z_fudge; k++){
	
	if((i+1)*BlockX < Ax){
	  if((k+1)*BlockZ < Ay){
	    SubA.subcreate(*this, i*BlockX, k*BlockZ, BlockX, BlockZ);
	  } else {
	    SubA.subcreate(*this, i*BlockX, k*BlockZ, BlockX, Ay - k*BlockZ);
	  }
	} else {
	  if((k+1)*BlockZ < Ay){
	    SubA.subcreate(*this, i*BlockX, k*BlockZ, Ax - i*BlockX, BlockZ);
	  } else {
	    SubA.subcreate(*this, i*BlockX, k*BlockZ, Ax - i*BlockX, Ay - k*BlockZ);
	  }
	}

	CacheA = SubA;

	if((j+1)*BlockY < By){
	  if((k+1)*BlockZ < Bx){
	    SubB.subcreate(B, k*BlockZ, j*BlockY, BlockZ, BlockY);
	  } else {
	    SubB.subcreate(B, k*BlockZ, j*BlockY, Bx - k*BlockZ, BlockY);
	  }
	} else {
	  if((k+1)*BlockZ < Bx){
	    SubB.subcreate(B, k*BlockZ, j*BlockY, BlockZ, By - j*BlockY);
	  } else {
	    SubB.subcreate(B, k*BlockZ, j*BlockY, Bx - k*BlockZ, By - j*BlockY);
	  }
	}

	CacheB = SubB;

	for(ii = 0; ii < CacheA.get_rows(); ii++){
	  for(jj = 0; jj < CacheB.get_cols(); jj++){
	    for(kk = 0; kk < CacheA.get_cols(); kk++){
	      CacheC(ii,jj) += CacheA(ii,kk)*CacheB(kk,jj);
	    }
	  }
	}

      }

      SubC = CacheC;

    }
  }

}
Example #13
0
SegImage* MeanFiller::fillDynamic(int startX, int startY, int startZ, int startRadius)
{
	int cols, rows, slices;
	int minx, miny, minz, maxx, maxy, maxz;
	cube sample, xes;
	vec values;
	stack<triple> historyEntity;
	image->getSize(cols, rows, slices);
	cube result = zeros(rows, cols, slices);
	cube sphere = Utils::sphere(startRadius);
	result(startY, startX, startZ, arma::size(sphere)) = sphere;
	res_image = Utils::convert(result);
	Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
	double thres;
	history.clear();
	result.reset();

	if (minx < 3) minx = 3;
	if (maxx > cols - 2) maxx = cols - 2;
	if (miny < 3) miny = 3;
	if (maxy > rows - 2) maxy = rows - 2;
	if (minz < 3) minz = 3;
	if (maxz > slices - 2) maxz = slices - 2;
	bool flag = false;

	while (!flag)
	{
		flag = true;

		xes = Utils::convert_d(image->getRegion(minx, miny, maxx, maxy, minz, maxz));
		sample = wBright * Utils::convert_d(res_image->getRegion(minx, miny, maxx, maxy, minz, maxz)) + 
			wCurv * xes;
		sample = Utils::conv3(Utils::conv3(sample, kernel), kernel);
		sample %= -xes + 1;
		values = sort(nonzeros(vectorise(sample)));
		thres = values(quantile * values.size());

		for (int i = minx; i < maxx; i++)
		{
			for (int j = miny; j < maxy; j++)
			{
				for (int k = minz; k < maxz; k++)
				{
					if (image->getVoxel(i, j, k) > 0 && res_image->getVoxel(i, j, k) == 0)
					{
						if (sample(j-miny, i - minx, k - minz) > thres)
						{
							res_image->setVoxel(i, j, k, 255);
							if (i <= minx && i >= 4)
								minx = i - 1;
							if (i >= maxx && i <= cols - 4 && maxx < i + 1)
								maxx = i + 1;
							if (j <= miny && j >= 4)
								miny = j - 1;
							if (j >= maxy && j <= rows - 4 && maxy < j + 1)
								maxy = j + 1;
							if (k <= minz && k >= 4)
								minz = k - 1;
							if (k >= maxz && k <= slices - 4 && maxz < k + 1)
								maxz = k + 1;
							historyEntity.push(triple(i, j, k));
							flag = false;
						}
					}
				}
			}
		}
		if (!flag) {
			if (history.size() >= h_size)
			{
				stack<triple> first = history.back();
				while (!first.empty())
				{
					first.pop();
				}
				history.pop_back();
			}
			history.push_front(historyEntity);
		}
	}
	return res_image;
}
Example #14
0
SegImage* MeanFiller::fill(int startX, int startY, int startZ, int startRadius)
{
	timer.start("Fill");
	int cols, rows, slices;
	stack<triple> historyEntity;
	cube sample;
	image->getSize(cols, rows, slices);
	cube result = zeros(rows, cols, slices);
	cube sphere = Utils::sphere(startRadius);
	result(startY, startX, startZ, arma::size(sphere)) = sphere;
	timer.stage("Init");
	res_image = Utils::convert(result);
	timer.stage("Convert");
	Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
	timer.stage("Bounds");
	result.reset();
	history.clear();

	if (minx < 3) minx = 3;
	if (maxx > cols - 2) maxx = cols - 2;
	if (miny < 3) miny = 3;
	if (maxy > rows - 2) maxy = rows - 2;
	if (minz < 3) minz = 3;
	if (maxz > slices - 2) maxz = slices - 2;
	bool flag = false;

	while(!flag)
	{
		historyEntity = stack<triple>();
		flag = true;
		for (int i = minx; i <= maxx; i++)
		{
			for (int j = miny; j <= maxy; j++)
			{
				for (int k = minz; k <= maxz; k++)
				{
					if (image->getVoxel(i,j,k) > 0 && res_image->getVoxel(i,j,k) == 0)
					{
						sample = wBright * Utils::convert_d(image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2)) +
							wCurv * Utils::convert_d(res_image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2));
						double der = derivate(sample);
						if (der > threshold)
						{
							res_image->setVoxel(i, j, k, 255);
							if (i <= minx && i >= 4 && minx > i - 1)
								minx = i - 1;
							if (i >= maxx && i <= cols - 4 && maxx < i + 1)
									maxx = i + 1;
							if (j <= miny && j >= 4 && miny > j - 1)
									miny = j - 1;
							if (j >= maxy && j <= rows - 4 && maxy < j + 1)
									maxy = j + 1;
							if (k <= minz && k >= 4 && minz > k - 1)
									minz = k - 1;
							if (k >= maxz && k <= slices - 4 && minz < k + 1)
									maxz = k + 1;
							historyEntity.push(triple(i, j, k));
							flag = false;
						}
					}
				}
			}
		}
		timer.stage("Full cycle");
		if (!flag) {
			if (history.size() >= h_size)
			{
				stack<triple> first = history.back();
				while (!first.empty())
				{
					first.pop();
				}
				history.pop_back();
			}
			history.push_front(historyEntity);
		}
	}
	timer.stop();
	return res_image;
}
Example #15
0
 void before_iteration(int iteration, graphchi_context & gcontext){
   last_validation_rmse = dvalidation_rmse;
   validation_rmse_vec = zeros(num_threads);
 }
Example #16
0
int main(int argc, const char ** argv) {

  mytimer.start();
  print_copyright();

  /* GraphChi initialization will read the command line 
     arguments and the configuration file. */
  graphchi_init(argc, argv);

  /* Metrics object for keeping track of performance counters
     and other information. Currently required. */
  metrics m("rating2");

  knn_sample_percent = get_option_float("knn_sample_percent", 1.0);
  if (knn_sample_percent <= 0 || knn_sample_percent > 1)
    logstream(LOG_FATAL)<<"Sample percente should be in the range (0, 1] " << std::endl;

  num_ratings   = get_option_int("num_ratings", 10);
  if (num_ratings <= 0)
    logstream(LOG_FATAL)<<"num_ratings, the number of recomended items for each user, should be >=1 " << std::endl;

  debug         = get_option_int("debug", 0);
  tokens_per_row = get_option_int("tokens_per_row", tokens_per_row);
  std::string algorithm     = get_option_string("algorithm");
  /* Basic arguments for RBM algorithm */
  rbm_bins      = get_option_int("rbm_bins", rbm_bins);
  rbm_scaling   = get_option_float("rbm_scaling", rbm_scaling);

  if (algorithm == "svdpp" || algorithm == "svd++")
    algo = SVDPP;
  else if (algorithm == "biassgd")
    algo = BIASSGD;
  else if (algorithm == "rbm")
    algo = RBM;
  else logstream(LOG_FATAL)<<"--algorithm should be svd++ or biassgd or rbm"<<std::endl;

  parse_command_line_args();

  /* Preprocess data if needed, or discover preprocess files */
  int nshards = 0;
  if (tokens_per_row == 3)
    nshards = convert_matrixmarket<edge_data>(training, 0, 0, 3, TRAINING, false);
  else if (tokens_per_row == 4)
    nshards = convert_matrixmarket4<edge_data4>(training);
  else logstream(LOG_FATAL)<<"--tokens_per_row should be either 3 or 4" << std::endl;

  assert(M > 0 && N > 0);
  latent_factors_inmem.resize(M+N); // Initialize in-memory vertices.

  //initialize data structure to hold the matrix read from file
  if (algo == RBM){
#pragma omp parallel for
    for (uint i=0; i< M+N; i++){
      if (i < M){
        latent_factors_inmem[i].pvec = zeros(D*3);
      }
      else {  
        latent_factors_inmem[i].pvec = zeros(rbm_bins + rbm_bins * D);
      }
    } 
  }
 
  read_factors(training);
  if ((uint)num_ratings > N){
    logstream(LOG_WARNING)<<"num_ratings is too big - setting it to: " << N << std::endl;
    num_ratings = N;
  }
  srand(time(NULL));

  /* Run */
  if (tokens_per_row == 3){
    RatingVerticesInMemProgram<VertexDataType, EdgeDataType> program;
    graphchi_engine<VertexDataType, EdgeDataType> engine(training, nshards, false, m); 
    set_engine_flags(engine);
    engine.run(program, 1);
  } 
  else if (tokens_per_row == 4){
    RatingVerticesInMemProgram<VertexDataType, edge_data4> program;
    graphchi_engine<VertexDataType, edge_data4> engine(training, nshards, false, m); 
    set_engine_flags(engine);
    engine.run(program, 1);
  }
  /* Output latent factor matrices in matrix-market format */
  output_knn_result(training);

  rating_stats();

  if (users_without_ratings > 0)
    logstream(LOG_WARNING)<<"Found " << users_without_ratings << " without ratings. For those users no items are recommended (item id 0)" << std::endl;

  if (users_no_ratings > 0)
    logstream(LOG_WARNING)<<"Failed to compute ratings for " << users_no_ratings << " Users. For those users no items are recommended (item id 0)" << std::endl;


  /* Report execution metrics */
  if (!quiet)
    metrics_report(m);
  return 0;
}
Example #17
0
mat Poisson1D::setPhip(mat phin, bool Equilibrum) {
	if (Equilibrum == true)
		return ( zeros(dev1D.getSumPoint()) ); // see Device1D::chargeDensityArrayFunct

	return ( dev1D.getBGArray() - phin - (fLnArray - fLpArray) );
}
Example #18
0
function result=multiple(x,y)

global countmul countzero;

accuracy=2^-15;
result=zeros(size(x,1),size(y,2));
 for a=1:size(x,1)
      for c=1:size(y,2)
            for b=1:size(x,2)
                     tempvalue=x(a,b)*y(b,c);  
                     countmul=countmul+1;
               if tempvalue<accuracy  
                      countzero=countzero+1;
                      %tempvalue=0;
               end
                  result(a,c)=result(a,c)+tempvalue;
            end
      end
 end
end
Example #19
0
void write(const star2d &A,char *var,char *fmt) {

	int i;
	double d;
	matrix m,m2,T,T_odd;

	if(pole||equator) {
		m2=zeros(1,A.nth+pole+equator);
		m2.setblock(0,0,equator,A.nth+equator-1,A.th);
		if(equator) m2(0)=PI/2;
		m=A.map.leg.eval_00(A.th,m2,T);
		m=A.map.leg.eval_11(A.th,m2,T_odd);
	} else {
		T=eye(A.nth);
		T_odd=T;
	}
	
	if(!strcmp(var,"nr")) {
		if(fmt) fprintf(stdout,fmt,A.nr);
		else {
			i=A.nr;
			fwrite(&i,sizeof(int),1,stdout);
		}
	} else if(!strcmp(var,"ndomains")) {
		if(fmt) fprintf(stdout,fmt,A.ndomains);
		else {
			i=A.ndomains;
			fwrite(&i,sizeof(int),1,stdout);
		}
	} else if(!strcmp(var,"npts")) {
		if(fmt) {
			for(i=0;i<A.ndomains;i++) {	
				fprintf(stdout,fmt,*(A.map.gl.npts+i));
				if(i<A.ndomains-1) fprintf(stdout,",");
			}
		} else {
			fwrite(A.map.gl.npts,sizeof(int),A.ndomains,stdout);
		}
	} else if(!strcmp(var,"xif")) {
		if(fmt) {
			for(i=0;i<A.ndomains+1;i++) {	
				fprintf(stdout,fmt,*(A.map.gl.xif+i));
				if(i<A.ndomains) fprintf(stdout,",");
			}
		} else {
			fwrite(A.map.gl.xif,sizeof(double),A.ndomains+1,stdout);
		}
	} else if(!strcmp(var,"nth")) {
		if(fmt) fprintf(stdout,fmt,A.nth);
		else {
			i=A.nth;
			fwrite(&i,sizeof(int),1,stdout);
		}
	} else if(!strcmp(var,"nex")) {
		if(fmt) fprintf(stdout,fmt,A.nex);
		else {
			i=A.nex;
			fwrite(&i,sizeof(int),1,stdout);
		}
	} else if(!strcmp(var,"ps")) {
		m=A.ps;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"m")) {
		if(fmt) fprintf(stdout,fmt,A.m);
		else {
			fwrite(&A.m,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"surff")) {
		if(fmt) fprintf(stdout,fmt,A.surff);
		else {
			fwrite(&A.surff,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"conv")) {
		if(fmt) fprintf(stdout,fmt,A.conv);
		else {
			fwrite(&A.conv,sizeof(int),1,stdout);
		}
	} else if(!strcmp(var,"Omega")) {
		if(dim) d=A.Omega*A.units.Omega;
		else d=A.Omega;
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Omega_bk")) {
		if(fmt) fprintf(stdout,fmt,A.Omega_bk);
		else {
			fwrite(&A.Omega_bk,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Omegac")) {
		if(dim) d=A.Omegac*A.units.Omega;
		else d=A.Omegac;
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Xc")) {
		if(fmt) fprintf(stdout,fmt,A.Xc);
		else {
			fwrite(&A.Xc,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"rhoc")) {
		if(fmt) fprintf(stdout,fmt,A.rhoc);
		else {
			fwrite(&A.rhoc,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Tc")) {
		if(fmt) fprintf(stdout,fmt,A.Tc);
		else {
			fwrite(&A.Tc,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"pc")) {
		if(fmt) fprintf(stdout,fmt,A.pc);
		else {
			fwrite(&A.pc,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"R")) {
		if(fmt) fprintf(stdout,fmt,A.R);
		else {
			fwrite(&A.R,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Rp")) {
		if(fmt) fprintf(stdout,fmt,A.R);
		else {
			fwrite(&A.R,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Re")) {
		d=A.map.leg.eval_00(A.r.row(A.nr-1),PI/2)(0)*A.units.r;
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"M")) {
		if(fmt) fprintf(stdout,fmt,A.M);
		else {
			fwrite(&A.M,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Lz")) {
		d=A.Lz();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Mcore")) {
		d=A.Mcore();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Lzcore")) {
		d=A.Lzcore();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"X")) {
		if(fmt) fprintf(stdout,fmt,A.X0);
		else {
			fwrite(&A.X0,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Y")) {
		if(fmt) fprintf(stdout,fmt,A.Y0);
		else {
			fwrite(&A.Y0,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Z")) {
		if(fmt) fprintf(stdout,fmt,A.Z0);
		else {
			fwrite(&A.Z0,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"R/R_SUN")) {
		if(fmt) fprintf(stdout,fmt,A.R/R_SUN);
		else {
			d=A.R/R_SUN;
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Rp/R_SUN")) {
		if(fmt) fprintf(stdout,fmt,A.R/R_SUN);
		else {
			d=A.R/R_SUN;
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Re/R_SUN")) {
		d=A.map.leg.eval_00(A.r.row(A.nr-1),PI/2)(0)*A.units.r/R_SUN;
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"M/M_SUN")) {
		if(fmt) fprintf(stdout,fmt,A.M/M_SUN);
		else {
			d=A.M/M_SUN;
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"opa")) {
		if(fmt) fprintf(stdout,fmt,A.opa.name);
		else {
			fwrite(A.opa.name,sizeof(char),strlen(A.opa.name)+1,stdout);
		}
	} else if(!strcmp(var,"eos")) {
		if(fmt) fprintf(stdout,fmt,A.eos.name);
		else {
			fwrite(A.eos.name,sizeof(char),strlen(A.eos.name)+1,stdout);
		}
	} else if(!strcmp(var,"r")) {
		if(dim) m=A.r*A.units.r;
		else m=A.r;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"z")) {
		if(dim) m=A.z*A.units.r;
		else m=A.z;
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"D")) {
		if(dim) m=A.D/A.units.r;
		else m=A.D;
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"I")) {
		if(dim) m=A.map.gl.I*A.units.r;
		else m=A.map.gl.I;
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"rex")) {
		if(dim) m=A.map.ex.r*A.units.r;
		else m=A.map.ex.r;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"phiex")) {
		if(dim) m=A.phiex*A.units.phi;
		else m=A.phiex;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"Dex")) {
		if(dim) m=A.map.ex.D/A.units.r;
		else m=A.map.ex.D;
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"th")) {
		m=zeros(1,A.nth+pole+equator);
		m.setblock(0,0,equator,A.nth-1+pole,A.th);
		if(equator) m(0)=PI/2;
		if(pole) m(m.ncols()-1)=0;
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"Dt")) {
		m2=(A.Dt,T_odd);
		m=zeros(A.nth+pole+equator,A.nth+pole+equator);
		m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"Dtodd")) {
		m2=(A.map.leg.D_11,T);
		m=zeros(A.nth+pole+equator,A.nth+pole+equator);
		m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"Dt2")) {
		m2=(A.Dt2,T);
		m=zeros(A.nth+pole+equator,A.nth+pole+equator);
		m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"It")) {
		m=zeros(A.nth+pole+equator,1);
		m.setblock(equator,A.nth+equator-1,0,0,A.map.leg.I_00);
		if(fmt) matrix_fmt(fmt,m);
		else m.write(stdout,'b');
	} else if(!strcmp(var,"Ts")) {
		m=A.Ts;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"map.R")) {
		if(dim) m=A.map.R.block(1,-1,0,-1)*A.units.r;
		else m=A.map.R.block(1,-1,0,-1);
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"rho")) {
		if(dim) m=A.rho*A.units.rho;
		else m=A.rho;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"phi")) {
		if(dim) m=A.phi*A.units.phi;
		else m=A.phi;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"p")) {
		if(dim) m=A.p*A.units.p;
		else m=A.p;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"Xr")) {
		m=A.comp.X();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"Yr")) {
		m=A.comp.Y();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"Zr")) {
		m=A.comp.Z();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strncmp(var,"X_",2)) {
		std::string elem(var+2);
		if(A.comp.count(elem)) m=A.comp[elem];
		else m=zeros(A.nr,A.nth);
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"w")) {
		if(dim) m=A.w*A.units.Omega;
		else m=A.w;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"T")) {
		if(dim) m=A.T*A.units.T;
		else m=A.T;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"vr")) {
		if(dim) m=A.vr*A.units.v;
		else m=A.vr;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"vt")) {
		if(dim) m=A.vt*A.units.v;
		else m=A.vt;
		if(fmt) matrix_fmt(fmt,(m,T_odd));
		else (m,T_odd).write(stdout,'b');
	} else if(!strcmp(var,"G")) {
		if(dim) m=A.G*A.units.v*A.units.r*A.units.rho;
		else m=A.G;
		if(fmt) matrix_fmt(fmt,(m,T_odd));
		else (m,T_odd).write(stdout,'b');
	} else if(!strcmp(var,"N2")) {
		m=A.N2();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"opa.k")) {
		m=A.opa.k;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"opa.xi")) {
		m=A.opa.xi;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"opa.dlnxi_lnT")) {
		m=A.opa.dlnxi_lnT;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"opa.dlnxi_lnrho")) {
		m=A.opa.dlnxi_lnrho;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"nuc.eps")) {
		m=A.nuc.eps;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"nuc.pp")) {
		m=A.nuc.pp;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"nuc.cno")) {
		m=A.nuc.cno;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.G1")) {
		m=A.eos.G1;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.cp")) {
		m=A.eos.cp;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.del_ad")) {
		m=A.eos.del_ad;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.G3_1")) {
		m=A.eos.G3_1;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.cv")) {
		m=A.eos.cv;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.d")) {
		m=A.eos.d;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.prad")) {
		m=A.eos.prad;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.chi_T")) {
		m=A.eos.chi_T;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.chi_rho")) {
		m=A.eos.chi_rho;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"eos.s")) {
		m=A.eos.s;
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"L")) {
		d=A.luminosity();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"L/L_SUN")) {
		d=A.luminosity()/L_SUN;
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Teff")) {
		m=A.Teff();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"Teff_eq")) {
		m=A.Teff();
		d=A.map.leg.eval_00(m,PI/2)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"Teff_pol")) {
		m=A.Teff();
		d=A.map.leg.eval_00(m,0)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"gsup")) {
		m=A.gsup();
		if(fmt) matrix_fmt(fmt,(m,T));
		else (m,T).write(stdout,'b');
	} else if(!strcmp(var,"gsup_eq")) {
		m=A.gsup();
		d=A.map.leg.eval_00(m,PI/2)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"gsup_pol")) {
		m=A.gsup();
		d=A.map.leg.eval_00(m,0)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"eps")) {
		d=1-1./A.map.leg.eval_00(A.r,PI/2)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"eps_c")) {
		d=0;
		if(A.conv) d=1-A.map.eta(A.conv)/A.map.leg.eval_00(A.map.R.row(A.conv),PI/2)(0);
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"virial")) {
		d=A.virial();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else if(!strcmp(var,"energy_test")) {
		d=A.energy_test();
		if(fmt) fprintf(stdout,fmt,d);
		else {
			fwrite(&d,sizeof(double),1,stdout);
		}
	} else {
		ester_err("Unknown variable %s", var);
		exit(1);
	}
}
void umat_plasticity_kin_iso_CCP(const vec &Etot, const vec &DEtot, vec &sigma, mat &Lt, mat &L, vec &sigma_in, const mat &DR, const int &nprops, const vec &props, const int &nstatev, vec &statev, const double &T, const double &DT, const double &Time, const double &DTime, double &Wm, double &Wm_r, double &Wm_ir, double &Wm_d, const int &ndi, const int &nshr, const bool &start, const int &solver_type, double &tnew_dt)                               
{
    
    UNUSED(nprops);
    UNUSED(nstatev);
    UNUSED(Time);
    UNUSED(DTime);
    UNUSED(nshr);
    UNUSED(tnew_dt);
    
    //From the props to the material properties
    double E = props(0);
    double nu= props(1);
    double alpha_iso = props(2);
    double sigmaY = props(3);
    double k=props(4);
    double m=props(5);
    double kX = props(6);
    
    //definition of the CTE tensor
    vec alpha = alpha_iso*Ith();
       
    ///@brief Temperature initialization
    double T_init = statev(0);
    //From the statev to the internal variables
    double p = statev(1);
    vec EP(6);
    EP(0) = statev(2);
    EP(1) = statev(3);
    EP(2) = statev(4);
    EP(3) = statev(5);
    EP(4) = statev(6);
    EP(5) = statev(7);
    
    ///@brief a is the internal variable associated with kinematical hardening
    vec a = zeros(6);
    a(0) = statev(8);
    a(1) = statev(9);
    a(2) = statev(10);
    a(3) = statev(11);
    a(4) = statev(12);
    a(5) = statev(13);
    
    //Rotation of internal variables (tensors)
    EP = rotate_strain(EP, DR);
    a = rotate_strain(a, DR);
    
    ///@brief Initialization
    if(start)
    {
        //Elstic stiffness tensor
        L = L_iso(E, nu, "Enu");
        T_init = T;
        vec vide = zeros(6);
        sigma = vide;
        EP = vide;
        a = vide;
        p = 0.;
        
        Wm = 0.;
        Wm_r = 0.;
        Wm_ir = 0.;
        Wm_d = 0.;
    }
    
    //Additional parameters and variables
    vec X = kX*(a%Ir05());
    
    double Hp=0.;
    double dHpdp=0.;
    
    if (p > iota)	{
        dHpdp = m*k*pow(p, m-1);
        Hp = k*pow(p, m);
    }
    else {
        dHpdp = 0.;
        Hp = 0.;
    }
    
    //Variables values at the start of the increment
    vec sigma_start = sigma;
    vec EP_start = EP;
    vec a_start = a;
    vec X_start = X;
    
    double A_p_start = -Hp;
    vec A_a_start = -X_start;
    
    //Variables required for the loop
    vec s_j = zeros(1);
    s_j(0) = p;
    vec Ds_j = zeros(1);
    vec ds_j = zeros(1);
    
    ///Elastic prediction - Accounting for the thermal prediction
    vec Eel = Etot + DEtot - alpha*(T+DT-T_init) - EP;
    sigma = el_pred(L, Eel, ndi);
    
    //Define the plastic function and the stress
    vec Phi = zeros(1);
    mat B = zeros(1,1);
    vec Y_crit = zeros(1);
    
    double dPhidp=0.;
    vec dPhida = zeros(6);
    vec dPhidsigma = zeros(6);
    double dPhidtheta = 0.;
    
    //Compute the explicit flow direction
    vec Lambdap = eta_stress(sigma-X);
    vec Lambdaa = eta_stress(sigma-X);
    std::vector<vec> kappa_j(1);
    kappa_j[0] = L*Lambdap;
    mat K = zeros(1,1);
    
    //Loop parameters
    int compteur = 0;
    double error = 1.;
    
    //Loop
    for (compteur = 0; ((compteur < maxiter_umat) && (error > precision_umat)); compteur++) {
        
        p = s_j(0);
        if (p > iota)	{
            dHpdp = m*k*pow(p, m-1);
            Hp = k*pow(p, m);
        }
        else {
            dHpdp = 0.;
            Hp = 0.;
        }
        dPhidsigma = eta_stress(sigma-X);
        dPhidp = -1.*dHpdp;
        dPhida = -1.*kX*(eta_stress(sigma - X)%Ir05());
        
        //compute Phi and the derivatives
        Phi(0) = Mises_stress(sigma-X) - Hp - sigmaY;
        
        Lambdap = eta_stress(sigma-X);
        Lambdaa = eta_stress(sigma-X);
        kappa_j[0] = L*Lambdap;
        
        K(0,0) = dPhidp + sum(dPhida%Lambdaa);
        B(0, 0) = -1.*sum(dPhidsigma%kappa_j[0]) + K(0,0);
        Y_crit(0) = sigmaY;
        
        Fischer_Burmeister_m(Phi, Y_crit, B, Ds_j, ds_j, error);
        
        s_j(0) += ds_j(0);
        EP = EP + ds_j(0)*Lambdap;
        a = a + ds_j(0)*Lambdaa;
        X = kX*(a%Ir05());
        
        //the stress is now computed using the relationship sigma = L(E-Ep)
        Eel = Etot + DEtot - alpha*(T + DT - T_init) - EP;
        sigma = el_pred(L, Eel, ndi);
    }
    
    //Computation of the increments of variables
    vec Dsigma = sigma - sigma_start;
    vec DEP = EP - EP_start;
    double Dp = Ds_j[0];
    vec Da = a - a_start;
    
    if (solver_type == 0) {
    
		//Computation of the tangent modulus
		mat Bhat = zeros(1, 1);
		Bhat(0, 0) = sum(dPhidsigma%kappa_j[0]) - K(0,0);
    
		vec op = zeros(1);
		mat delta = eye(1,1);
    
		for (int i=0; i<1; i++) {
			if(Ds_j[i] > iota)
				op(i) = 1.;
		}
    
		mat Bbar = zeros(1,1);
		for (int i = 0; i < 1; i++) {
			for (int j = 0; j < 1; j++) {
				Bbar(i, j) = op(i)*op(j)*Bhat(i, j) + delta(i,j)*(1-op(i)*op(j));
			}
		}
    
		mat invBbar = zeros(1, 1);
		mat invBhat = zeros(1, 1);
		invBbar = inv(Bbar);
		for (int i = 0; i < 1; i++) {
			for (int j = 0; j < 1; j++) {
				invBhat(i, j) = op(i)*op(j)*invBbar(i, j);
			}
		}
    
		std::vector<vec> P_epsilon(1);
		P_epsilon[0] = invBhat(0, 0)*(L*dPhidsigma);
		std::vector<double> P_theta(1);
		P_theta[0] = dPhidtheta - sum(dPhidsigma%(L*alpha));
		
		Lt = L - (kappa_j[0]*P_epsilon[0].t());
	}
    else if(solver_type == 1) {
        sigma_in = -L*EP;
    }
    
    double A_p = -Hp;
    vec A_a = -X;
    
    double Dgamma_loc = 0.5*sum((sigma_start+sigma)%DEP) + 0.5*(A_p_start + A_p)*Dp + 0.5*sum((A_a_start + A_a)%Da);
    
    //Computation of the mechanical and thermal work quantities
    Wm += 0.5*sum((sigma_start+sigma)%DEtot);
    Wm_r += 0.5*sum((sigma_start+sigma)%(DEtot-DEP)) - 0.5*sum((A_a_start + A_a)%Da);
    Wm_ir += -0.5*(A_p_start + A_p)*Dp;
    Wm_d += Dgamma_loc;
            
    ///@brief statev evolving variables
    //statev
    statev(0) = T_init;
    statev(1) = p;
    
    statev(2) = EP(0);
    statev(3) = EP(1);
    statev(4) = EP(2);
    statev(5) = EP(3);
    statev(6) = EP(4);
    statev(7) = EP(5);
    
    statev(8) = a(0);
    statev(9) = a(1);
    statev(10) = a(2);
    statev(11) = a(3);
    statev(12) = a(4);
    statev(13) = a(5);
}
Example #21
0
int tf_csim_casdec_x()
{
  CSIM::fir_x		fir_x1, fir_x2, fir_x3;
  CSIM::counter		cnt1, cnt2, cnt3;
  CSIM::wgn_x		wgn_x1;
  CSIM::casdec_x    casdec_x1;

 int N;

  cvec c0;
  cvec x_x;
  bvec ce, ce1, ce2, ce3;
  cvec ya_x1, ya_x2, ya_x3;
  cmat ya, yb, yc;
  
  //----------------------------------------
  // test of csim
  //----------------------------------------


  cout << "CSIM::casdec_x Test"  << endl;
  try {
	// mav<4>
	c0="(0.25,0.0) (0.25,0.0) (0.25, 0.0) (0.25,0.0)"; 
	cout << "c0=" << c0 << endl;
	// discrete cascade settings
	fir_x1.set_taps(c0); fir_x1.reset();
	fir_x2.set_taps(c0); fir_x2.reset();
	fir_x3.set_taps(c0); fir_x3.reset();		
	cnt1.set_N(2); cnt1.reset();
	cnt2.set_N(2); cnt2.reset();
	cnt3.set_N(2); cnt3.reset();
	cout << "discrete cascade settings "  << endl;
	// device cascade settings
	casdec_x1.set_size(3);
	casdec_x1.set_taps(c0);
	casdec_x1.reset();
	cout << "device cascade settings "  << endl;
	N=256;
	// clock and signal	
	ce.set_length(N); ce.ones(); // clock
	cout << "ce=" << ce << endl;

	// discrete cascade proc
	while (1) {
		x_x = wgn_x1.generate(ce); // source - random mean = (0.0+j0.0) sigma =1.0
		cout << "x_x=" << x_x << endl;

		ya_x1 = fir_x1.process(ce,x_x); ce1 = cnt1.generate(ce);
		ya_x2 = fir_x2.process(ce1,ya_x1); ce2 = cnt2.generate(ce1);
		ya_x3 = fir_x3.process(ce2,ya_x2); ce3 = cnt3.generate(ce2);
		ya.set_size(N,2);
		ya.set_col(0,ya_x3);
		ya.set_col(1,to_cvec(to_vec(ce3), zeros(ce3.length())));
		// cout << "discrete cascade proc "  << endl;
		// device cascade proc 
		yb = casdec_x1.process(ce, x_x);
		// cout << "device cascade proc "  << endl;
		yc.set_size(N,4);
		yc.set_col(0, ya.get_col(0));
		yc.set_col(1, yb.get_col(0));
		yc.set_col(2, ya.get_col(1));
		yc.set_col(3, yb.get_col(1));

		cout << "yc=" << endl << yc << endl;
	}
  }
   catch (sci_exception except) {
		cout<< "\n sci exception:" << endl <<except.get_msg() <<":" << except.get_info() << endl;
  }
  return 0;
 
}
Example #22
0
 /** \brief Create a dense matrix or a matrix with specified sparsity with all entries zero */
 static MatType zeros(casadi_int nrow=1, casadi_int ncol=1) {
   return zeros(Sparsity::dense(nrow, ncol));
 }
Example #23
0
	/**
	 * Called before an iteration is started.
	 */
	void before_iteration(int iteration, graphchi_context &gcontext) {
		liklihood_vec = zeros(gcontext.execthreads);
		err_vec = zeros(gcontext.execthreads);
	}
Example #24
0
 static MatType zeros(const std::pair<casadi_int, casadi_int>& rc) {
   return zeros(rc.first, rc.second);
 }
Example #25
0
	/**
	 *  Vertex update function
	 */
	void update(graphchi_vertex<VertexDataType, EdgeDataType> &vertex, graphchi_context &gcontext) {
		if (vertex.num_edges() > 0) {
			vertex_data & vdata = latent_factors_inmem[vertex.id()];
			vec R_cache = zeros(vertex.num_edges());

			// for each latent feature
			for (int x = 0; x < D; x++) {
				double numerator = 0.0;
				double denominator = 0.0;
				bool compute_rmse = (vertex.num_outedges() > 0 && x == 0);

				// for each user/item have connection with this node
				for (int j = 0; j < vertex.num_edges(); j++) {
					float observation = vertex.edge(j)->get_data();	// rating value of this connection
					vertex_data & nbr_latent = latent_factors_inmem[vertex.edge(j)->vertex_id()];// latent feature vector of this node
					double prediction;
					double rmse = 0;

					// if x = 0, calculate rmse and initialize R_cache
					if (x == 0) {
						rmse = bsvd_predict(vdata, nbr_latent, observation, prediction);
						R_cache[j] = observation - prediction;
					}

					//** B term
					double B = exp(alpha * (prediction - maxval));
					//** C term
					double C = exp(alpha * (minval - prediction));

					// compute numerator
					numerator += 2 * nbr_latent.pvec[x] * (R_cache[j] + vdata.pvec[x] * nbr_latent.pvec[x])
								- lambda * alpha * nbr_latent.pvec[x] * (B - C)
								+ lambda * alpha * alpha * nbr_latent.pvec[x] * nbr_latent.pvec[x] * vdata.pvec[x] * (B + C);

					//compute denominator
					denominator += nbr_latent.pvec[x] * nbr_latent.pvec[x] * (2 + lambda * alpha * alpha * (B + C));

					//record rmse
					if (compute_rmse)
						rmse_vec[omp_get_thread_num()] += rmse;
				}

				// update if denominator != 0
				if (denominator != 0.0) {
					double z = numerator / denominator;
					vec old = vdata.pvec;

					for (int j = 0; j < vertex.num_edges(); j++) {
						vertex_data & nbr_latent = latent_factors_inmem[vertex.edge(j)->vertex_id()];
						//update using equation (7) in ICDM paper
						//R_ij -= (z - w_it )*h_jt
						R_cache[j] -= ((z - old[x]) * nbr_latent.pvec[x]);
					}

					//update using equation (8) in ICDM paper
					//w_it = z;
					vdata.pvec[x] = z;
				}
			}
		}
	}
Example #26
0
void NitfWriter::done(PointTableRef table)
{
    LasWriter::done(table);

    try
    {
        ::nitf::Record record(NITF_VER_21);
        ::nitf::FileHeader header = record.getHeader();
        header.getFileHeader().set("NITF");
        header.getComplianceLevel().set(m_cLevel);
        header.getSystemType().set(m_sType);
        header.getOriginStationID().set(m_oStationId);
        header.getFileTitle().set(m_fileTitle);
        header.getClassification().set(m_fileClass);
        header.getMessageCopyNum().set("00000");
        header.getMessageNumCopies().set("00000");
        header.getEncrypted().set("0");
        header.getBackgroundColor().setRawData(const_cast<char*>("000"), 3);
        header.getOriginatorName().set(m_origName);
        header.getOriginatorPhone().set(m_origPhone);
        header.getSecurityGroup().getClassificationSystem().set(m_securityClassificationSystem);
        header.getSecurityGroup().getControlAndHandling().set(m_securityControlAndHandling);
        header.getSecurityGroup().getClassificationText().set(m_sic);

        ::nitf::DESegment des = record.newDataExtensionSegment();

        des.getSubheader().getFilePartType().set("DE");
        des.getSubheader().getTypeID().set("LIDARA DES");
        des.getSubheader().getVersion().set("01");
        des.getSubheader().getSecurityClass().set(m_securityClass);
        ::nitf::FileSecurity security = record.getHeader().getSecurityGroup();
        des.getSubheader().setSecurityGroup(security.clone());

        ::nitf::TRE usrHdr("LIDARA DES", "raw_data");
        usrHdr.setField("raw_data", "not");
        ::nitf::Field fld = usrHdr.getField("raw_data");
        fld.setType(::nitf::Field::BINARY);

        flush();
        m_oss.flush();
        std::streambuf *buf = m_oss.rdbuf();
        long size = buf->pubseekoff(0, m_oss.end);
        buf->pubseekoff(0, m_oss.beg);

        std::vector<char> bytes(size);
        buf->sgetn(bytes.data(), size);

        des.getSubheader().setSubheaderFields(usrHdr);

        ::nitf::ImageSegment image = record.newImageSegment();
        ::nitf::ImageSubheader subheader = image.getSubheader();


        BOX3D bounds =  reprojectBoxToDD(table.spatialRef(), m_bounds);

        //NITF decimal degree values for corner coordinates only has a
        // precision of 3 after the decimal. This may cause an invalid
        // polygon due to rounding errors with a small tile. Therefore
        // instead of rounding min values will use the floor value and
        // max values will use the ceiling values.
        bounds.minx = (floor(bounds.minx * 1000)) / 1000.0;
        bounds.miny = (floor(bounds.miny * 1000)) / 1000.0;
        bounds.maxx = (ceil(bounds.maxx * 1000)) / 1000.0;
        bounds.maxy = (ceil(bounds.maxy * 1000)) / 1000.0;

        double corners[4][2];
        corners[0][0] = bounds.maxy;
        corners[0][1] = bounds.minx;
        corners[1][0] = bounds.maxy;
        corners[1][1] = bounds.maxx;
        corners[2][0] = bounds.miny;
        corners[2][1] = bounds.maxx;
        corners[3][0] = bounds.miny;
        corners[3][1] = bounds.minx;
        subheader.setCornersFromLatLons(NRT_CORNERS_DECIMAL, corners);

        subheader.getImageSecurityClass().set(m_imgSecurityClass);
        subheader.setSecurityGroup(security.clone());
        if (m_imgDate.size())
            subheader.getImageDateAndTime().set(m_imgDate);

        ::nitf::BandInfo info;
        ::nitf::LookupTable lt(0,0);
        info.init("G",    /* The band representation, Nth band */
                  " ",      /* The band subcategory */
                  "N",      /* The band filter condition */
                  "   ",    /* The band standard image filter code */
                  0,        /* The number of look-up tables */
                  0,        /* The number of entries/LUT */
                  lt);     /* The look-up tables */

        std::vector< ::nitf::BandInfo> bands;
        bands.push_back(info);
        subheader.setPixelInformation(
            "INT",      /* Pixel value type */
            8,         /* Number of bits/pixel */
            8,         /* Actual number of bits/pixel */
            "R",       /* Pixel justification */
            "NODISPLY",     /* Image representation */
            "VIS",     /* Image category */
            1,         /* Number of bands */
            bands);

        subheader.setBlocking(
            8,   /*!< The number of rows */
            8,  /*!< The number of columns */
            8, /*!< The number of rows/block */
            8,  /*!< The number of columns/block */
            "P");                /*!< Image mode */

        //Image Header fields to set
        subheader.getImageId().set("None");
        subheader.getImageTitle().set(m_imgIdentifier2);

        // 64 char string
        std::string zeros(64, '0');

        std::unique_ptr< ::nitf::BandSource> band(new ::nitf::MemorySource(
            const_cast<char*>(zeros.c_str()),
            zeros.size() /* memory size */,
            0 /* starting offset */,
            1 /* bytes per pixel */,
            0 /*skip*/));
        ::nitf::ImageSource iSource;
        iSource.addBand(*band);

        //AIMIDB
        if (!m_aimidb.empty())
        {
            boost::optional<const Options&> options = m_aimidb.getOptions();
            if (options)
            {
                ::nitf::TRE tre("AIMIDB");
                std::vector<Option> opts = options->getOptions();
                for (auto i = opts.begin(); i != opts.end(); ++i)
                {
                    tre.setField(i->getName(), i->getValue<std::string>());
                }
                subheader.getExtendedSection().appendTRE(tre);
            }
        }

        //ACFTB
        if (!m_acftb.empty())
        {
            boost::optional<const Options&> options = m_acftb.getOptions();
            if (options)
            {
                ::nitf::TRE tre("ACFTB");
                std::vector<Option> opts = options->getOptions();
                for (auto i = opts.begin(); i != opts.end(); ++i)
                {
                    tre.setField(i->getName(), i->getValue<std::string>());
                }
                subheader.getExtendedSection().appendTRE(tre);
            }
        }

        ::nitf::Writer writer;
        ::nitf::IOHandle output_io(m_filename.c_str(), NITF_ACCESS_WRITEONLY,
            NITF_CREATE);
        writer.prepare(output_io, record);

        ::nitf::SegmentWriter sWriter = writer.newDEWriter(0);

        ::nitf::SegmentMemorySource sSource(bytes.data(), size, 0, 0, false);
        sWriter.attachSource(sSource);

        ::nitf::ImageWriter iWriter = writer.newImageWriter(0);
        iWriter.attachSource(iSource);

        writer.write();
        output_io.close();
    }
    catch (except::Throwable & t)
    {
        std::ostringstream oss;
        // std::cout << t.getTrace();
        throw pdal_error(t.getMessage());
    }
}
Example #27
0
USING_NAMESPACE_ACADO

int main( ){
	
	// Define variables, functions and constants:
	// ----------------------------------------------------------
    DifferentialState   dT1;
    DifferentialState   dT2;
    DifferentialState   dT3;
    DifferentialState   dT4;
    
    DifferentialState   T1;
    DifferentialState   T2;
    DifferentialState   T3;
    DifferentialState   T4;
    
    DifferentialState   W1;
    DifferentialState   W2;
    DifferentialState   W3;
    DifferentialState   W4;
    
    DifferentialState   q1;
    DifferentialState   q2;
    DifferentialState   q3;
    DifferentialState   q4;
    
    DifferentialState   Omega1;
    DifferentialState   Omega2;
    DifferentialState   Omega3;
    
    DifferentialState   V1;
    DifferentialState   V2;
    DifferentialState   V3;
    
    DifferentialState   P1;		// x
    DifferentialState   P2;		// y
    DifferentialState   P3;		// z
    
    DifferentialState   IP1;
    DifferentialState   IP2;
    DifferentialState   IP3;

    Control             U1;
    Control             U2;
    Control             U3;
    Control             U4;

    DifferentialEquation   f1, f2;   
	
    const double rho = 1.23;
    const double A = 0.1;
    const double Cl = 0.25;
    const double Cd = 0.3*Cl;
    const double m = 10;
    const double g = 9.81;
    const double L  = 0.5;
    const double Jp = 1e-2;
    const double xi = 1e-2;
    const double J1 = 0.25;
    const double J2 = 0.25;
    const double J3 = 1;
    const double gain = 1e-4;

    const double alpha = 0.0;


	// Define the quadcopter ODE model in fully nonlinear form:
	// ----------------------------------------------------------
	f1 << U1*gain; 
	f1 << U2*gain; 
	f1 << U3*gain; 
	f1 << U4*gain; 
	f1 << dT1; 
	f1 << dT2; 
	f1 << dT3; 
	f1 << dT4; 
	f1 << (T1 - W1*xi)/Jp; 
	f1 << (T2 - W2*xi)/Jp; 
	f1 << (T3 - W3*xi)/Jp; 
	f1 << (T4 - W4*xi)/Jp; 
	f1 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f1 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f1 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f1 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f1 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1; 
	f1 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2; 
	f1 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3; 
	f1 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
	f1 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
	f1 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g; 
	f1 << V1; 
	f1 << V2; 
	f1 << V3; 
	f1 << P1; 
	f1 << P2; 
	f1 << P3; 


	// Define the quadcopter ODE model in 3-stage format:
	// ----------------------------------------------------------
	
	// LINEAR INPUT SYSTEM (STAGE 1):
	Matrix M1, A1, B1;
	M1 = eye(12);
	A1 = zeros(12,12);
	B1 = zeros(12,4);
	
	A1(4,0) = 1.0;
	A1(5,1) = 1.0;
	A1(6,2) = 1.0;
	A1(7,3) = 1.0;
	A1(8,4) = 1.0/Jp;	A1(8,8) = -xi/Jp;
	A1(9,5) = 1.0/Jp;	A1(9,9) = -xi/Jp;
	A1(10,6) = 1.0/Jp;	A1(10,10) = -xi/Jp;
	A1(11,7) = 1.0/Jp;	A1(11,11) = -xi/Jp;
	
	B1(0,0) = gain;
	B1(1,1) = gain;
	B1(2,2) = gain;
	B1(3,3) = gain;
	
	// NONLINEAR SYSTEM (STAGE 2):
	f2 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f2 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f2 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f2 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4); 
	f2 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1; 
	f2 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2; 
	f2 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3; 
	f2 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
	f2 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m); 
	f2 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g; 
	
	// LINEAR OUTPUT SYSTEM (STAGE 3):
	Matrix M3, A3;
	M3 = eye(6);
	A3 = zeros(6,6);
	
	A3(3,0) = 1.0;
	A3(4,1) = 1.0;
	A3(5,2) = 1.0;
    
    OutputFcn f3;
	
	f3 << V1;
	f3 << V2;
	f3 << V3;
	f3 << 0.0;
	f3 << 0.0;
	f3 << 0.0;


	// ----------------------------------------------------------
	// ----------------------------------------------------------
	SIMexport sim1( 10, 1.0 );
	
	sim1.setModel( f1 );
	sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
	
	sim1.set( NUM_INTEGRATOR_STEPS, 50 );
	sim1.setTimingSteps( 10000 );
	
	acadoPrintf( "-----------------------------------------------------------\n  Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n" );
	sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );


	// ----------------------------------------------------------
	// ----------------------------------------------------------
	SIMexport sim2( 10, 1.0 );
	
	sim2.setLinearInput( M1, A1, B1 );
	sim2.setModel( f2 );
	sim2.setLinearOutput( M3, A3, f3 );
	sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
	
	sim2.set( NUM_INTEGRATOR_STEPS, 50 );
	sim2.setTimingSteps( 10000 );
	
	acadoPrintf( "-----------------------------------------------------------\n  Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n" );
	sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );


	return 0;
}
Example #28
0
void reset_rmse(int exec_threads){
  logstream(LOG_DEBUG)<<"Detected number of threads: " << exec_threads << std::endl;
  num_threads = exec_threads;
  rmse_vec = zeros(exec_threads);
}
bool
CppDetEstCircle4 (vector < double >x, vector < double >y,
                  double cellsize, unsigned int maxR, unsigned int sigmacoef,
                  double epsion, unsigned int maxIter, unsigned int minnumfit,
                  vector < double >&detectedcircle,
                  vector < double >&mapfinalHTcircle,
                  vector < double >&iterationdata, vector < double >&ArcInfo,
                  vector < double >&finalx, vector < double >&finaly,
                  vector < double >&DebugInfo)
{
  // based from 0.
  // detectedcircle: [yc, xc, r], in unit of meter.
  // mapfinalHTcircle: [rowc, colc, r], in unit of pixel.
  if (x.size () != y.size ()) {
#ifdef MATLABPRINT
    mexPrintf
      ("CppDetEstCircle ==> input x and y do not have the same length!\n");
#endif
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    ArcInfo.clear ();
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return false;
  }
  mxArray *mx_x, *mx_y, *mx_refmatrix;
  double *tempGetPr;

  vector < unsigned int >img, dim_img (2), dim_refmatrix (2);
  vector < double >refmatrix;
  // call CppGridPts to grid the points cloud into cellsize*cellsize cells
  bool callflag;
  callflag =
    CppGridPts (x, y, cellsize, maxR, minnumfit, img, dim_img, refmatrix,
                dim_refmatrix);
  if (!callflag) {
#ifdef MATLABPRINT
    mexPrintf ("CppDetEstCircle ==> calling CppGridPts failed!\n");
#endif
    // maybe inputs are invalid!
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    ArcInfo.clear ();
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return false;
  }
  if (img.size () == 0 || refmatrix.size () == 0) {
    // less than minnumfit points in this section
#ifdef DEBUG
    mexPrintf
      ("CppDetEstCircle ==> calling CppGridPts ==> less than minnumfit points in this section!\n");
#endif
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = 0.0;
    iterationdata[1] = mxGetNaN ();
    iterationdata[2] = mxGetNaN ();
    iterationdata[3] = static_cast < double >(x.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  }
  // convert x, y to row and col by MATLAB function: map2pix
  // xpix, ypix: based from 0.
  vector < double >xpix (x.size ()), ypix (y.size ());
  mxArray *prhs[3], *plhs[2];
  mx_x = mxCreateDoubleMatrix (x.size (), 1, mxREAL);
  mx_y = mxCreateDoubleMatrix (y.size (), 1, mxREAL);
  mx_refmatrix =
    mxCreateDoubleMatrix (dim_refmatrix[0], dim_refmatrix[1], mxREAL);
  tempGetPr = mxGetPr (mx_x);
  for (unsigned int i = 0; i < x.size (); i++) {
    tempGetPr[i] = x[i];
  }
  tempGetPr = mxGetPr (mx_y);
  for (unsigned int i = 0; i < y.size (); i++) {
    tempGetPr[i] = y[i];
  }
  tempGetPr = mxGetPr (mx_refmatrix);
  for (unsigned int i = 0; i < refmatrix.size (); i++) {
    tempGetPr[i] = refmatrix[i];
  }
  prhs[1] = mx_x;
  prhs[2] = mx_y;
  prhs[0] = mx_refmatrix;
  mexCallMATLAB (2, plhs, 3, prhs, "map2pix");
  tempGetPr = mxGetPr (plhs[1]);
  for (unsigned int i = 0; i < x.size (); i++) {
    xpix[i] = tempGetPr[i] - 1;
  }
  tempGetPr = mxGetPr (plhs[0]);
  for (unsigned int i = 0; i < y.size (); i++) {
    ypix[i] = tempGetPr[i] - 1;
  }
  mxDestroyArray (mx_x);
  mxDestroyArray (mx_y);

  vector < double >estcircle;
  CppCircleFitting (xpix, ypix, estcircle);
  if (estcircle.size () == 0) {
#ifdef DEBUG
    mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = 0.0;
    iterationdata[1] = mxGetNaN ();
    iterationdata[2] = mxGetNaN ();
    iterationdata[3] = static_cast < double >(x.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  }
  if (estcircle[2] < 0) {
#ifdef DEBUG
    mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = 0.0;
    iterationdata[1] = mxGetNaN ();
    iterationdata[2] = mxGetNaN ();
    iterationdata[3] = static_cast < double >(x.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  }
  vector < double >precircle (3, mxGetInf ());
  vector < bool > ROIimage (img.size ());
  vector < double >ROIxpix, ROIypix;
  vector < unsigned int >inputImage (img.size ());
  vector < unsigned int >edgeImage (img.size ());
  vector < unsigned int >HTrow, HTcol, HTr, HTpeak;
  vector < double >zeros (img.size (), 0.0);
  vector < double >finalHTcircle, allRsigma;
  unsigned int iterativecount = 0;
  bool fitflag = true;
  double centerchange, radiuschange, finalRsigma;
  transform (img.begin (), img.end (), zeros.begin (), ROIimage.begin (),
             std::not_equal_to < double >());
  centerchange =
    sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
          (estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
  radiuschange = fabs (estcircle[2] - precircle[2]);
  while (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
    // In this while loop, estcircle and precircle are both in unit of pixel.

    // initially detect circle by Hough Transform.
    inputImage.clear ();
    inputImage.resize (img.size (), 0);
    transform (img.begin (), img.end (), ROIimage.begin (),
               inputImage.begin (), std::multiplies < unsigned int >());
    //int debugcount=0;
    //for (unsigned int i=0; i<img.size(); i++)
    //{
    //      if ( (img[i]!=0) && ROIimage[i] )
    //      {
    //              inputImage[i]=img[i];
    //              debugcount++;
    //      }
    //}
    HTrow.clear ();
    HTcol.clear ();
    HTr.clear ();
    HTpeak.clear ();
    if (!
        (CppIterCHT
         (inputImage, dim_img, maxR, edgeImage, dim_img, HTrow, HTcol, HTr,
          HTpeak)
)) {
#ifdef MATLABPRINT
      mexPrintf ("CppDetEstCircle ==> calling CppIterCHT failed!\n");
#endif
      return false;
    }
    if (HTrow.size () == 0) {
      // no circle detected by CHT
      fitflag = false;
      break;
    }
    // select one circle from the above derived by CHT. 
    finalHTcircle.clear ();
    allRsigma.clear ();
    finalRsigma =
      CppSelectHTCircle (edgeImage, dim_img, HTrow, HTcol, HTr, finalHTcircle,
                         allRsigma);
    if (finalRsigma < 0) {
#ifdef MATLABPRINT
      mexPrintf ("CppDetEstCircle ==> calling CppSelectHTCircle failed!\n");
#endif
      return false;
    }
    // mark ROI
    ROIimage.clear ();
    if (!
        (CppMarkROI
         (edgeImage, dim_img, finalHTcircle, xpix, ypix, sigmacoef,
          finalRsigma, ROIimage, ROIxpix, ROIypix)
)) {
#ifdef MATLABPRINT
      mexPrintf ("CppDetEstCircle ==> calling CppMarkROI failed!\n");
#endif
      return false;
    }

    if (ROIxpix.size () < minnumfit) {
#ifdef DEBUG
      mexPrintf
        ("CppDetEstCircle ==> less than minnumfit points used to fit a circle!\n");
#endif
      fitflag = false;
      break;
    }
    precircle.clear ();
    precircle.resize (estcircle.size (), 0.0);
    copy (estcircle.begin (), estcircle.end (), precircle.begin ());
    // fit circles using points in ROI
    CppCircleFitting (ROIxpix, ROIypix, estcircle);
    if (estcircle.size () == 0) {
#ifdef DEBUG
      mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
      fitflag = false;
      break;
    }
    if (estcircle[2] < 0) {
#ifdef DEBUG
      mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
      fitflag = false;
      break;
    }
    // calculate change compared with previous circle
    centerchange =
      sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
            (estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
    radiuschange = fabs (estcircle[2] - precircle[2]);
    iterativecount += 1;

    if (iterativecount == maxIter) {
      fitflag = true;
      break;
    }

  }

  // Now, we begin to check if estcircle(detectedcircle) is derived in while loop and if the derived is valid. 
  // We check it from 3 aspects. 

  // 1st check: whether circle is derived, see if fitflag is true
  if (!fitflag) {
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = static_cast < double >(iterativecount);
    iterationdata[1] = centerchange * cellsize;
    iterationdata[2] = radiuschange * cellsize;
    iterationdata[3] = static_cast < double >(ROIxpix.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  }
  //2nd check: 
  //see if the detectedcircle(or estcircle) locates within point
  //cloud of trunk, if true, this horizontal point cloud section is
  //unqualified, discard the detectedcircle. If in the 3*3 window
  //centered at detectedcircle point density in every cell is greater
  //than 1, we think the circle center locating within trunk. 
  double ptsdensity;
  ptsdensity =
    CppNeighborPtsDensity (img, dim_img, 3, estcircle[0], estcircle[1]);
  if (ptsdensity < 0) {
#ifdef MATLABPRINT
    mexPrintf ("CppDetEstCircle ==> calling CppNeighborPtsDensity failed!\n");
#endif
    return false;
  }
  if (mxIsInf (ptsdensity)) {
    // If ptsdensity is Inf, the derived center locates outside the rasterized image.

    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    detectedcircle.resize (3, 0.0);
    mapfinalHTcircle.resize (3, 0.0);
    prhs[0] = mx_refmatrix;
    prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
    prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
    tempGetPr = mxGetPr (prhs[1]);
    tempGetPr[0] = estcircle[0] + 1;
    tempGetPr[1] = finalHTcircle[0] + 1;
    tempGetPr = mxGetPr (prhs[2]);
    tempGetPr[0] = estcircle[1] + 1;
    tempGetPr[1] = finalHTcircle[1] + 1;
    mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
    tempGetPr = mxGetPr (plhs[0]);      //xc
    detectedcircle[1] = tempGetPr[0];
    mapfinalHTcircle[1] = tempGetPr[1]; // xc
    tempGetPr = mxGetPr (plhs[1]);      //yc
    detectedcircle[0] = tempGetPr[0];
    mapfinalHTcircle[0] = tempGetPr[1]; // yc
    detectedcircle[2] = estcircle[2] * cellsize;
    mapfinalHTcircle[2] = finalHTcircle[2] * cellsize;  // r
    mxDestroyArray (prhs[1]);
    mxDestroyArray (prhs[2]);
    mxDestroyArray (plhs[0]);
    mxDestroyArray (plhs[1]);

    /*detectedcircle.assign(estcircle.begin(), estcircle.end());
       mapfinalHTcircle.assign(finalHTcircle.begin(), finalHTcircle.end()); */

    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = static_cast < double >(iterativecount);
    iterationdata[1] = mxGetNaN ();
    iterationdata[2] = mxGetNaN ();
    iterationdata[3] = static_cast < double >(ROIxpix.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  } else if (ptsdensity > 1) {
    // If ptsdensity is greater than 1, the derived center is on the trunk, not valid!
    detectedcircle.clear ();
    mapfinalHTcircle.clear ();
    iterationdata.clear ();
    iterationdata.resize (4, 0.0);
    iterationdata[0] = static_cast < double >(iterativecount);
    iterationdata[1] = centerchange * cellsize;
    iterationdata[2] = radiuschange * cellsize;
    iterationdata[3] = static_cast < double >(ROIxpix.size ());
    ArcInfo.clear ();
    ArcInfo.resize (3, 0.0);
    finalx.clear ();
    finaly.clear ();
    DebugInfo.clear ();
    return true;
  }
  // 3rd check: Iteration (while loop) ends possibly without getting right circle but due to reaching maximum iteration count or failed HT.
  // So here the estcircle derived from while loop is checked whether satisfying the right circle condition. 
  if (iterativecount == maxIter) {
    if (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
      detectedcircle.clear ();
      mapfinalHTcircle.clear ();
      iterationdata.clear ();
      iterationdata.resize (4, 0.0);
      iterationdata[0] = static_cast < double >(maxIter);
      iterationdata[1] = centerchange * cellsize;
      iterationdata[2] = radiuschange * cellsize;
      iterationdata[3] = static_cast < double >(ROIxpix.size ());
      ArcInfo.clear ();
      ArcInfo.resize (3, 0.0);
      finalx.clear ();
      finaly.clear ();
      DebugInfo.clear ();
      return true;
    }
  }
  //After all checked, we can output the valid derived circle.
  detectedcircle.clear ();
  mapfinalHTcircle.clear ();
  detectedcircle.resize (3, 0.0);
  mapfinalHTcircle.resize (3, 0.0);
  prhs[0] = mx_refmatrix;
  prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
  prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
  tempGetPr = mxGetPr (prhs[1]);
  tempGetPr[0] = estcircle[0] + 1;
  tempGetPr[1] = finalHTcircle[0] + 1;
  tempGetPr = mxGetPr (prhs[2]);
  tempGetPr[0] = estcircle[1] + 1;
  tempGetPr[1] = finalHTcircle[1] + 1;
  mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
  tempGetPr = mxGetPr (plhs[0]);        //xc
  detectedcircle[1] = tempGetPr[0];
  mapfinalHTcircle[1] = tempGetPr[1];   // xc
  tempGetPr = mxGetPr (plhs[1]);        //yc
  detectedcircle[0] = tempGetPr[0];
  mapfinalHTcircle[0] = tempGetPr[1];   // yc
  detectedcircle[2] = estcircle[2] * cellsize;
  mapfinalHTcircle[2] = finalHTcircle[2] * cellsize;    // r
  mxDestroyArray (prhs[1]);
  mxDestroyArray (prhs[2]);
  mxDestroyArray (plhs[0]);
  mxDestroyArray (plhs[1]);

  // convert ROIxpix, ROIypix to finalx, finaly
  finalx.clear ();
  finalx.resize (ROIxpix.size ());
  finaly.clear ();
  finaly.resize (ROIypix.size ());
  prhs[0] = mx_refmatrix;
  prhs[1] = mxCreateDoubleMatrix (ROIypix.size (), 1, mxREAL);
  prhs[2] = mxCreateDoubleMatrix (ROIxpix.size (), 1, mxREAL);
  tempGetPr = mxGetPr (prhs[1]);
  for (unsigned int i = 0; i < ROIypix.size (); i++) {
    tempGetPr[i] = ROIypix[i] + 1;
  }
  tempGetPr = mxGetPr (prhs[2]);
  for (unsigned int i = 0; i < ROIxpix.size (); i++) {
    tempGetPr[i] = ROIxpix[i] + 1;
  }
  mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
  tempGetPr = mxGetPr (plhs[0]);        //finalx
  for (unsigned int i = 0; i < ROIxpix.size (); i++) {
    finalx[i] = tempGetPr[i];
  }
  tempGetPr = mxGetPr (plhs[1]);        //finaly
  for (unsigned int i = 0; i < ROIypix.size (); i++) {
    finaly[i] = tempGetPr[i];
  }
  mxDestroyArray (prhs[1]);
  mxDestroyArray (prhs[2]);
  mxDestroyArray (plhs[0]);
  mxDestroyArray (plhs[1]);
  mxDestroyArray (mx_refmatrix);

  iterationdata.clear ();
  iterationdata.resize (4, 0.0);
  iterationdata[0] = static_cast < double >(iterativecount);
  iterationdata[1] = centerchange * cellsize;
  iterationdata[2] = radiuschange * cellsize;
  iterationdata[3] = static_cast < double >(ROIxpix.size ());
  ArcInfo.clear ();
  if (!CppArcInfo (ROIxpix, ROIypix, estcircle, ArcInfo)) {
#ifdef MATLABPRINT
    mexPrintf ("CppDetEstCircle ==> calling CppArcInfo failed!\n");
#endif
    return false;
  }
  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  // Because when rasterizing point clouds
  // in the function CppGridPts the center of the first pixel, 
  // i.e. the pixel at [1,1] locates at (minx, maxy), 
  // we have to transform the angle in i-j(raster) coordinate system
  // to x-y(point clouds) coordinate system. 
  // In x-y coordinate system, y axis points to north, 
  // so angle is presented in counterclockwise. 
  // In contrast, in i-j coordinate system, 
  // i axis (i.e. y axis in raster) points to south, 
  // so angle is presented in clockwise.
  // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ArcInfo[0] = 2 * PI - ArcInfo[0];
  ArcInfo[1] = 2 * PI - ArcInfo[1];

  DebugInfo.clear ();
  DebugInfo.resize (1);
  DebugInfo[0] = finalRsigma * cellsize;

#ifdef DEBUG
  mexPrintf ("CppDetEstCircle ==> finally points used to fit a circle: %d!\n",
             ROIxpix.size ());
#endif

  return true;
}
Example #30
0
//------------------------------------------------------------------------------
void HamiltonMatrix::setHamiltonian(mat h) {
    H = h;
    Ht = zeros(h.n_rows ,h.n_cols);
}