// RK4 integration void double_integrator_dynamics( double *z, double *f, void *user_data ) { // Double integrator dynamics, 2d int num_states = 4; int num_controls = 2; MatrixXd A(num_states, num_states); MatrixXd B(num_states, num_controls); Vector4d x_t(z[0], z[1], z[2], z[3]); Vector2d u_t(z[4], z[5]); double t = z[6]; //double t = ((double*) user_data)[0]; A.topRightCorner(num_states/2, num_states/2).setIdentity(); B.bottomRows(num_controls).setIdentity(); Vector4d k1 = A * x_t + B * u_t; Vector4d k2 = A * (x_t + (k1.array()/2.0).matrix()) + B * u_t; Vector4d k3 = A * (x_t + (k2.array()/2.0).matrix()) + B * u_t; Vector4d k4 = A * (x_t + k3) + B * u_t; Vector4d x_new = (x_t.array() + t/6 * (k1.array() + 2*k2.array() + 2*k3.array() + k4.array())).matrix(); f[0] = x_new(0); f[1] = x_new(1); f[2] = x_new(2); f[3] = x_new(3); }
int main(void) { serial_open(19200, SERIAL_8N1); ADMUX |= (1<<REFS0); ADCSRA|=(1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); pinMode(53, OUTPUT); pinMode(13, OUTPUT); x_init(); x_new(1, dimmer, 1); x_new(2, setDelay, 1); x_new(3, blinker, 1); while (1) { x_delay(500); } }
void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x) { int iter = 0; int m = A.size().height; int n = A.size().width; Mat_<double> AT = A.t(); double error = 1e-8; Mat_<double> H = AT*A; Mat_<double> f = -AT*b; Mat_<double> x_old = Mat_<double>::zeros(n,1); Mat_<double> x_new = Mat_<double>::zeros(n,1); Mat_<double> mu_old = Mat_<double>::zeros(n,1); Mat_<double> mu_new = Mat_<double>::zeros(n,1); Mat_<double> r = Mat_<double>::zeros(n,1); f.copyTo(mu_old); while(iter < NNLS_MAX_ITER) { iter++; for(int k=0;k<n;k++) { x_old.copyTo(x_new); x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) ); if(x_new(k,0) != x_old(k,0)) { r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k); r.copyTo(mu_new); } x_new.copyTo(x_old); mu_new.copyTo(mu_old); } if(eKKT(H,f,x_new,error) == true) { break; } } x_new.copyTo(x); }
bool PowInt<T>::propagate() { T& x = *arg1; const T& y = *(this->val); // Inverse operator T x_new( sqrt(y) ); if (Inf(x) >= 0.0) ; else if (Sup(x) <= 0.0) x_new = T(-Sup(x_new), -Inf(x_new)); else x_new = T(-Sup(x_new), Sup(x_new)); if ( Disjoint(x, x_new) ) return true; else { x = Intersect(x, x_new); } return false; }
void CG3::operator()(cudaColorSpinorField &x, cudaColorSpinorField &b) { // Check to see that we're not trying to invert on a zero-field source const double b2 = norm2(b); if(b2 == 0){ profile.TPSTOP(QUDA_PROFILE_INIT); printfQuda("Warning: inverting on zero-field source\n"); x=b; param.true_res = 0.0; param.true_res_hq = 0.0; return; } ColorSpinorParam csParam(x); csParam.create = QUDA_ZERO_FIELD_CREATE; cudaColorSpinorField x_prev(b, csParam); cudaColorSpinorField r_prev(b, csParam); cudaColorSpinorField temp(b, csParam); cudaColorSpinorField r(b); cudaColorSpinorField w(b); mat(r, x, temp); // r = Mx double r2 = xmyNormCuda(b,r); // r = b - Mx PrintStats("CG3", 0, r2, b2, 0.0); double stop = stopping(param.tol, b2, param.residual_type); if(convergence(r2, 0.0, stop, 0.0)) return; // First iteration mat(w, r, temp); double rAr = reDotProductCuda(r,w); double rho = 1.0; double gamma_prev = 0.0; double gamma = r2/rAr; cudaColorSpinorField x_new(x); cudaColorSpinorField r_new(r); axpyCuda(gamma, r, x_new); // x_new += gamma*r axpyCuda(-gamma, w, r_new); // r_new -= gamma*w // end of first iteration // axpbyCuda(a,b,x,y) => y = a*x + b*y int k = 1; // First iteration performed above double r2_prev; while(!convergence(r2, 0.0, stop, 0.0) && k<param.maxiter){ x_prev = x; x = x_new; r_prev = r; r = r_new; mat(w, r, temp); rAr = reDotProductCuda(r,w); r2_prev = r2; r2 = norm2(r); // Need to rearrange this! PrintStats("CG3", k, r2, b2, 0.0); gamma_prev = gamma; gamma = r2/rAr; rho = 1.0/(1. - (gamma/gamma_prev)*(r2/r2_prev)*(1.0/rho)); x_new = x; axCuda(rho,x_new); axpyCuda(rho*gamma,r,x_new); axpyCuda((1. - rho),x_prev,x_new); r_new = r; axCuda(rho,r_new); axpyCuda(-rho*gamma,w,r_new); axpyCuda((1.-rho),r_prev,r_new); double rr_old = reDotProductCuda(r_new,r); printfQuda("rr_old = %1.14lf\n", rr_old); k++; } if(k == param.maxiter) warningQuda("Exceeded maximum iterations %d", param.maxiter); // compute the true residual mat(r, x, temp); param.true_res = sqrt(xmyNormCuda(b, r)/b2); PrintSummary("CG3", k, r2, b2); return; }
static bool _internal_get_from_bb_collection_alloc (xmmsv_t *bb, xmmsv_coll_t **coll) { int i; int32_t type; int32_t n_items; int id; int32_t *idlist = NULL; char *key, *val; /* Get the type and create the collection */ if (!_internal_get_from_bb_int32_positive (bb, &type)) { return false; } *coll = xmmsv_coll_new (type); /* Get the list of attributes */ if (!_internal_get_from_bb_int32_positive (bb, &n_items)) { goto err; } for (i = 0; i < n_items; i++) { unsigned int len; if (!_internal_get_from_bb_string_alloc (bb, &key, &len)) { goto err; } if (!_internal_get_from_bb_string_alloc (bb, &val, &len)) { free (key); goto err; } xmmsv_coll_attribute_set (*coll, key, val); free (key); free (val); } /* Get the idlist */ if (!_internal_get_from_bb_int32_positive (bb, &n_items)) { goto err; } if (!(idlist = x_new (int32_t, n_items + 1))) { goto err; } for (i = 0; i < n_items; i++) { if (!_internal_get_from_bb_int32 (bb, &id)) { goto err; } idlist[i] = id; } idlist[i] = 0; xmmsv_coll_set_idlist (*coll, idlist); free (idlist); idlist = NULL; /* Get the operands */ if (!_internal_get_from_bb_int32_positive (bb, &n_items)) { goto err; } for (i = 0; i < n_items; i++) { xmmsv_coll_t *operand; if (!_internal_get_from_bb_int32_positive (bb, &type) || type != XMMSV_TYPE_COLL || !_internal_get_from_bb_collection_alloc (bb, &operand)) { goto err; } xmmsv_coll_add_operand (*coll, operand); xmmsv_coll_unref (operand); } return true; err: if (idlist != NULL) { free (idlist); } xmmsv_coll_unref (*coll); return false; }
DBL_MATRIX Alignment::lineDistort_(DBL_MATRIX& linePoints, DBL_MATRIX& y, DBL_MATRIX& rt_vec, double c1, double c2, DBL_MATRIX& alpha) { if(alpha.columnCount() != 1){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Wrong dimensionality of input - alpha must be a column vector!" << std::endl; } mspp_precondition(alpha.columnCount() == 1 , "Alignment::lineDistort_(): Wrong dimensionality of input - alpha must be a column vector!"); if(linePoints.columnCount() != y.columnCount()-1){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Dimension mismatch - the dimension of linePoints must be one less than the dimension of y!" << std::endl; } mspp_precondition(alpha.columnCount() == 1 , "Alignment::lineDistort_(): Dimension mismatch - the dimension of linePoints must be one less than the dimension of y!"); if(alpha.rowCount() != linePoints.rowCount()){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Dimension mismatch - the length of alpha does not match the given data!" << std::endl; } mspp_precondition(alpha.columnCount() == 1 , "Alignment::lineDistort_(): Dimension mismatch - the length of alpha does not match the given data!"); if(linePoints.columnCount() != rt_vec.columnCount()-1){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Dimension mismatch - rt_vec must be 1 dimension larger than linePoints!" << std::endl; } mspp_precondition(linePoints.columnCount() == rt_vec.columnCount()-1 , "Alignment::lineDistort_(): Dimension mismatch - rt_vec must be 1 dimension larger than linePoints!"); if(y.columnCount() != rt_vec.columnCount()){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Dimension mismatch - rt_vec and y must have equal dimensionality!" << std::endl; } mspp_precondition(y.columnCount() == rt_vec.columnCount() , "Alignment::lineDistort_(): Dimension mismatch - rt_vec and y must have equal dimensionality!"); //get number of dimensions int dim = rt_vec.columnCount(); int size = linePoints.rowCount(); //go through points in line for(int u = 0; u < size; u++){ MSPP_LOG(logINFO) << "Alignment: Processing line point " << u+1 << "/" << size << std::endl; //calculate distance vector between current point u and next point u+1 std::vector<double> dist (dim-1); if(u < size-1){ for(int i = 0; i < dim-1; i++){ dist[i] = linePoints(u+1,i) - linePoints(u,i); } } //init stepsize lambda double lambda = 1; //get current support vector DBL_MATRIX y_curr = extractRow(y,u).transpose(); //get current line points DBL_MATRIX linePoints_curr = extractRow(linePoints,u).transpose(); //init iteration counter int ITER = 0; while(ITER < 10000){ ITER++; if(ITER==1000){ MSPP_LOG(logDEBUG) << "Alignment::lineDistort_(): Algorithm probably does not converge!" << std::endl; } if(ITER==9999){ MSPP_LOG(logERROR) << "Alignment::lineDistort_(): Too many iterations. Algorithm does not converge!" << std::endl; } //calculate negative gradient on hyperplane DBL_MATRIX proj_vec = -Alignment::gradOnHPlane_(linePoints_curr,y_curr,rt_vec,alpha(u,0)); //calculate vector with length 1 pointing in gradient direction double proj_vec_norm = 0; for(int i = 0; i<proj_vec.rowCount(); i++){ proj_vec_norm += proj_vec(i,0)*proj_vec(i,0); } proj_vec_norm = sqrt(proj_vec_norm); DBL_MATRIX d (proj_vec.rowCount(),1); for(int i = 0; i<proj_vec.rowCount(); i++){ d(i,0) = proj_vec(i,0)/proj_vec_norm; } //compute new stepsize lambda = Alignment::stepSizeND_(linePoints_curr,y_curr,d,lambda,c1,c2,rt_vec,alpha(u,0)); //new position of current point u DBL_MATRIX x_new (d.rowCount(),1); for(int i = 0; i<d.rowCount(); i++){ x_new(i,0) = linePoints_curr(i,0) + lambda*d(i,0); } //termination condition double mean_change = 0; for(int i = 0; i<x_new.size(); i++){ mean_change += abs(x_new(i,0) - linePoints_curr(i,0)); } //update current linepoints linePoints_curr = x_new; //double cond = (0.1*lambda>1e-5 ? 0.1*lambda : 1e-5); double cond = 1e-3; if(mean_change < cond){ break; }; } for(int i = 0; i<linePoints_curr.rowCount(); i++){ linePoints(u,i) = linePoints_curr(i,0); } //shift next point u+1 to a better start position if(u < linePoints.rowCount()-1){ for(unsigned int i = 0; i<dist.size(); i++){ linePoints(u+1,i) = (linePoints(u+1,i) + linePoints(u,i) + dist[i])/2.; } } } //Output of line distortion DBL_MATRIX newLinePoints (linePoints.rowCount(),linePoints.columnCount()+1); //calculate last component for(int i = 0; i<linePoints.rowCount(); i++){ double sum_y = 0; for(int k = 0; k<y.columnCount();k++){ sum_y += y(i,k); } double sum_lp = 0; for(int k = 0; k<linePoints.columnCount();k++){ sum_lp += linePoints(i,k); } double z = sum_y - sum_lp; for(int k = 0; k<newLinePoints.columnCount()-1;k++){ newLinePoints(i,k) = linePoints(i,k); } newLinePoints(i,newLinePoints.columnCount()-1) = z; } //check monotonicity of MTS bool sane = true; for(int i = 0; i < newLinePoints.columnCount(); i++){ for(int j = 1; j < newLinePoints.rowCount(); j++){ if(newLinePoints(j,i) < newLinePoints(j-1,i)){ sane = false; } } } if(!sane){ std::cout << "WARNING: Master Time Scale is not monotonous. Results may be incorrect!" << std::endl; } return newLinePoints; }
int local(Trial &T, TBox &box, TBox &domain, double eps_cl, double *mgr, Global &glob, int axis, RCRVector x_av #ifdef NLOPT_UTIL_H , nlopt_stopping *stop #endif ) { int n=box.GetDim(); RVector x(n); double tmp, f; x=T.xvals ; #ifdef LS_DEBUG cout << "Local Search, x=" << x << endl; #endif if (box.OutsideBox(x, domain) != 0) { cout << "Starting point is not inside the boundary. Exiting...\n" ; exit(1) ; return LS_Out ; } // Check if we are close to a stationary point located previously if (box.CloseToMin(x, &tmp, eps_cl)) { #ifdef LS_DEBUG cout << "Close to a previously located stationary point, exiting" << endl; #endif T.objval=tmp; return LS_Old ; } #if 0 if (axis != -1) { cout << "NLopt code only works with axis == -1, exiting...\n" ; exit(EXIT_FAILURE); } f_local_data data; data.glob = &glob; data.maxgrad = *mgr; data.stop = stop; nlopt_result ret = nlopt_minimize(NLOPT_LOCAL_LBFGS, n, f_local, &data, box.lb.raw_data(), box.ub.raw_data(), x.raw_data(), &f, stop->minf_max, stop->ftol_rel, stop->ftol_abs, stop->xtol_rel, stop->xtol_abs, stop->maxeval - stop->nevals, stop->maxtime - stop->start); *mgr = data.maxgrad; T.xvals=x ; T.objval=f ; if (ret == NLOPT_MAXEVAL_REACHED || ret == NLOPT_MAXTIME_REACHED) return LS_MaxEvalTime; else if (ret > 0) return LS_New; else return LS_Out; // failure #else /* not using NLopt local optimizer ... use original STOgo BFGS code */ int k_max, info, outside = 0; int k, i, good_enough, iTmp ; double maxgrad, delta, f_new; double alpha, gamma, beta, d2, s2, nom, den, ro ; double nrm_sd, nrm_hn, snrm_hn, nrm_dl ; RVector g(n), h_sd(n), h_dl(n), h_n(n), x_new(n), g_new(n) ; RVector s(n),y(n),z(n),w(n) ; // Temporary vectors RMatrix B(n), H(n) ; // Hessian and it's inverse k_max = max_iter*n ; // Initially B and H are equal to the identity matrix B=0 ; H=0 ; for (i=0 ; i<n ; i++) { B(i,i)=1 ; H(i,i)=1 ; } RVector g_av(x_av.GetLength()); if (axis==-1) { f=glob.ObjectiveGradient(x,g,OBJECTIVE_AND_GRADIENT); } else { x_av(axis)=x(0); f=glob.ObjectiveGradient(x_av,g_av,OBJECTIVE_AND_GRADIENT); g(0)=g_av(axis); } IF_NLOPT_CHECK_EVALS; FC++;GC++; if (axis == -1) { // Skipping AV #ifdef INI3 // Elaborate scheme to initalize delta delta=delta_coef*norm2(g) ; copy(g,z) ; axpy(1.0,x,z) ; if (!box.InsideBox(z)) { if (box.Intersection(x,g,z)==TRUE) { axpy(-1.0,x,z) ; delta=min(delta,delta_coef*norm2(z)) ; } else { // Algorithm broke down, use INI1 delta = (1.0/7)*box.ShortestSide(&iTmp) ; } } #endif #ifdef INI2 // Use INI2 scheme delta = box.ClosestSide(x)*delta_coef ; if (delta<MacEpsilon) // Patch to avoid trust region with radius close to zero delta = (1.0/7)*box.ShortestSide(&iTmp) ; #endif #ifdef INI1 delta = delta_coef*box.ShortestSide(&iTmp) ; #endif } else { // Use a simple scheme for the 1D minimization (INI1) delta = (1.0/7.0)*box.ShortestSide(&iTmp) ; } k=0 ; good_enough = 0 ; info=LS_New ; outside=0 ; maxgrad=*mgr ; while (good_enough == 0) { k++ ; if (k>k_max) { #ifdef LS_DEBUG cout << "Maximum number of iterations reached\n" ; #endif info=LS_MaxIter ; break ; } // Update maximal gradient value maxgrad=max(maxgrad,normInf(g)) ; // Steepest descent, h_sd = -g copy(g,h_sd) ; scal(-1.0,h_sd) ; nrm_sd=norm2(h_sd) ; if (nrm_sd < epsilon) { // Stop criterion (gradient) fullfilled #ifdef LS_DEBUG cout << "Gradient small enough" << endl ; #endif good_enough = 1 ; break ; } // Compute Newton step, h_n = -H*g gemv('N',-1.0, H, g, 0.0, h_n) ; nrm_hn = norm2(h_n) ; if (nrm_hn < delta) { // Pure Newton step copy(h_n, h_dl) ; #ifdef LS_DEBUG cout << "[Newton step] " ; #endif } else { gemv('N',1.0,B,g,0.0,z) ; tmp=dot(g,z) ; if (tmp==0) { info = LS_Unstable ; break ; } alpha=(nrm_sd*nrm_sd)/tmp ; // Normalization (N38,eq. 3.30) scal(alpha,h_sd) ; nrm_sd=fabs(alpha)*nrm_sd ; if (nrm_sd >= delta) { gamma = delta/nrm_sd ; // Normalization (N38, eq. 3.33) copy(h_sd,h_dl) ; scal(gamma,h_dl) ; #ifdef LS_DEBUG cout << "[Steepest descent] " ; #endif } else { // Combination of Newton and SD steps d2 = delta*delta ; copy(h_sd,s) ; s2=nrm_sd*nrm_sd ; nom = d2 - s2 ; snrm_hn=nrm_hn*nrm_hn ; tmp = dot(h_n,s) ; den = tmp-s2 + sqrt((tmp-d2)*(tmp-d2)+(snrm_hn-d2)*(d2-s2)) ; if (den==0) { info = LS_Unstable ; break ; } // Normalization (N38, eq. 3.31) beta = nom/den ; copy(h_n,h_dl) ; scal(beta,h_dl) ; axpy((1-beta),h_sd,h_dl) ; #ifdef LS_DEBUG cout << "[Mixed step] " ; #endif } } nrm_dl=norm2(h_dl) ; //x_new = x+h_dl ; copy(x,x_new) ; axpy(1.0,h_dl,x_new) ; // Check if x_new is inside the box iTmp=box.OutsideBox(x_new, domain) ; if (iTmp == 1) { #ifdef LS_DEBUG cout << "x_new is outside the box " << endl ; #endif outside++ ; if (outside>max_outside_steps) { // Previous point was also outside, exit break ; } } else if (iTmp == 2) { #ifdef LS_DEBUG cout << " x_new is outside the domain" << endl ; #endif info=LS_Out ; break ; } else { outside=0 ; } // Compute the gain if (axis==-1) f_new=glob.ObjectiveGradient(x_new,g_new,OBJECTIVE_AND_GRADIENT); else { x_av(axis)=x_new(0); f_new=glob.ObjectiveGradient(x_av,g_av,OBJECTIVE_AND_GRADIENT); } IF_NLOPT_CHECK_EVALS; FC++; GC++; gemv('N',0.5,B,h_dl,0.0,z); ro = (f_new-f) / (dot(g,h_dl) + dot(h_dl,z)); // Quadratic model if (ro > 0.75) { delta = delta*2; } if (ro < 0.25) { delta = delta/3; } if (ro > 0) { // Update the Hessian and it's inverse using the BFGS formula #if 0 // changed by SGJ to compute OBJECTIVE_AND_GRADIENT above if (axis==-1) glob.ObjectiveGradient(x_new,g_new,GRADIENT_ONLY); else { x_av(axis)=x_new(0); glob.ObjectiveGradient(x_av,g_av,GRADIENT_ONLY); g_new(0)=g_av(axis); } GC++; IF_NLOPT_CHECK_EVALS; #else if (axis != -1) g_new(0)=g_av(axis); #endif // y=g_new-g copy(g_new,y); axpy(-1.0,g,y); // Check curvature condition alpha=dot(y,h_dl); if (alpha <= sqrt(MacEpsilon)*nrm_dl*norm2(y)) { #ifdef LS_DEBUG cout << "Curvature condition violated " ; #endif } else { // Update Hessian gemv('N',1.0,B,h_dl,0.0,z) ; // z=Bh_dl beta=-1/dot(h_dl,z) ; ger(1/alpha,y,y,B) ; ger(beta,z,z,B) ; // Update Hessian inverse gemv('N',1.0,H,y,0.0,z) ; // z=H*y gemv('T',1.0,H,y,0.0,w) ; // w=y'*H beta=dot(y,z) ; beta=(1+beta/alpha)/alpha ; // It should be possible to do this updating more efficiently, by // exploiting the fact that (h_dl*y'*H) = transpose(H*y*h_dl') ger(beta,h_dl,h_dl,H) ; ger(-1/alpha,z,h_dl,H) ; ger(-1/alpha,h_dl,w,H) ; } if (nrm_dl < norm2(x)*epsilon) { // Stop criterion (iteration progress) fullfilled #ifdef LS_DEBUG cout << "Progress is marginal" ; #endif good_enough = 1 ; } // Check if we are close to a stationary point located previously if (box.CloseToMin(x_new, &f_new, eps_cl)) { // Note that x_new and f_new may be overwritten on exit from CloseToMin #ifdef LS_DEBUG cout << "Close to a previously located stationary point, exiting" << endl; #endif info = LS_Old ; good_enough = 1 ; } // Update x, g and f copy(x_new,x) ; copy(g_new,g) ; f=f_new ; #ifdef LS_DEBUG cout << " x=" << x << endl ; #endif } else { #ifdef LS_DEBUG cout << "Step is no good, ro=" << ro << " delta=" << delta << endl ; #endif } } // wend // Make sure the routine returns correctly... // Check if last iterate is outside the boundary if (box.OutsideBox(x, domain) != 0) { info=LS_Out; f=DBL_MAX; } if (info == LS_Unstable) { cout << "Local search became unstable. No big deal but exiting anyway\n" ; exit(1); } *mgr=maxgrad ; T.xvals=x ; T.objval=f ; if (outside>0) return LS_Out ; else return info ; #endif }
int main(int argc, char **argv) { ros::init(argc, argv, "model_validation"); double current_time; /** Linearized model initial values and operation points */ double state[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; double input[4] = {0.0, 0.0, 0.0, 0.0}; double state_op[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; double *new_state = (double *) malloc(sizeof(double) * 12); Eigen::Map<Eigen::VectorXd> x(&state[0], num_states_); Eigen::Map<Eigen::VectorXd> u(&input[0], num_inputs_); Eigen::Map<Eigen::VectorXd> x_new(new_state, num_states_); /** Nonlinear simulator initial values */ double *new_NL_state = (double *) malloc(sizeof(double) * 12); double NL_state[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; Eigen::Map<Eigen::VectorXd> x_NL_new(new_NL_state, num_states_); Eigen::Map<Eigen::VectorXd> x_NL(&NL_state[0], num_states_); u_.resize(4); x_.resize(12); x_NL_.resize(12); time_.push_back(0.0); for (int j = 0; j < num_inputs_; j++) { u_[j].push_back(u(j)); } for (int j = 0; j < num_states_; j++) { x_[j].push_back(x(j)); x_NL_[j].push_back(x_NL(j)); } /** Simulation **/ double t_sim = 3.5; int num_iteration = ceil(t_sim / ts_); for (int i = 0; i < num_iteration; i++) { current_time = i * ts_; /** INPUT SELECTION (359.484 is the stable value for the rotors to hover) **/ if ((current_time >= 0. && current_time < 1.) || (current_time > 2.5 && current_time < 3.)) { u(0) = 359.484; u(1) = 359.484; u(2) = 359.484; u(3) = 359.484; } else if (current_time > 1. && current_time < 2.5) { u(0) = 370.; u(1) = 370.; u(2) = 370.; u(3) = 370.; } else { u(0) = 359.484; u(1) = 359.484; u(2) = 359.484; u(3) = 359.484; } /** STATE SPACE FORMULATION AND CALCULATION OF THE NEXT STEP **/ new_state = simulatePlant(state, input, state_op, ts_); for (int j = 0; j < num_states_; j++) { state[j] = new_state[j]; x_[j].push_back(x(j)); } new_NL_state = simulateNonLinearPlant(NL_state, input); for (int j = 0; j < num_states_; j++) { NL_state[j] = new_NL_state[j]; x_NL_[j].push_back(x_NL(j)); } /** WRITING TO RECORDING VECTORS **/ for (int j = 0; j < num_inputs_; j++) { u_[j].push_back(u(j)); } time_.push_back(current_time); i++; } bool write = false; write = writeToDisc(); if (write) std::cout << "Satisfactory recorded validation data." << std::endl; return 0; }
double* simulateNonLinearPlant(double *current_state, double *current_input) { // Map into Eigen objects for easier manipulation Eigen::Map<Eigen::VectorXd> x_current(current_state, 12, 1); Eigen::Map<Eigen::VectorXd> u_current(current_input, 4, 1); Eigen::VectorXd x_new = Eigen::MatrixXd::Zero(num_states_, 1); double phi = x_current(6); double theta = x_current(7); double psi = x_current(8); double p = x_current(9); double q = x_current(10); double r = x_current(11); double U1 = Ct_ * (pow(u_current(0),2) + pow(u_current(1),2) + pow(u_current(2),2) + pow(u_current(3),2)); double U2 = Ct_ * (- pow(u_current(1),2) + pow(u_current(3),2)); double U3 = Ct_ * (pow(u_current(0),2) - pow(u_current(2),2)); double U4 = Cq_ * (-pow(u_current(0),2) + pow(u_current(1),2) - pow(u_current(2),2) + pow(u_current(3),2)); // Solve the difference equations recursively x_new(0) = x_current(0) + ts_ * x_current(3); x_new(1) = x_current(1) + ts_ * x_current(4); x_new(2) = x_current(2) + ts_ * x_current(5); x_new(3) = x_current(3) + (ts_ / m_) * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi)) * U1; x_new(4) = x_current(4) + (ts_ / m_) * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)) * U1; x_new(5) = x_current(5) + (ts_ / m_) * (-m_ * g_ + cos(theta) * cos(phi) * U1); x_new(6) = x_current(6) + ts_ * (p + q * sin(phi) * tan(theta) + r * cos(phi) * tan(theta)); x_new(7) = x_current(7) + ts_ * (q * cos(phi) - r * sin(phi)); x_new(8) = x_current(8) + ts_ * (q * sin(phi) + r * cos(phi)) / cos(theta); x_new(9) = x_current(9) + ts_ * ((Iyy_ - Izz_) * q * r / Ixx_ + (d_ / Ixx_) * U2); x_new(10) = x_current(10) + ts_* ((Izz_ - Ixx_) * p * r / Iyy_ + (d_ / Iyy_) * U3); x_new(11) = x_current(11) + ts_* ((Ixx_ - Iyy_) * p * q / Izz_ + (1 / Izz_) * U4); x_current = x_new; return current_state; }
bool SolverUnconstrained<Data,Problem>::optimize( vector_type& x ) { M_solver_stats.clear(); // Controlling parameters // trust region radius value_type Delta = M_options.Delta_init; vector_type x_new ( x.size() ); vector_type stot ( x.size() ); value_type norm_Tgrad_fx = this->norm_Theta_x_grad_fx( x, Delta ); value_type norm_s_til = 0; M_prob.copy_x0_to_x( x ); // FIXME: if ( M_options.verbose() ) //M_prob.print_complete( x ); if ( M_solver_stats.collectStats() ) M_solver_stats.push( norm_Tgrad_fx, 0, 0, 0, 0, 0, 0, 0, Delta, 0, 0, 0 ); try { int iter = 0; // // Apply bound constrained Trust region algorithm // while ( iter == 0 || ( iter < M_options.max_TR_iter && norm_Tgrad_fx > M_options.TR_tol ) ) { iter++; int n_CGiter, n_restarts, n_indef, n_crosses_def, n_crosses_indef, n_truss_exit_def, n_truss_exit_indef; value_type _s_til_x_G_til_x_s_til, phi_til, rho, Delta_used = Delta; DVLOG(2) << "\n===================== iter = " << iter << " ==========================="; //DVLOG(2) << "\nx = " << x << "\n"; DVLOG(2) << "\n -> norm_Tgrad_fx = " << norm_Tgrad_fx; /** find an approximate stot for the step to make * solve : * Find \f$\tilde{s}^k = \mathrm{arg}\mathrm{min}_{s \in R^n}{\tilde{\phi}^k : ||s|| < \Delta^k}\f$ */ this->CGstep( x, Delta, stot, norm_s_til, n_CGiter, n_restarts, n_indef, n_crosses_def, n_crosses_indef, n_truss_exit_def, n_truss_exit_indef, _s_til_x_G_til_x_s_til, phi_til ); // x_new = x + stot; f_type __fx_new; M_prob.evaluate( x_new, __fx_new, diff_order<0>() ); f_type __fx; M_prob.evaluate( x, __fx, diff_order<0>() ); // compute actual merit function reduction value_type ared_til = __fx_new.value( 0 ) - __fx.value( 0 ) + 0.5 * _s_til_x_G_til_x_s_til; rho = ared_til / phi_til; ///////////////////////////////////////////////////////////////////////// // Trust region radius calculation: if ( M_options.allow_Trust_radius_calculations ) { if ( rho <= 1e-8 ) Delta = M_options.rho_decrease * Delta; else { x = x + stot; if ( rho > M_options.rho_big ) Delta = std::max( M_options.rho_increase_big*norm_s_til, Delta ); else if ( rho > M_options.rho_small ) Delta = std::max( M_options.rho_increase_small*norm_s_til, Delta ); } norm_Tgrad_fx = this->norm_Theta_x_grad_fx( x, Delta ); } else { x = x + stot; norm_Tgrad_fx = this->norm_Theta_x_grad_fx( x, Delta ); } ///////////////////////////////////////////////////////////////////////// if ( M_solver_stats.collectStats() ) { M_solver_stats.push( norm_Tgrad_fx, n_CGiter, n_restarts, n_indef, n_crosses_def, n_crosses_indef, n_truss_exit_def, n_truss_exit_indef, Delta_used, ared_til, phi_til, rho ); } if ( norm_Tgrad_fx > 1e-5 ) M_prob.setAccuracy( std::min( 1e-1, norm_Tgrad_fx ) ); DVLOG(2) << "norm_Tgrad_fx = " << norm_Tgrad_fx << "\n"; } } catch ( std::exception const& __ex ) { f_type __fx_new; M_prob.evaluate ( x, __fx_new, diff_order<2>() ); if ( norm_inf( __fx_new.gradient( 0 ) ) > 1e-10 ) throw __ex; } vector_type __l( _E_n ), __u( _E_n ); lambda_LS( x, __l, __u ); if ( M_solver_stats.collectStats() ) M_solver_stats.push( x, __l, __u ); return true; }
static bool _internal_get_from_bb_collection_alloc (xmmsv_t *bb, xmmsv_coll_t **coll) { int i; int32_t type; int32_t n_items; int id; int32_t *idlist = NULL; xmmsv_t *dict, *attrs; xmmsv_dict_iter_t *it; /* Get the type and create the collection */ if (!_internal_get_from_bb_int32_positive (bb, &type)) { return false; } *coll = xmmsv_coll_new (type); /* Get the attributes */ if (!_internal_get_from_bb_value_dict_alloc (bb, &dict)) { return false; } attrs = xmmsv_coll_attributes_get (*coll); xmmsv_get_dict_iter (dict, &it); while (xmmsv_dict_iter_valid (it)) { const char *key; xmmsv_t *value; xmmsv_dict_iter_pair (it, &key, &value); xmmsv_dict_set (attrs, key, value); xmmsv_dict_iter_next (it); } xmmsv_unref (dict); /* Get the idlist */ if (!_internal_get_from_bb_int32_positive (bb, &n_items)) { goto err; } if (!(idlist = x_new (int32_t, n_items + 1))) { goto err; } for (i = 0; i < n_items; i++) { if (!_internal_get_from_bb_int32 (bb, &id)) { goto err; } idlist[i] = id; } idlist[i] = 0; xmmsv_coll_set_idlist (*coll, idlist); free (idlist); idlist = NULL; /* Get the operands */ if (!_internal_get_from_bb_int32_positive (bb, &n_items)) { goto err; } for (i = 0; i < n_items; i++) { xmmsv_coll_t *operand; if (!_internal_get_from_bb_int32_positive (bb, &type) || type != XMMSV_TYPE_COLL || !_internal_get_from_bb_collection_alloc (bb, &operand)) { goto err; } xmmsv_coll_add_operand (*coll, operand); xmmsv_coll_unref (operand); } return true; err: if (idlist != NULL) { free (idlist); } xmmsv_coll_unref (*coll); return false; }