// 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;
}
