Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
IPM
#include <Eigen/Core>
#include <Eigen/Sparse>
using namespace std;
using namespace Eigen;
void sinitialize(VectorXd &s,double mu,unsigned int sizex,VectorXd &x)
{
for (unsigned int i=0;i<sizex;i++)
if (x[i]!=0)s[i] = mu/x[i];
else s[i]=0.1;
}
void Jinitialize(SparseMatrix<double> &J,unsigned int sizex,unsigned int sizea,MatrixXd &Q,MatrixXd &A)
{
J.reserve(sizex*(sizex+1+2*sizea+2));
VectorXd d(2*sizex+sizea);
for(unsigned int i=0;i<sizex;i++)
{
for(unsigned int j=0;j<sizex;j++)
{
J.insert(i,j)=Q(i,j);//(0,0)
if (i==j)J.insert(i,sizex+sizea+i)= -1;//(0,2)
}
for(unsigned int j=0;j<sizea;j++)
J.insert(i,sizex+j)=-A(j,i);//(0,1)
}
for (unsigned int i=0;i<sizea;i++)
for (unsigned int j=0;j<sizex;j++)
J.insert(sizex+i,j)=A(i,j);//(1,0)
}
//min{1/2*x'*Q*x+q'*x} subject to A*x=a
void InteriorPointMethod(MatrixXd &Q, VectorXd &q,VectorXd &x,MatrixXd &A,VectorXd &a)
{
double mu = 0.1,alpha = 1;
const double eta = 0.5,epsilon = 0.001,sig_min = 0.01,sig_max = 0.75;
unsigned int sizex = x.size(),sizea = a.size();
VectorXd lmd = VectorXd::Zero(sizea);
//set si subject to xisi=mu
VectorXd s(sizex);
sinitialize(s,mu,sizex,x);
// Q -A' -I
//J (x,l,s)= A O O
// Sk O Xk
SparseMatrix<double,ColMajor,int> J(2*sizex+sizea,2*sizex+sizea);
Jinitialize(J,sizex,sizea,Q,A);
VectorXd d(2*sizex+sizea),F(2*sizex+sizea);
cerr << "[initial value]\n"<<x.head(1.0*sizex/2).transpose()<<endl;
cerr << "Solving QP";
for (int k=0;mu>epsilon;k++)
{
cerr <<".";
cerr.flush();
double sigma = (sig_min+sig_max)/2;//5
for(unsigned int i=0;i<sizex;i++)
{
J.coeffRef(sizex+sizea+i,i)=s[i];//update Sk
J.coeffRef(sizex+sizea+i,sizex+sizea+i)=x[i];//update Xk
}
F <<
Q * x + q -A.transpose()*lmd-s,
A*x-a,
(VectorXd)(x.array()*s.array())-VectorXd::Ones(sizex)*mu*sigma;
F=-F;
// Fx
//F= Fl
// Fs
J.makeCompressed();
SparseQR<SparseMatrix<double>,COLAMDOrdering<int> > solver;
solver.analyzePattern(J);
solver.factorize(J);
d = solver.solve(F);//determine dk =(Δxk,Δλk,Δsk)
alpha=1;
for (unsigned int i=0;i<sizex;i++)
if(d[i]<0&&x[i]+alpha*d[i]<0)
alpha = -x[i]/d[i];
for (unsigned int i=0;i<sizea;i++)
if (d[i+sizex+sizea]<0&&s[i]+alpha*d[i+sizex+sizea]<0)
alpha = -s[i]/d[i+sizex+sizea];
alpha = eta*alpha < 1 ? eta*alpha : 1;
mu = (x.transpose()*s)[0]/sizex;
for (unsigned int i=0;i<sizex;i++)x[i]+=alpha*d[i];
for (unsigned int i=0;i<sizea;i++)lmd[i]+=alpha*d[sizex+i];
for (unsigned int i=0;i<sizex;i++)s[i]+=alpha*d[sizex+sizea+i];
}
cerr << endl;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment