\(\newcommand{\W}[1]{ \; #1 \; }\) \(\newcommand{\R}[1]{ {\rm #1} }\) \(\newcommand{\B}[1]{ {\bf #1} }\) \(\newcommand{\D}[2]{ \frac{\partial #1}{\partial #2} }\) \(\newcommand{\DD}[3]{ \frac{\partial^2 #1}{\partial #2 \partial #3} }\) \(\newcommand{\Dpow}[2]{ \frac{\partial^{#1}}{\partial {#2}^{#1}} }\) \(\newcommand{\dpow}[2]{ \frac{ {\rm d}^{#1}}{{\rm d}\, {#2}^{#1}} }\)
qp_interior.hpp¶
View page sourceqp_interior Source Code¶
namespace {
// ------------------------------------------------------------------------
template <class Vector>
double qp_interior_max_abs(const Vector& v)
{ double max_abs = 0.0;
for(size_t j = 0; j < size_t(v.size()); j++)
max_abs = std::max( max_abs, std::fabs(v[j]) );
return max_abs;
}
// ------------------------------------------------------------------------
template <class Vector>
void qp_interior_split(
const Vector& v, Vector& v_x, Vector& v_y, Vector& v_s
)
{ size_t n = v_x.size();
size_t m = v_y.size();
CPPAD_ASSERT_UNKNOWN( size_t(v_s.size()) == m );
CPPAD_ASSERT_UNKNOWN( size_t(v.size()) == n + m + m );
for(size_t i = 0; i < n; i++)
v_x[i] = v[i];
for(size_t i = 0; i < m; i++)
{ v_y[i] = v[n + i];
v_s[i] = v[n + m + i];
}
return;
}
// ------------------------------------------------------------------------
template <class Vector>
void qp_interior_join(
Vector& v, const Vector& v_x, const Vector& v_y, const Vector& v_s
)
{ size_t n = v_x.size();
size_t m = v_y.size();
CPPAD_ASSERT_UNKNOWN( size_t(v_s.size()) == m );
CPPAD_ASSERT_UNKNOWN( size_t(v.size()) == n + m + m );
for(size_t i = 0; i < n; i++)
v[i] = v_x[i];
for(size_t i = 0; i < m; i++)
v[n + i] = v_y[i];
for(size_t i = 0; i < m; i++)
v[n + m + i] = v_s[i];
return;
}
// ------------------------------------------------------------------------
template <class Vector>
Vector qp_interior_F_0(
const Vector& c ,
const Vector& C ,
const Vector& g ,
const Vector& G ,
const Vector& x ,
const Vector& y ,
const Vector& s )
{ size_t n = g.size();
size_t m = c.size();
// compute r_x(x, y, s) = g + G x + y^T C
Vector r_x(n);
for(size_t j = 0; j < n; j++)
{ r_x[j] = g[j];
for(size_t i = 0; i < n; i++)
r_x[j] += G[j * n + i] * x[i];
for(size_t i = 0; i < m; i++)
r_x[j] += y[i] * C[i * n + j];
}
// compute r_y(x, y, s) = C x + c + s
Vector r_y(m);
for(size_t i = 0; i < m; i++)
{ r_y[i] = c[i] + s[i];
for(size_t j = 0; j < n; j++)
r_y[i] += C[i * n + j] * x[j];
}
// compute r_s(x, y, s) = D(s) * D(y) * 1_m - mu * 1_m
// where mu = 0
Vector r_s(m);
for(size_t i = 0; i < m; i++)
r_s[i] = s[i] * y[i];
//
// combine into one vector
Vector F_0(n + m + m);
qp_interior_join(F_0, r_x, r_y, r_s);
//
return F_0;
}
}
//
namespace CppAD { // BEGIN_CPPAD_NAMESPACE
// BEGIN PROTOTYPE
template <class Vector>
bool qp_interior(
size_t level ,
const Vector& c ,
const Vector& C ,
const Vector& g ,
const Vector& G ,
double epsilon ,
size_t maxitr ,
const Vector& xin ,
Vector& xout ,
Vector& yout ,
Vector& sout )
// END PROTOTYPE
{ size_t m = c.size();
size_t n = g.size();
CPPAD_ASSERT_KNOWN(
level <= 1,
"qp_interior: level is greater than one"
);
CPPAD_ASSERT_KNOWN(
size_t(C.size()) == m * n,
"qp_interior: size of C is not m * n"
);
CPPAD_ASSERT_KNOWN(
size_t(G.size()) == n * n,
"qp_interior: size of G is not n * n"
);
if( level > 0 )
{ std::cout << "start qp_interior\n";
CppAD::abs_print_mat("c", m, 1, c);
CppAD::abs_print_mat("C", m, n, C);
CppAD::abs_print_mat("g", n, 1, g);
CppAD::abs_print_mat("G", n, n, G);
CppAD::abs_print_mat("xin", n, 1, xin);
}
//
// compute the maximum absolute element of the problem vectors and matrices
double max_element = 0.0;
for(size_t i = 0; i < size_t(C.size()); i++)
max_element = std::max(max_element , std::fabs(C[i]) );
for(size_t i = 0; i < size_t(c.size()); i++)
max_element = std::max(max_element , std::fabs(c[i]) );
for(size_t i = 0; i < size_t(G.size()); i++)
max_element = std::max(max_element , std::fabs(G[i]) );
for(size_t i = 0; i < size_t(g.size()); i++)
max_element = std::max(max_element , std::fabs(g[i]) );
//
double mu = 1e-1 * max_element;
//
if( max_element == 0.0 )
{ if( level > 0 )
std::cout << "end qp_interior: line_search failed\n";
return false;
}
//
// initialize x, y, s
xout = xin;
for(size_t i = 0; i < m; i++)
{ double sum = c[i];
for(size_t j = 0; j < n; j++)
sum += C[ i * n + j ] * xout[j];
if( sum > 0.0 )
{ if( level > 0 ) std::cout <<
"end qp_interior: xin is not in interior of feasible set\n";
return false;
}
//
sout[i] = std::sqrt(mu);
yout[i] = std::sqrt(mu);
}
// ----------------------------------------------------------------------
// initialie F_0(xout, yout, sout)
Vector F_0 = qp_interior_F_0(c, C, g, G, xout, yout, sout);
double F_max_abs = qp_interior_max_abs( F_0 );
for(size_t itr = 0; itr <= maxitr; itr++)
{
// check for convergence
if( F_max_abs <= epsilon )
{ if( level > 0 )
std::cout << "end qp_interior: ok = true\n";
return true;
}
if( itr == maxitr )
{ if( level > 0 ) std::cout <<
"end qp_interior: max # iterations without convergence\n";
return false;
}
//
// compute F_mu(xout, yout, sout)
Vector F_mu = F_0;
for(size_t i = 0; i < m; i++)
F_mu[n + m + i] -= mu;
//
// r_x, r_y, r_s (xout, yout, sout)
Vector r_x(n), r_y(m), r_s(m);
qp_interior_split(F_mu, r_x, r_y, r_s);
//
// tmp_m = D(s)^{-1} * [ r_s - D(y) r_y ]
Vector tmp_m(m);
for(size_t i = 0; i < m; i++)
tmp_m[i] = ( r_s[i] - yout[i] * r_y[i] ) / sout[i];
//
// right_x = C^T * D(s)^{-1} * [ r_s - D(y) r_y ] - r_x
Vector right_x(n);
for(size_t j = 0; j < n; j++)
{ right_x[j] = 0.0;
for(size_t i = 0; i < m; i++)
right_x[j] += C[ i * n + j ] * tmp_m[i];
right_x[j] -= r_x[j];
}
//
// Left_x = G + C^T * D(y / s) * C
Vector Left_x = G;
for(size_t i = 0; i < n; i++)
{ for(size_t j = 0; j < n; j++)
{ for(size_t k = 0; k < m; k++)
{ double y_s = yout[k] / sout[k];
Left_x[ i * n + j] += C[k * n + j] * y_s * C[k * n + i];
}
}
}
// delta_x
Vector delta_x(n);
double logdet;
LuSolve(n, 1, Left_x, right_x, delta_x, logdet);
//
// C_delta_x = C * delta_x
Vector C_delta_x(m);
for(size_t i = 0; i < m; i++)
{ C_delta_x[i] = 0.0;
for(size_t j = 0; j < n; j++)
C_delta_x[i] += C[ i * n + j ] * delta_x[j];
}
//
// delta_y = D(s)^-1 * [D(y) * r_y - r_s + D(y) * C * delta_x]
Vector delta_y(m);
for(size_t i = 0; i < m; i++)
{ delta_y[i] = yout[i] * r_y[i] - r_s[i] + yout[i] * C_delta_x[i];
delta_y[i] /= sout[i];
}
// delta_s = - r_y - C * delta_x
Vector delta_s(m);
for(size_t i = 0; i < m; i++)
delta_s[i] = - r_y[i] - C_delta_x[i];
//
// delta_xys
Vector delta_xys(n + m + m);
qp_interior_join(delta_xys, delta_x, delta_y, delta_s);
// -------------------------------------------------------------------
//
// The initial derivative in direction Delta_xys is equal to
// the negative of the norm square of F_mu
//
// line search parameter lam
Vector x(n), y(m), s(m);
double lam = 2.0;
bool lam_ok = false;
while( ! lam_ok && lam > 1e-5 )
{ lam = lam / 2.0;
for(size_t j = 0; j < n; j++)
x[j] = xout[j] + lam * delta_xys[j];
lam_ok = true;
for(size_t i = 0; i < m; i++)
{ y[i] = yout[i] + lam * delta_xys[n + i];
s[i] = sout[i] + lam * delta_xys[n + m + i];
lam_ok &= s[i] > 0.0 && y[i] > 0.0;
}
if( lam_ok )
{ Vector F_mu_tmp = qp_interior_F_0(c, C, g, G, x, y, s);
for(size_t i = 0; i < m; i++)
F_mu_tmp[n + m + i] -= mu;
// avoid cancellation roundoff in difference of norm squared
// |v + dv|^2 = v^T * v + 2 * v^T * dv + dv^T * dv
// |v + dv|^2 - |v|^2 = 2 * v^T * dv + dv^T * dv
double F_norm_sq = 0.0;
double diff_norm_sq = 0.0;
for(size_t i = 0; i < n + m + m; i++)
{ double dv = F_mu_tmp[i] - F_mu[i];
F_norm_sq += F_mu[i] * F_mu[i];
diff_norm_sq += 2.0 * F_mu[i] * dv + dv * dv;
}
lam_ok &= diff_norm_sq < - lam * F_norm_sq / 4.0;
}
}
if( ! lam_ok )
{ if( level > 0 )
std::cout << "end qp_interior: line search failed\n";
return false;
}
//
// update current solution
xout = x;
yout = y;
sout = s;
//
// updage F_0
F_0 = qp_interior_F_0(c, C, g, G, xout, yout, sout);
F_max_abs = qp_interior_max_abs( F_0 );
//
// update mu
if( F_max_abs <= 1e1 * mu )
mu = mu / 1e2;
if( level > 0 )
{ std::cout << "itr = " << itr
<< ", mu = " << mu
<< ", lam = " << lam
<< ", F_max_abs = " << F_max_abs << "\n";
abs_print_mat("xout", 1, n, xout);
}
}
if( level > 0 )
std::cout << "end qp_interior: progam error\n";
return false;
}
} // END_CPPAD_NAMESPACE