/**
     * @brief A wrapper around private functions, which update Cholesky factor and 
     *  resolve the system.
     *
     * @param[in] ppar   parameters.
     * @param[in] active_set a vector of active constraints.
     * @param[in] x     initial guess.
     * @param[out] dx   feasible descent direction, must be allocated.
     */
    void chol_solve::up_resolve(
            const AS::problem_parameters& ppar, 
            const vector <AS::constraint>& active_set, 
            const double *x, 
            double *dx)
    {
        int ic_num = active_set.size()-1;
        constraint c = active_set.back();

        update (ppar, c, ic_num);
        update_z (ppar, c, ic_num, x);
        resolve (ppar, active_set, x, dx);
    }
void update_channels(void) 
{ 

  {
  if ((int )b0_req_up == 1) {
    {
    update_b0();
    }
  } else {

  }
  if ((int )b1_req_up == 1) {
    {
    update_b1();
    }
  } else {

  }
  if ((int )d0_req_up == 1) {
    {
    update_d0();
    }
  } else {

  }
  if ((int )d1_req_up == 1) {
    {
    update_d1();
    }
  } else {

  }
  if ((int )z_req_up == 1) {
    {
    update_z();
    }
  } else {

  }

  return;
}
}
Exemple #3
0
double QuadProg::solve_quadprog(Matrix<double>& G, Vector<double>& g0, 
                      const Matrix<double>& CE, const Vector<double>& ce0,  
                      const Matrix<double>& CI, const Vector<double>& ci0, 
                      Vector<double>& x)
{
  std::ostringstream msg;
  {
    //Ensure that the dimensions of the matrices and vectors can be
    //safely converted from unsigned int into to int without overflow.
    unsigned mx = std::numeric_limits<int>::max();
    if(G.ncols() >= mx || G.nrows() >= mx || 
       CE.nrows() >= mx || CE.ncols() >= mx ||
       CI.nrows() >= mx || CI.ncols() >= mx || 
       ci0.size() >= mx || ce0.size() >= mx || g0.size() >= mx){
      msg << "The dimensions of one of the input matrices or vectors were "
	  << "too large." << std::endl
	  << "The maximum allowable size for inputs to solve_quadprog is:"
	  << mx << std::endl;
      throw std::logic_error(msg.str());
    }
  }
  int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
  if ((int)G.nrows() != n)
  {
    msg << "The matrix G is not a square matrix (" << G.nrows() << " x " 
	<< G.ncols() << ")";
    throw std::logic_error(msg.str());
  }
  if ((int)CE.nrows() != n)
  {
    msg << "The matrix CE is incompatible (incorrect number of rows " 
	<< CE.nrows() << " , expecting " << n << ")";
    throw std::logic_error(msg.str());
  }
  if ((int)ce0.size() != p)
  {
    msg << "The vector ce0 is incompatible (incorrect dimension " 
	<< ce0.size() << ", expecting " << p << ")";
    throw std::logic_error(msg.str());
  }
  if ((int)CI.nrows() != n)
  {
    msg << "The matrix CI is incompatible (incorrect number of rows " 
	<< CI.nrows() << " , expecting " << n << ")";
    throw std::logic_error(msg.str());
  }
  if ((int)ci0.size() != m)
  {
    msg << "The vector ci0 is incompatible (incorrect dimension " 
	<< ci0.size() << ", expecting " << m << ")";
    throw std::logic_error(msg.str());
  }
  x.resize(n);
  register int i, j, k, l; /* indices */
  int ip; // this is the index of the constraint to be added to the active set
  Matrix<double> R(n, n), J(n, n);
  Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
  double f_value, psi, c1, c2, sum, ss, R_norm;
  double inf;
  if (std::numeric_limits<double>::has_infinity)
    inf = std::numeric_limits<double>::infinity();
  else
    inf = 1.0E300;
  double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1 
    * and the full step length t2 */
  Vector<int> A(m + p), A_old(m + p), iai(m + p);
  int q, iq, iter = 0;
  Vector<bool> iaexcl(m + p);
	
  /* p is the number of equality constraints */
  /* m is the number of inequality constraints */
  q = 0;  /* size of the active set A (containing the indices of the active constraints) */
#ifdef TRACE_SOLVER
  std::cout << std::endl << "Starting solve_quadprog" << std::endl;
  print_matrix("G", G);
  print_vector("g0", g0);
  print_matrix("CE", CE);
  print_vector("ce0", ce0);
  print_matrix("CI", CI);
  print_vector("ci0", ci0);
#endif  
  
  /*
   * Preprocessing phase
   */
	
  /* compute the trace of the original matrix G */
  c1 = 0.0;
  for (i = 0; i < n; i++)
  {
    c1 += G[i][i];
  }
  /* decompose the matrix G in the form L^T L */
  cholesky_decomposition(G);
#ifdef TRACE_SOLVER
  print_matrix("G", G);
#endif
  /* initialize the matrix R */
  for (i = 0; i < n; i++)
  {
    d[i] = 0.0;
    for (j = 0; j < n; j++)
      R[i][j] = 0.0;
  }
  R_norm = 1.0; /* this variable will hold the norm of the matrix R */
  
  /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  c2 = 0.0;
  for (i = 0; i < n; i++) 
  {
    d[i] = 1.0;
    forward_elimination(G, z, d);
    for (j = 0; j < n; j++)
      J[i][j] = z[j];
    c2 += z[i];
    d[i] = 0.0;
  }
#ifdef TRACE_SOLVER
  print_matrix("J", J);
#endif
  
  /* c1 * c2 is an estimate for cond(G) */
  
  /* 
    * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
   * this is a feasible point in the dual space
   * x = G^-1 * g0
   */
  cholesky_solve(G, x, g0);
  for (i = 0; i < n; i++)
    x[i] = -x[i];
  /* and compute the current solution value */ 
  f_value = 0.5 * scalar_product(g0, x);
#ifdef TRACE_SOLVER
  std::cout << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x);
