//----------- Begin of function LinAlg::quadratic_prog ------//
//! Interior Point Quadratic Programming
//! c, Q, A, b, xNames (no global or member variable access)
//!
//! try, by BM:
//! c = { 0,0 }
//! Q = { {2,0}, {0,5} }
//! A = { {5,6} }
//! b = { 10 }
//! xNames={x1,x2}
//!
bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) {
// init local variables
int maxIteration = 20;
const REAL SIGMA = 1/15.0; // 1/10.0;
const REAL R = 9.0/10;
int iter = 0;
int n = v_c.Storage();
int m = v_b.Storage();
maxIteration *= loopCountMultiplier;
#ifdef DEBUG_VC
DEBUG_LOG("----------- quadratic_prog begin -----------");
// print_input(c,Q,A,b,xNames)
if ( n==10 && m==12 ) {
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9), v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12)
);
DEBUG_LOG(s);
/*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,",
v_b(10),v_b(11),v_b(12),
v_b(13),v_b(14),v_b(15),
v_b(16),v_b(17),v_b(18)
);
DEBUG_LOG(s);
sprintf(s, "%f, %f};",
v_b(19),v_b(20));
DEBUG_LOG(s);*/
}
else if ( n==9 && m==20 ) { // stage 1
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20)
);
DEBUG_LOG(s);
}
else if ( n==10 && m==22 ) { // stage 2
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9),v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22)
);
DEBUG_LOG(s);
}
#endif
Vector v_eN(n); v_eN = 1;
Vector v_eM(m); v_eM = 1;
Vector v_x(n); v_x = 1;
Vector v_z(n); v_z = 1;
Vector v_y(m); v_y = 1;
Vector v_w(m); v_w = 1;
// check m_A is nxm, m_Q is nxn
err_when(m_A.Nrows() != m || m_A.Ncols() != n );
err_when(m_Q.Nrows() != n || m_Q.Ncols() != n );
// init for while loop not_converged() checking
//
Vector v_Road = v_b - m_A*(v_x) + v_w;
Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x;
REAL gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
REAL mute = SIGMA*gamma / (n+m);
DiagonalMatrix X(n), Xinv(n);
DiagonalMatrix Y(m), Yinv(m);
DiagonalMatrix Z(n);
DiagonalMatrix W(m);
Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m);
Vector KKFvec(n+m); //, v_tmp1(n), v_tmp2(m);
Vector m_temp(m+n);
Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m);
REAL theeta;
X=0; Xinv=0; Z=0;
Y=0; Yinv=0; W=0; {
// Matrix _t1(n,n), _t2(n,n); _t1=1; _t2=2;
// _t1 = SP(_t1,_t2);
}
Matrix KKFmat(m+n,m+n);
Matrix KKFmatInverse(m+n,m+n);
REAL min=1;
while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) {
iter++;
v_Road = v_b - m_A*v_x + v_w;
v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x;
gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
mute = SIGMA*gamma / (n+m);
X.set_diagonal(v_x);
Y.set_diagonal(v_y);
Z.set_diagonal(v_z);
W.set_diagonal(v_w);
Xinv.set_diagonal(ElmDivide(v_eN, v_x));
Yinv.set_diagonal(ElmDivide(v_eM, v_y));
T1 = -(Xinv * Z + m_Q);
T2 = m_A.t();
T3 = m_A;
T4 = Yinv*W;
//PRT_MAT << "T1: " << T1 << endl;
KKFmat = (T1|T2) & (T3|T4);
KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x)
& (v_b - m_A*v_x + mute*Yinv*v_eM);
Try {
/*{
bool _f;
DiagonalMatrix _dmat(n+m); _dmat=1;
Matrix _inv(n+m,n+m);
SVD(KKFmat, _dmat, _inv);
_dmat=1;
if ( KKFmat*_inv == _dmat )
_f = true;
else
_f = false;
}*/
min = fabs(KKFmat(1,1));
for (int i=2; i<=m+n; i++) {
if ( min > fabs(KKFmat(i,i)) ) // check diagonal element
min = fabs(KKFmat(i,i));
}
KKFmatInverse = KKFmat.i();
/*
if ( KKFmat.LogDeterminant().Value() )
KKFmatInverse = KKFmat.i();
else
break;
*/
m_temp = KKFmatInverse * KKFvec;
v_dx = m_temp.Rows(1,n);
v_dy = m_temp.Rows(n+1,n+m);
v_dz = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx);
v_dw = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy);
//PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl;
//PRT_MAT << "v_x: " << v_x << endl;
//PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl;
theeta =
(R/((
ElmDivide(-v_dx,v_x)
& ElmDivide(-v_dw,v_w)
& ElmDivide(-v_dy,v_y)
& ElmDivide(-v_dz,v_z)
)).MaximumValue()
);
if(theeta>1) // theeta = min(theeta,1)
theeta = 1;
v_x += theeta * v_dx;
v_y += theeta * v_dy;
v_w += theeta * v_dw;
v_z += theeta * v_dz;
#ifdef DEBUG_CONSOLE
cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl;
cout << "Ro:" << v_Road << endl;
PRT_MAT15 << "v_x:" << v_x << endl;
PRT_MAT15 << "v_y:" << v_y << endl;
PRT_MAT15 << "v_w:" << v_w << endl;
PRT_MAT15 << "v_z:" << v_z << endl;
#elif defined(DEBUG_VC)
char s[200];
sprintf(s, "#iter %d: %f, %f, %f",
iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
DEBUG_LOG(s);
if ( n == 9 && false ) {
DEBUG_LOG("x = ");
sprintf(s, " %f, %f, %f, %f, %f, %f, %f, %f, %f",
v_x(1),v_x(2),v_x(3),
v_x(4),v_x(5),v_x(6),
v_x(7),v_x(8),v_x(9));
DEBUG_LOG(s);
}
#endif
}
CatchAll {
#ifdef DEBUG_CONSOLE
cout << Exception::what();
#endif
#ifdef DEBUG_VC
DEBUG_LOG("olinalg: failure in quad_prog");
DEBUG_LOG((char *)Exception::what());
#endif
return false;
}
} // while
v_xNames = v_x;
if ( min < 0.00001 ) { // 1214 || KKFmat.LogDeterminant().Value() == 0 )
//char s[200];
//sprintf(s, "#iter %d: %f, %f, %f",
// iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
//DEBUG_LOG(s);
DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---");
}
else
DEBUG_LOG("----------- quad_prog normal exit -----------");
#ifdef DEBUG_CONSOLE
cout << "#iter: " << iter;
#endif
return true;
}
// Find the minimum fitness value close to a discrete GA gene using
// inverse hessian minimization
double US_MPI_Analysis::minimize_dmga( DGene& dgene, double fitness )
{
DbgLv(1) << my_rank << "dg:IHM:minimize dgene comps" << dgene.components.size() << fitness;
int vsize = nfloatc;
US_Vector vv( vsize ); // Input values
US_Vector uu( vsize ); // Vector of derivatives
US_Vector zz( vsize ); // Vector of normalizing factors
// Create hessian as identity matrix
QVector< QVector< double > > hessian( vsize );
for ( int ii = 0; ii < vsize; ii++ )
{
hessian[ ii ] = QVector< double >( vsize, 0.0 );
hessian[ ii ][ ii ] = 1.0;
}
dgmarker.resize( vsize );
marker_from_dgene( dgmarker, dgene );
// Convert gene to array of normalized doubles and save normalizing factors
for ( int ii = 0; ii < vsize; ii++ )
{
double vval = dgmarker[ ii ];
double vpwr = (double)qFloor( log10( vval ) );
double vnorm = pow( 10.0, -vpwr );
vv.assign( ii, vval * vnorm );
zz.assign( ii, vnorm );
DbgLv(1) << my_rank << "dg:IHM: ii" << ii << "vval vnorm" << vval << vnorm
<< "vpwr" << vpwr << "vvi" << vv[ii];
}
lamm_gsm_df_dmga( vv, uu, zz ); // uu is vector of derivatives
static const double epsilon_f = 1.0e-7;
static const int max_iterations = 20;
int iteration = 0;
double epsilon = epsilon_f * fitness * 4.0;
bool neg_cnstr = ( vv[ 0 ] < 0.1 ); // Negative constraint?
while ( uu.L2norm() >= epsilon_f && iteration < max_iterations )
{
iteration++;
if ( fitness == 0.0 ) break;
US_Vector v_s1 = vv;
double g_s1 = fitness;
double s1 = 0.0;
double s2 = 0.5;
double s3 = 1.0;
DbgLv(1) << my_rank << "dg:IHM: iteration" << iteration << "fitness" << fitness;
// v_s2 = vv - uu * s2
US_Vector v_s2( vsize );
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s2 = get_fitness_v_dmga( v_s2, zz );
DbgLv(1) << my_rank << "dg:IHM: g_s2" << g_s2 << "s2" << s2 << "epsilon" << epsilon;
// Cut down until we have a decrease
while ( s2 > epsilon && g_s2 > g_s1 )
{
s3 = s2;
s2 *= 0.5;
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
DbgLv(1) << my_rank << "dg:IHM: g_s2" << g_s2;
// Test for initial decrease
if ( s2 <= epsilon || ( s3 - s2 ) < epsilon ) break;
US_Vector v_s3( vsize );
// v_s3 = vv - uu * s3
vector_scaled_sum( v_s3, uu, -s3, vv );
if ( neg_cnstr && v_s3[ 0 ] < 0.1 )
{
v_s3.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s3 = get_fitness_v_dmga( v_s3, zz );
int reps = 0;
static const int max_reps = 100;
while ( ( ( s2 - s1 ) > epsilon ) &&
( ( s3 - s2 ) > epsilon ) &&
( reps++ < max_reps ) )
{
double s1_s2 = 1.0 / ( s1 - s2 );
double s1_s3 = 1.0 / ( s1 - s3 );
double s2_s3 = 1.0 / ( s2 - s3 );
double s1_2 = sq( s1 );
double s2_2 = sq( s2 );
double s3_2 = sq( s3 );
double aa = ( ( g_s1 - g_s3 ) * s1_s3 -
( g_s2 - g_s3 ) * s2_s3
) * s1_s2;
double bb = ( g_s3 * ( s2_2 - s1_2 ) +
g_s2 * ( s1_2 - s3_2 ) +
g_s1 * ( s3_2 - s2_2 )
) *
s1_s2 * s1_s3 * s2_s3;
static const double max_a = 1.0e-25;
if ( qAbs( aa ) < max_a )
{
// Restore gene from array of normalized doubles
for ( int ii = 0; ii < vsize; ii++ )
{
dgmarker[ ii ] = vv[ ii ] / zz[ ii ];
}
dgene_from_marker( dgmarker, dgene );
return fitness;
}
double xx = -bb / ( 2.0 * aa );
double prev_g_s2 = g_s2;
if ( xx < s1 )
{
if ( xx < ( s1 + s1 - s2 ) ) // Keep it close
{
xx = s1 + s1 - s2; // xx <- s1 + ds
if ( xx < 0 ) xx = s1 / 2.0;
}
if ( xx < 0 ) // Wrong direction!
{
if ( s1 < 0 ) s1 = 0.0;
xx = 0;
}
// OK, take xx, s1, s2
v_s3 = v_s2;
g_s3 = g_s2; // 3 <- 2
s3 = s2;
v_s2 = v_s1;
g_s2 = g_s1;
s2 = s1; // 2 <- 1
s1 = xx; // 1 <- xx
// v_s1 = vv - uu * s1
vector_scaled_sum( v_s1, uu, -s1, vv );
if ( neg_cnstr && v_s1[ 0 ] < 0.1 )
{
v_s1.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s1 = get_fitness_v_dmga( v_s1, zz );
}
else if ( xx < s2 ) // Take s1, xx, s2
{
v_s3 = v_s2;
g_s3 = g_s2; // 3 <- 2
s3 = s2;
s2 = xx; // 2 <- xx
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
else if ( xx < s3 ) // Take s2, xx, s3
{
v_s1 = v_s2;
g_s1 = g_s2;
s1 = s2; // 2 <- 1
s2 = xx; // 2 <- xx
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
else // xx >= s3
{
if ( xx > ( s3 + s3 - s2 ) ) // if xx > s3 + ds/2
{
// v_s4 = vv - uu * xx
US_Vector v_s4( vsize );
vector_scaled_sum( v_s4, uu, -xx, vv );
if ( neg_cnstr && v_s4[ 0 ] < 0.1 )
{
v_s4.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s4 = get_fitness_v_dmga( v_s4, zz );
if ( g_s4 > g_s2 && g_s4 > g_s3 && g_s4 > g_s1 )
{
xx = s3 + s3 - s2; // xx = s3 + ds/2
}
}
// Take s2, s3, xx
v_s1 = v_s2;
g_s1 = g_s2; // 1 <- 2
s1 = s2;
v_s2 = v_s3;
g_s2 = g_s3;
s2 = s3; // 2 <- 3
s3 = xx; // 3 <- xx
// v_s3 = vv - uu * s3
vector_scaled_sum( v_s3, uu, -s3, vv );
if ( neg_cnstr && v_s3[ 0 ] < 0.1 )
{
v_s3.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s3 = get_fitness_v_dmga( v_s3, zz );
}
if ( qAbs( prev_g_s2 - g_s2 ) < epsilon ) break;
} // end of inner loop
US_Vector v_p( vsize );
if ( g_s2 < g_s3 && g_s2 < g_s1 )
{
v_p = v_s2;
fitness = g_s2;
}
else if ( g_s1 < g_s3 )
{
v_p = v_s1;
fitness = g_s1;
}
else
{
v_p = v_s3;
fitness = g_s3;
}
US_Vector v_g( vsize ); // Vector of derivatives
lamm_gsm_df_dmga( v_p, v_g, zz ); // New gradient in v_g (old in uu)
US_Vector v_dx( vsize );
// v_dx = v_p - vv
vector_scaled_sum( v_dx, vv, -1.0, v_p );
vv = v_p; // vv = v_p
// dgradient v_dg = v_g - uu
US_Vector v_dg( vsize );
vector_scaled_sum( v_dg, uu, -1.0, v_g );
US_Vector v_hdg( vsize );
// v_hdg = hessian * v_dg ( matrix * vector )
for ( int ii = 0; ii < vsize; ii++ )
{
double dotprod = 0.0;
for ( int jj = 0; jj < vsize; jj++ )
dotprod += ( hessian[ ii ][ jj ] * v_dg[ jj ] );
v_hdg.assign( ii, dotprod );
}
double fac = v_dg.dot( v_dx );
double fae = v_dg.dot( v_hdg );
double sumdg = v_dg.dot( v_dg );
double sumxi = v_dx.dot( v_dx );
if ( fac > sqrt( epsilon * sumdg * sumxi ) )
{
fac = 1.0 / fac;
double fad = 1.0 / fae;
for ( int ii = 0; ii < vsize; ii++ )
{
v_dg.assign( ii, fac * v_dx[ ii ] - fad * v_hdg[ ii ] );
}
for ( int ii = 0; ii < vsize; ii++ )
{
for ( int jj = ii; jj < vsize; jj++ )
{
hessian[ ii ][ jj ] +=
fac * v_dx [ ii ] * v_dx [ jj ] -
fad * v_hdg[ ii ] * v_hdg[ jj ] +
fae * v_dg [ ii ] * v_dg [ jj ];
// It's a symmetrical matrix
hessian[ jj ][ ii ] = hessian[ ii ][ jj ];
}
}
}
// uu = hessian * v_g ( matrix * vector )
for ( int ii = 0; ii < vsize; ii++ )
{
double dotprod = 0.0;
for ( int jj = 0; jj < vsize; jj++ )
dotprod += ( hessian[ ii ][ jj ] * v_g[ jj ] );
uu.assign( ii, dotprod );
}
} // end while ( uu.L2norm() > epsilon )
// Restore gene from array of normalized doubles
for ( int ii = 0; ii < vsize; ii++ )
{
dgmarker[ ii ] = vv[ ii ] / zz[ ii ];
DbgLv(1) << my_rank << "dg:IHM: ii" << ii << "vvi zzi dgmi"
<< vv[ii] << zz[ii] << dgmarker[ii];
}
dgene_from_marker( dgmarker, dgene );
DbgLv(1) << my_rank << "dg:IHM: FITNESS" << fitness;
return fitness;
}