#endif
  
  /* Add equality constraints to the working set A */
  iq = 0;
  for (i = 0; i < p; i++)
  {
    for (j = 0; j < n; j++)
      np[j] = CE[j][i];
    compute_d(d, J, np);
    update_z(z, J, d, iq);
    update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
    print_matrix("R", R, n, iq);
    print_vector("z", z);
    print_vector("r", r, iq);
    print_vector("d", d);
#endif
    
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint 
      becomes feasible */
    t2 = 0.0;
    if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
      t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
    
    /* set x = x + t2 * z */
    for (k = 0; k < n; k++)
      x[k] += t2 * z[k];
    
    /* set u = u+ */
    u[iq] = t2;
    for (k = 0; k < iq; k++)
      u[k] -= t2 * r[k];
    
    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
    A[i] = -i - 1;
    
    if (!add_constraint(R, J, d, iq, R_norm))
    {	  
      // Equality constraints are linearly dependent
      throw std::runtime_error("Constraints are linearly dependent");
      return f_value;
    }
  }
  
  /* set iai = K \ A */
  for (i = 0; i < m; i++)
    iai[i] = i;
  
l1:	iter++;
#ifdef TRACE_SOLVER
  print_vector("x", x);
#endif
  /* step 1: choose a violated constraint */
  for (i = p; i < iq; i++)
  {
    ip = A[i];
    iai[ip] = -1;
  }
	
  /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
  ss = 0.0;
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
  ip = 0; /* ip will be the index of the chosen violated constraint */
  for (i = 0; i < m; i++)
  {
    iaexcl[i] = true;
    sum = 0.0;
    for (j = 0; j < n; j++)
      sum += CI[j][i] * x[j];
    sum += ci0[i];
    s[i] = sum;
    psi += std::min(0.0, sum);
  }
#ifdef TRACE_SOLVER
  print_vector("s", s, m);
#endif
  
  
  if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
  {
    /* numerically there are not infeasibilities anymore */
    q = iq;
    
    return f_value;
  }
  
  /* save old values for u and A */
  for (i = 0; i < iq; i++)
  {
    u_old[i] = u[i];
    A_old[i] = A[i];
  }
  /* and for x */
  for (i = 0; i < n; i++)
    x_old[i] = x[i];
  
l2: /* Step 2: check for feasibility and determine a new S-pair */
    for (i = 0; i < m; i++)
    {
      if (s[i] < ss && iai[i] != -1 && iaexcl[i])
      {
        ss = s[i];
        ip = i;
      }
    }
  if (ss >= 0.0)
  {
    q = iq;
    
    return f_value;
  }
  
  /* set np = n[ip] */
  for (i = 0; i < n; i++)
    np[i] = CI[i][ip];
  /* set u = [u 0]^T */
  u[iq] = 0.0;
  /* add ip to the active set A */
  A[iq] = ip;
  
#ifdef TRACE_SOLVER
  std::cout << "Trying with constraint " << ip << std::endl;
  print_vector("np", np);
#endif
  
l2a:/* Step 2a: determine step direction */
    /* compute z = H np: the step direction in the primal space (through J, see the paper) */
    compute_d(d, J, np);
  update_z(z, J, d, iq);
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
  update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
  std::cout << "Step direction z" << std::endl;
  print_vector("z", z);
  print_vector("r", r, iq + 1);
  print_vector("u", u, iq + 1);
  print_vector("d", d);
  print_vector("A", A, iq + 1);
#endif
  
  /* Step 2b: compute step length */
  l = 0;
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
  t1 = inf; /* +inf */
  /* find the index l s.t. it reaches the minimum of u+[x] / r */
  for (k = p; k < iq; k++)
  {
    if (r[k] > 0.0)
    {
      if (u[k] / r[k] < t1)
	    {
	      t1 = u[k] / r[k];
	      l = A[k];
	    }
    }
  }
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
  if (fabs(scalar_product(z, z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
    t2 = -s[ip] / scalar_product(z, np);
  else
    t2 = inf; /* +inf */
  
  /* the step is chosen as the minimum of t1 and t2 */
  t = std::min(t1, t2);
#ifdef TRACE_SOLVER
  std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
  
  /* Step 2c: determine new S-pair and take step: */
  
  /* case (i): no step in primal or dual space */
  if (t >= inf)
  {
    /* QPP is infeasible */
    // FIXME: unbounded to raise
    q = iq;
    return inf;
  }
  /* case (ii): step in dual space */
  if (t2 >= inf)
  {
    /* set u = u +  t * [-r 1] and drop constraint l from the active set A */
    for (k = 0; k < iq; k++)
      u[k] -= t * r[k];
    u[iq] += t;
    iai[l] = l;
    delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
    std::cout << " in dual space: " 
      << f_value << std::endl;
    print_vector("x", x);
    print_vector("z", z);
    print_vector("A", A, iq + 1);
#endif
    goto l2a;
  }
  
  /* case (iii): step in primal and dual space */
  
  /* set x = x + t * z */
  for (k = 0; k < n; k++)
    x[k] += t * z[k];
  /* update the solution value */
  f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
  /* u = u + t * [-r 1] */
  for (k = 0; k < iq; k++)
    u[k] -= t * r[k];
  u[iq] += t;
#ifdef TRACE_SOLVER
  std::cout << " in both spaces: " 
    << f_value << std::endl;
  print_vector("x", x);
  print_vector("u", u, iq + 1);
  print_vector("r", r, iq + 1);
  print_vector("A", A, iq + 1);
#endif
  
  if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
  {
#ifdef TRACE_SOLVER
    std::cout << "Full step has taken " << t << std::endl;
    print_vector("x", x);
#endif
    /* full step has taken */
    /* add constraint ip to the active set*/
    if (!add_constraint(R, J, d, iq, R_norm))
    {
      iaexcl[ip] = false;
      delete_constraint(R, J, A, u, n, p, iq, ip);
#ifdef TRACE_SOLVER
      print_matrix("R", R);
      print_vector("A", A, iq);
			print_vector("iai", iai);
#endif
      for (i = 0; i < m; i++)
        iai[i] = i;
      for (i = p; i < iq; i++)
	    {
	      A[i] = A_old[i];
	      u[i] = u_old[i];
				iai[A[i]] = -1;
	    }
      for (i = 0; i < n; i++)
        x[i] = x_old[i];
      goto l2; /* go to step 2 */
    }    
    else
      iai[ip] = -1;
#ifdef TRACE_SOLVER
    print_matrix("R", R);
    print_vector("A", A, iq);
		print_vector("iai", iai);
#endif
    goto l1;
  }
  
  /* a patial step has taken */
#ifdef TRACE_SOLVER
  std::cout << "Partial step has taken " << t << std::endl;
  print_vector("x", x);
#endif
  /* drop constraint l */
  iai[l] = l;
  delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
  print_matrix("R", R);
  print_vector("A", A, iq);
#endif
  
  /* update s[ip] = CI * x + ci0 */
  sum = 0.0;
  for (k = 0; k < n; k++)
    sum += CI[k][ip] * x[k];
  s[ip] = sum + ci0[ip];
  
#ifdef TRACE_SOLVER
  print_vector("s", s, m);
#endif
  goto l2a;
}
Exemple #4
0
inline double solve_quadprog(MatrixXd & G,  VectorXd & g0,  
                      const MatrixXd & CE, const VectorXd & ce0,  
                      const MatrixXd & CI, const VectorXd & ci0, 
                      VectorXd& x)
{
  int i, j, k, l; /* indices */
  int ip, me, mi;
  int n=g0.size();  int p=ce0.size();  int m=ci0.size();  
  MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
  
  LLT<MatrixXd,Lower> chol(G.cols());
 
  VectorXd s(m+p), z(n), r(m + p), d(n),  np(n), u(m + p);
  VectorXd x_old(n), u_old(m + p);
  double f_value, psi, c1, c2, sum, ss, R_norm;
  const double inf = std::numeric_limits<double>::infinity();
  double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1 
    * and the full step length t2 */
  VectorXi A(m + p), A_old(m + p), iai(m + p);
  int q;
  int iq, iter = 0;
  bool iaexcl[m + p];
 	
  me = p; /* number of equality constraints */
  mi = m; /* number of inequality constraints */
  q = 0;  /* size of the active set A (containing the indices of the active constraints) */
  
  /*
   * Preprocessing phase
   */
	
  /* compute the trace of the original matrix G */
  c1 = G.trace();
	
	/* decompose the matrix G in the form LL^T */
  chol.compute(G);
 
  /* initialize the matrix R */
  d.setZero();
  R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */
  
	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
  J.setIdentity();
  J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
 print_matrix("J", J, n);
#endif
  
	/* c1 * c2 is an estimate for cond(G) */
  
	/* 
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
  x = chol.solve(g0);
  x = -x;
	/* and compute the current solution value */ 
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x, n);
#endif
  
	/* Add equality constraints to the working set A */
  iq = 0;
	for (i = 0; i < me; i++)
	{
    np = CE.col(i);
    compute_d(d, J, np);
		update_z(z, J, d,  iq);
		update_r(R, r, d,  iq);
#ifdef TRACE_SOLVER
		print_matrix("R", R, iq);
		print_vector("z", z, n);
		print_vector("r", r, iq);
		print_vector("d", d, n);
#endif
    
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint 
      becomes feasible */
    t2 = 0.0;
    if (internal::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
    
    x += t2 * z;

    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);
    
    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * z.dot(np);
    A(i) = -i - 1;
    
    if (!add_constraint(R, J, d, iq, R_norm))
    {
      // FIXME: it should raise an error
      // Equality constraints are linearly dependent
      return f_value;
    }
  }
  
	/* set iai = K \ A */
	for (i = 0; i < mi; i++)
		iai(i) = i;
  
l1:	iter++;
#ifdef TRACE_SOLVER
  print_vector("x", x, n);
#endif
  /* step 1: choose a violated constraint */
	for (i = me; i < iq; i++)
	{
	  ip = A(i);
		iai(ip) = -1;
	}
	
	/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
	ss = 0.0;
	psi = 0.0; /* this value will contain the sum of all infeasibilities */
	ip = 0; /* ip will be the index of the chosen violated constraint */
	for (i = 0; i < mi; i++)
	{
		iaexcl[i] = true;
		sum = CI.col(i).dot(x) + ci0(i);
		s(i) = sum;
		psi += std::min(0.0, sum);
	}
#ifdef TRACE_SOLVER
  print_vector("s", s, mi);
#endif

    
	if (internal::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
	{
    /* numerically there are not infeasibilities anymore */
    q = iq;
		return f_value;
  }
    
  /* save old values for u, x and A */
   u_old.head(iq) = u.head(iq);
   A_old.head(iq) = A.head(iq);
   x_old = x;
    
l2: /* Step 2: check for feasibility and determine a new S-pair */
	for (i = 0; i < mi; i++)
	{
		if (s(i) < ss && iai(i) != -1 && iaexcl[i])
		{
			ss = s(i);
			ip = i;
		}
	}
  if (ss >= 0.0)
  {
    q = iq;
    return f_value;
  }
    
  /* set np = n(ip) */
  np = CI.col(ip);
  /* set u = (u 0)^T */
  u(iq) = 0.0;
  /* add ip to the active set A */
  A(iq) = ip;

#ifdef TRACE_SOLVER
	std::cerr << "Trying with constraint " << ip << std::endl;
	print_vector("np", np, n);
#endif
    
l2a:/* Step 2a: determine step direction */
  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
  compute_d(d, J, np);
  update_z(z, J, d, iq);
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
  update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
  std::cerr << "Step direction z" << std::endl;
		print_vector("z", z, n);
		print_vector("r", r, iq + 1);
    print_vector("u", u, iq + 1);
    print_vector("d", d, n);
    print_ivector("A", A, iq + 1);
#endif
    
  /* Step 2b: compute step length */
  l = 0;
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
  t1 = inf; /* +inf */
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
  for (k = me; k < iq; k++)
  {
    double tmp;
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
    {
      t1 = tmp;
      l = A(k);
    }
  }
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
  if (internal::abs(z.dot(z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
    t2 = -s(ip) / z.dot(np);
  else
    t2 = inf; /* +inf */

  /* the step is chosen as the minimum of t1 and t2 */
  t = std::min(t1, t2);
#ifdef TRACE_SOLVER
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
  
  /* Step 2c: determine new S-pair and take step: */
  
  /* case (i): no step in primal or dual space */
  if (t >= inf)
  {
    /* QPP is infeasible */
    // FIXME: unbounded to raise
    q = iq;
    return inf;
  }
  /* case (ii): step in dual space */
  if (t2 >= inf)
  {
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
    u.head(iq) -= t * r.head(iq);
    u(iq) += t;
    iai(l) = l;
    delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
    std::cerr << " in dual space: " 
      << f_value << std::endl;
    print_vector("x", x, n);
    print_vector("z", z, n);
		print_ivector("A", A, iq + 1);
#endif
    goto l2a;
  }
  
  /* case (iii): step in primal and dual space */
  
  x += t * z;
  /* update the solution value */
  f_value += t * z.dot(np) * (0.5 * t + u(iq));
  
  u.head(iq) -= t * r.head(iq);
  u(iq) += t;
#ifdef TRACE_SOLVER
  std::cerr << " in both spaces: " 
    << f_value << std::endl;
	print_vector("x", x, n);
	print_vector("u", u, iq + 1);
	print_vector("r", r, iq + 1);
	print_ivector("A", A, iq + 1);
#endif
  
  if (t == t2)
  {
#ifdef TRACE_SOLVER
    std::cerr << "Full step has taken " << t << std::endl;
    print_vector("x", x, n);
#endif
    /* full step has taken */
    /* add constraint ip to the active set*/
		if (!add_constraint(R, J, d, iq, R_norm))
		{
			iaexcl[ip] = false;
			delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
      print_matrix("R", R, n);
      print_ivector("A", A, iq);
#endif
			for (i = 0; i < m; i++)
				iai(i) = i;
			for (i = 0; i < iq; i++)
			{
				A(i) = A_old(i);
				iai(A(i)) = -1;
				u(i) = u_old(i);
			}
			x = x_old;
      goto l2; /* go to step 2 */
		}    
    else
      iai(ip) = -1;
#ifdef TRACE_SOLVER
    print_matrix("R", R, n);
    print_ivector("A", A, iq);
#endif
    goto l1;
  }
  
  /* a patial step has taken */
#ifdef TRACE_SOLVER
  std::cerr << "Partial step has taken " << t << std::endl;
  print_vector("x", x, n);
#endif
  /* drop constraint l */
	iai(l) = l;
	delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
  print_matrix("R", R, n);
  print_ivector("A", A, iq);
#endif
  
  s(ip) = CI.col(ip).dot(x) + ci0(ip);

#ifdef TRACE_SOLVER
  print_vector("s", s, mi);
#endif
  goto l2a;
}