void SpinAdapted::diagonalise(Matrix& sym, DiagonalMatrix& d, Matrix& vec)
{
int nrows = sym.Nrows();
int ncols = sym.Ncols();
assert(nrows == ncols);
d.ReSize(nrows);
vec.ReSize(nrows, nrows);
Matrix workmat;
workmat = sym;
vector<double> workquery(1);
int info = 0;
double* dptr = d.Store();
int query = -1;
DSYEV('V', 'L', nrows, workmat.Store(), nrows, dptr, &(workquery[0]), query, info); // do query to find best size
int optlength = static_cast<int>(workquery[0]);
vector<double> workspace(optlength);
DSYEV('V', 'U', nrows, workmat.Store(), nrows, dptr, &(workspace[0]), optlength, info); // do query to find best size
if (info > 0)
{
pout << "failed to converge " << endl;
abort();
}
for (int i = 0; i < nrows; ++i)
for (int j = 0; j < ncols; ++j)
vec(j+1,i+1) = workmat(i+1,j+1);
}
void SpinAdapted::svd(Matrix& M, DiagonalMatrix& d, Matrix& U, Matrix& V)
{
int nrows = M.Nrows();
int ncols = M.Ncols();
assert(nrows >= ncols);
int minmn = min(nrows, ncols);
int maxmn = max(nrows, ncols);
int eigenrows = min(minmn, minmn);
d.ReSize(minmn);
Matrix Ut;
Ut.ReSize(nrows, nrows);
V.ReSize(ncols, ncols);
int lwork = maxmn * maxmn + 100;
double* workspace = new double[lwork];
// first transpose matrix
Matrix Mt;
Mt = M.t();
int info = 0;
DGESVD('A', 'A', nrows, ncols, Mt.Store(), nrows, d.Store(),
Ut.Store(), nrows, V.Store(), ncols, workspace, lwork, info);
U.ReSize(nrows, ncols);
SpinAdapted::Clear(U);
for (int i = 0; i < nrows; ++i)
for (int j = 0; j < ncols; ++j)
U(i+1,j+1) = Ut(j+1,i+1);
delete[] workspace;
}
/*!
@fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
const Real eps_, const Real lambda_max_, const Real dt);
@brief Constructor.
*/
Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
const Real eps_, const Real lambda_max_, const Real dt_):
dt(dt_),
eps(eps_),
lambda_max(lambda_max_),
mrobot(mrobot_)
{
robot_type = CLICK_mDH;
// Initialize with same joint position (and rates) has the robot.
q = mrobot.get_q();
qp = mrobot.get_qp();
qp_prev = qp;
Kpep = ColumnVector(3); Kpep = 0;
Koe0Quat = ColumnVector(3); Koe0Quat = 0;
v = ColumnVector(6); v = 0;
if(Kp_.Nrows()==3)
Kp = Kp_;
else
{
Kp = DiagonalMatrix(3); Kp = 0.0;
cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
}
if(Ko_.Nrows()==3)
Ko = Ko_;
else
{
Ko = DiagonalMatrix(3); Ko = 0.0;
cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
}
}
void test1(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
cout << "\n\nTest 1 - traditional, bad\n";
// traditional sum of squares and products method of calculation
// but not adjusting means; maybe subject to round-off error
// make matrix of predictor values with 1s into col 1 of matrix
int npred1 = npred+1; // number of cols including col of ones.
Matrix X(nobs,npred1);
X.Column(1) = 1.0;
// load x1 and x2 into X
// [use << rather than = when loading arrays]
X.Column(2) << x1; X.Column(3) << x2;
// vector of Y values
ColumnVector Y(nobs); Y << y;
// form sum of squares and product matrix
// [use << rather than = for copying Matrix into SymmetricMatrix]
SymmetricMatrix SSQ; SSQ << X.t() * X;
// calculate estimate
// [bracket last two terms to force this multiplication first]
// [ .i() means inverse, but inverse is not explicity calculated]
ColumnVector A = SSQ.i() * (X.t() * Y);
// Get variances of estimates from diagonal elements of inverse of SSQ
// get inverse of SSQ - we need it for finding D
DiagonalMatrix D; D << SSQ.i();
ColumnVector V = D.AsColumn();
// Calculate fitted values and residuals
ColumnVector Fitted = X * A;
ColumnVector Residual = Y - Fitted;
Real ResVar = Residual.SumSquare() / (nobs-npred1);
// Get diagonals of Hat matrix (an expensive way of doing this)
DiagonalMatrix Hat; Hat << X * (X.t() * X).i() * X.t();
// print out answers
cout << "\nEstimates and their standard errors\n\n";
// make vector of standard errors
ColumnVector SE(npred1);
for (int i=1; i<=npred1; i++) SE(i) = sqrt(V(i)*ResVar);
// use concatenation function to form matrix and use matrix print
// to get two columns
cout << setw(11) << setprecision(5) << (A | SE) << endl;
cout << "\nObservations, fitted value, residual value, hat value\n";
// use concatenation again; select only columns 2 to 3 of X
cout << setw(9) << setprecision(3) <<
(X.Columns(2,3) | Y | Fitted | Residual | Hat.AsColumn());
cout << "\n\n";
}
void test3(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
cout << "\n\nTest 3 - Cholesky\n";
// traditional sum of squares and products method of calculation
// with subtraction of means - using Cholesky decomposition
Matrix X(nobs,npred);
X.Column(1) << x1; X.Column(2) << x2;
ColumnVector Y(nobs); Y << y;
ColumnVector Ones(nobs); Ones = 1.0;
RowVector M = Ones.t() * X / nobs;
Matrix XC(nobs,npred);
XC = X - Ones * M;
ColumnVector YC(nobs);
Real m = Sum(Y) / nobs; YC = Y - Ones * m;
SymmetricMatrix SSQ; SSQ << XC.t() * XC;
// Cholesky decomposition of SSQ
LowerTriangularMatrix L = Cholesky(SSQ);
// calculate estimate
ColumnVector A = L.t().i() * (L.i() * (XC.t() * YC));
// calculate estimate of constant term
Real a = m - (M * A).AsScalar();
// Get variances of estimates from diagonal elements of invoice of SSQ
DiagonalMatrix D; D << L.t().i() * L.i();
ColumnVector V = D.AsColumn();
Real v = 1.0/nobs + (L.i() * M.t()).SumSquare();
// Calculate fitted values and residuals
int npred1 = npred+1;
ColumnVector Fitted = X * A + a;
ColumnVector Residual = Y - Fitted;
Real ResVar = Residual.SumSquare() / (nobs-npred1);
// Get diagonals of Hat matrix (an expensive way of doing this)
Matrix X1(nobs,npred1); X1.Column(1)<<Ones; X1.Columns(2,npred1)<<X;
DiagonalMatrix Hat; Hat << X1 * (X1.t() * X1).i() * X1.t();
// print out answers
cout << "\nEstimates and their standard errors\n\n";
cout.setf(ios::fixed, ios::floatfield);
cout << setw(11) << setprecision(5) << a << " ";
cout << setw(11) << setprecision(5) << sqrt(v*ResVar) << endl;
ColumnVector SE(npred);
for (int i=1; i<=npred; i++) SE(i) = sqrt(V(i)*ResVar);
cout << setw(11) << setprecision(5) << (A | SE) << endl;
cout << "\nObservations, fitted value, residual value, hat value\n";
cout << setw(9) << setprecision(3) <<
(X | Y | Fitted | Residual | Hat.AsColumn());
cout << "\n\n";
}
Matrix BaseController::pseudoInverse(const Matrix M)
{
Matrix result;
//int rows = this->rows();
//int cols = this->columns();
// calculate SVD decomposition
Matrix U,V;
DiagonalMatrix D;
NEWMAT::SVD(M,D,U,V, true, true);
Matrix Dinv = D.i();
result = V * Dinv * U.t();
return result;
}
void SpinAdapted::operatorfunctions::TensorProduct (const SpinBlock *ablock, const Baseoperator<Matrix>& a, const Baseoperator<Matrix>& b, const SpinBlock* cblock, const StateInfo* cstateinfo, DiagonalMatrix& cDiagonal, double scale)
{
if (fabs(scale) < TINY) return;
const int aSz = a.nrows();
const int bSz = b.nrows();
const char conjC = (cblock->get_leftBlock() == ablock) ? 'n' : 't';
const SpinBlock* bblock = (cblock->get_leftBlock() == ablock) ? cblock->get_rightBlock() : cblock->get_leftBlock();
const StateInfo& s = cblock->get_stateInfo();
const StateInfo* lS = s.leftStateInfo, *rS = s.rightStateInfo;
for (int aQ = 0; aQ < aSz; ++aQ)
if (a.allowed(aQ, aQ))
for (int bQ = 0; bQ < bSz; ++bQ)
if (b.allowed(bQ, bQ))
if (s.allowedQuanta (aQ, bQ, conjC))
{
int cQ = s.quantaMap (aQ, bQ, conjC)[0];
Real scaleA = scale;
Real scaleB = 1;
if (conjC == 'n')
{
scaleB *= dmrginp.get_ninej()(lS->quanta[aQ].get_s().getirrep() , rS->quanta[bQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep(),
a.get_spin().getirrep(), b.get_spin().getirrep(), 0,
lS->quanta[aQ].get_s().getirrep() , rS->quanta[bQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep());
scaleB *= Symmetry::spatial_ninej(lS->quanta[aQ].get_symm().getirrep() , rS->quanta[bQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep(),
a.get_symm().getirrep(), b.get_symm().getirrep(), 0,
lS->quanta[aQ].get_symm().getirrep() , rS->quanta[bQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep());
if (b.get_fermion() && IsFermion (lS->quanta [aQ])) scaleB *= -1.0;
for (int aQState = 0; aQState < lS->quantaStates[aQ] ; aQState++)
MatrixDiagonalScale(a.operator_element(aQ, aQ)(aQState+1, aQState+1)*scaleA*scaleB, b.operator_element(bQ, bQ),
cDiagonal.Store()+s.unBlockedIndex[cQ]+aQState*rS->quantaStates[bQ]);
}
else
{
scaleB *= dmrginp.get_ninej()(lS->quanta[bQ].get_s().getirrep() , rS->quanta[aQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep(),
b.get_spin().getirrep(), a.get_spin().getirrep(), 0,
lS->quanta[bQ].get_s().getirrep() , rS->quanta[aQ].get_s().getirrep(), cstateinfo->quanta[cQ].get_s().getirrep());
scaleB *= Symmetry::spatial_ninej(lS->quanta[bQ].get_symm().getirrep() , rS->quanta[aQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep(),
b.get_symm().getirrep(), a.get_symm().getirrep(), 0,
lS->quanta[bQ].get_symm().getirrep() , rS->quanta[aQ].get_symm().getirrep(), cstateinfo->quanta[cQ].get_symm().getirrep());
if (a.get_fermion()&& IsFermion(lS->quanta[bQ])) scaleB *= -1.0;
for (int bQState = 0; bQState < lS->quantaStates[bQ] ; bQState++)
MatrixDiagonalScale(b.operator_element(bQ, bQ)(bQState+1, bQState+1)*scaleA*scaleB, a.operator_element(aQ, aQ),
cDiagonal.Store()+s.unBlockedIndex[cQ]+bQState*rS->quantaStates[aQ]);
}
}
}
void Foam::multiply
(
scalarRectangularMatrix& ans, // value changed in return
const scalarRectangularMatrix& A,
const DiagonalMatrix<scalar>& B,
const scalarRectangularMatrix& C
)
{
if (A.m() != B.size())
{
FatalErrorIn
(
"multiply("
"const scalarRectangularMatrix& A, "
"const DiagonalMatrix<scalar>& B, "
"const scalarRectangularMatrix& C, "
"scalarRectangularMatrix& answer)"
) << "A and B must have identical inner dimensions but A.m = "
<< A.m() << " and B.n = " << B.size()
<< abort(FatalError);
}
if (B.size() != C.n())
{
FatalErrorIn
(
"multiply("
"const scalarRectangularMatrix& A, "
"const DiagonalMatrix<scalar>& B, "
"const scalarRectangularMatrix& C, "
"scalarRectangularMatrix& answer)"
) << "B and C must have identical inner dimensions but B.m = "
<< B.size() << " and C.n = " << C.n()
<< abort(FatalError);
}
ans = scalarRectangularMatrix(A.n(), C.m(), scalar(0));
for(register label i = 0; i < A.n(); i++)
{
for(register label g = 0; g < C.m(); g++)
{
for(register label l = 0; l < C.n(); l++)
{
ans[i][g] += C[l][g] * A[i][l]*B[l];
}
}
}
}
int main()
{
{
// Get the data
ColumnVector X(6);
ColumnVector Y(6);
X << 1 << 2 << 3 << 4 << 6 << 8;
Y << 3.2 << 7.9 << 11.1 << 14.5 << 16.7 << 18.3;
// Do the fit
Model_3pe model(X); // the model object
NonLinearLeastSquares NLLS(model); // the non-linear least squares
// object
ColumnVector Para(3); // for the parameters
Para << 9 << -6 << .5; // trial values of parameters
cout << "Fitting parameters\n";
NLLS.Fit(Y,Para); // do the fit
// Inspect the results
ColumnVector SE; // for the standard errors
NLLS.GetStandardErrors(SE);
cout << "\n\nEstimates and standard errors\n" <<
setw(10) << setprecision(2) << (Para | SE) << endl;
Real ResidualSD = sqrt(NLLS.ResidualVariance());
cout << "\nResidual s.d. = " << setw(10) << setprecision(2) <<
ResidualSD << endl;
SymmetricMatrix Correlations;
NLLS.GetCorrelations(Correlations);
cout << "\nCorrelationMatrix\n" <<
setw(10) << setprecision(2) << Correlations << endl;
ColumnVector Residuals;
NLLS.GetResiduals(Residuals);
DiagonalMatrix Hat;
NLLS.GetHatDiagonal(Hat);
cout << "\nX, Y, Residual, Hat\n" << setw(10) << setprecision(2) <<
(X | Y | Residuals | Hat.AsColumn()) << endl;
// recover var/cov matrix
SymmetricMatrix D;
D << SE.AsDiagonal() * Correlations * SE.AsDiagonal();
cout << "\nVar/cov\n" << setw(14) << setprecision(4) << D << endl;
}
#ifdef DO_FREE_CHECK
FreeCheck::Status();
#endif
return 0;
}
TEST(CholeskyTest, testCholesky)
{
Matrix A = Matrix(3,3,false);
A.fillWithArgs(
2.0,-1.0, 1.0,
-1.0, 2.0,-1.0,
1.0,-1.0, 2.0 );
/* A.fillWithArgs(
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0 );*/
cout << "Input Matrix:" << endl << A << endl;
Matrix *U;
DiagonalMatrix *D;
Cholesky::udutDecompose(&A, &U, &D);
cout << *U;
cout << endl << "[";
for (int i = 0; i < D->size(); i++)
{
cout << D->at(i) << " ";
}
cout << "]" << endl;
UpperUnitaryMatrix *U1;
DiagonalMatrix *D1;
Cholesky::udutDecompose(&A,&U1,&D1);
cout << *U1;
cout << endl << "[";
for (int i = 0; i < D->size(); i++)
{
cout << D->at(i) << " ";
}
cout << "]" << endl;
delete U;
delete U1;
delete D;
delete D1;
}
void getGeneralizedInverse(Matrix& G, Matrix& Gi) {
#ifdef DEBUG
cout << "\n\ngetGeneralizedInverse - Singular Value\n";
#endif
// Singular value decomposition method
// do SVD
Matrix U, V;
DiagonalMatrix D;
SVD(G,D,U,V); // X = U * D * V.t()
#ifdef DEBUG
cout << "D:\n";
cout << setw(9) << setprecision(6) << (D);
cout << "\n\n";
#endif
DiagonalMatrix Di;
Di << D.i();
#ifdef DEBUG
cout << "Di:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
int i=Di.Nrows();
for (; i>=1; i--) {
if (Di(i) > 1000.0) {
Di(i) = 0.0;
}
}
#ifdef DEBUG
cout << "Di with biggies zeroed out:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
//Matrix Gi;
Gi << (U * (Di * V.t()));
return;
}
//======================================================================
Vector rmvn_mt(RNG &rng, const Vector &mu, const DiagonalMatrix &V) {
Vector ans(mu);
const ConstVectorView variances(V.diag());
for (int i = 0; i < mu.size(); ++i) {
ans[i] += rnorm_mt(rng, 0, sqrt(variances[i]));
}
return ans;
}
//#####################################################################
// Function Fast_Singular_Value_Decomposition
//#####################################################################
template<class T> void Matrix<T,3,2>::
fast_singular_value_decomposition(Matrix<T,3,2>& U,DiagonalMatrix<T,2>& singular_values,Matrix<T,2>& V) const
{
if(!is_same<T,double>::value){
Matrix<double,3,2> U_double;DiagonalMatrix<double,2> singular_values_double;Matrix<double,2> V_double;
Matrix<double,3,2>(*this).fast_singular_value_decomposition(U_double,singular_values_double,V_double);
U=Matrix<T,3,2>(U_double);singular_values=DiagonalMatrix<T,2>(singular_values_double);V=Matrix<T,2>(V_double);return;}
// now T is double
DiagonalMatrix<T,2> lambda;normal_equations_matrix().solve_eigenproblem(lambda,V);
if(lambda.x11<0) lambda=lambda.clamp_min(0);
singular_values=lambda.sqrt();
U.set_column(0,(*this*V.column(0)).normalized());
Vector<T,3> other=cross(weighted_normal(),U.column(0));
T other_magnitude=other.magnitude();
U.set_column(1,other_magnitude?other/other_magnitude:U.column(0).unit_orthogonal_vector());
}
void PrincipalComponentsAnalysis::_sortEigens(Matrix& eigenVectors,
DiagonalMatrix& eigenValues)
{
// simple bubble sort, slow, but I don't expect very large matrices. Lots of room for
// improvement.
bool change = true;
while (change)
{
change = false;
for (int c = 0; c < eigenVectors.Ncols() - 1; c++)
{
if (eigenValues.element(c) < eigenValues.element(c + 1))
{
std::swap(eigenValues.element(c), eigenValues.element(c + 1));
for (int r = 0; r < eigenVectors.Nrows(); r++)
{
std::swap(eigenVectors.element(r, c), eigenVectors.element(r, c + 1));
}
}
}
}
}
void test4(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
cout << "\n\nTest 4 - QR triangularisation\n";
// QR triangularisation method
// load data - 1s into col 1 of matrix
int npred1 = npred+1;
Matrix X(nobs,npred1); ColumnVector Y(nobs);
X.Column(1) = 1.0; X.Column(2) << x1; X.Column(3) << x2; Y << y;
// do Householder triangularisation
// no need to deal with constant term separately
Matrix X1 = X; // Want copy of matrix
ColumnVector Y1 = Y;
UpperTriangularMatrix U; ColumnVector M;
QRZ(X1, U); QRZ(X1, Y1, M); // Y1 now contains resids
ColumnVector A = U.i() * M;
ColumnVector Fitted = X * A;
Real ResVar = Y1.SumSquare() / (nobs-npred1);
// get variances of estimates
U = U.i(); DiagonalMatrix D; D << U * U.t();
// Get diagonals of Hat matrix
DiagonalMatrix Hat; Hat << X1 * X1.t();
// print out answers
cout << "\nEstimates and their standard errors\n\n";
ColumnVector SE(npred1);
for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
cout << setw(11) << setprecision(5) << (A | SE) << endl;
cout << "\nObservations, fitted value, residual value, hat value\n";
cout << setw(9) << setprecision(3) <<
(X.Columns(2,3) | Y | Fitted | Y1 | Hat.AsColumn());
cout << "\n\n";
}
void test5(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
cout << "\n\nTest 5 - singular value\n";
// Singular value decomposition method
// load data - 1s into col 1 of matrix
int npred1 = npred+1;
Matrix X(nobs,npred1); ColumnVector Y(nobs);
X.Column(1) = 1.0; X.Column(2) << x1; X.Column(3) << x2; Y << y;
// do SVD
Matrix U, V; DiagonalMatrix D;
SVD(X,D,U,V); // X = U * D * V.t()
ColumnVector Fitted = U.t() * Y;
ColumnVector A = V * ( D.i() * Fitted );
Fitted = U * Fitted;
ColumnVector Residual = Y - Fitted;
Real ResVar = Residual.SumSquare() / (nobs-npred1);
// get variances of estimates
D << V * (D * D).i() * V.t();
// Get diagonals of Hat matrix
DiagonalMatrix Hat; Hat << U * U.t();
// print out answers
cout << "\nEstimates and their standard errors\n\n";
ColumnVector SE(npred1);
for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
cout << setw(11) << setprecision(5) << (A | SE) << endl;
cout << "\nObservations, fitted value, residual value, hat value\n";
cout << setw(9) << setprecision(3) <<
(X.Columns(2,3) | Y | Fitted | Residual | Hat.AsColumn());
cout << "\n\n";
}
int my_main() // called by main()
{
Tracer tr("my_main "); // for tracking exceptions
int n = 7; // this is the order we will work with
int i, j;
// declare a matrix
SymmetricMatrix H(n);
// load values for Hilbert matrix
for (i = 1; i <= n; ++i) for (j = 1; j <= i; ++j)
H(i, j) = 1.0 / (i + j - 1);
// print the matrix
cout << "SymmetricMatrix H" << endl;
cout << setw(10) << setprecision(7) << H << endl;
// calculate its eigenvalues and eigenvectors and print them
Matrix U; DiagonalMatrix D;
EigenValues(H, D, U);
cout << "Eigenvalues of H" << endl;
cout << setw(17) << setprecision(14) << D.AsColumn() << endl;
cout << "Eigenvector matrix, U" << endl;
cout << setw(10) << setprecision(7) << U << endl;
// check orthogonality
cout << "U * U.t() (should be near identity)" << endl;
cout << setw(10) << setprecision(7) << (U * U.t()) << endl;
// check decomposition
cout << "U * D * U.t() (should be near H)" << endl;
cout << setw(10) << setprecision(7) << (U * D * U.t()) << endl;
return 0;
}
void Print(const DiagonalMatrix& X)
{
++PCN;
cout << "\nMatrix type: " << X.Type().Value() << " (";
cout << X.Nrows() << ", ";
cout << X.Ncols() << ")\n\n";
if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
int nr=X.Nrows(); int nc=X.Ncols();
for (int i=1; i<=nr; i++)
{
for (int j=1; j<i; j++) cout << "\t";
if (i<=nc) cout << X(i,i) << "\t";
cout << "\n";
}
cout << flush; ++PCZ;
}
void SymMatrixBlocks::SetDiag(DiagonalMatrix &src, DiagonalMatrix &ret)
{
// this function treats DiagonalMatrix src as a column vector with
// the diagonal entries as the vector components.
// the result is another diagonal matrix with the diagonal entries
// treates as the components of the resulting vector from the
// matrix-vector multiply.
ret.Zero();
// these really must be of the same size to make sense.
if( m_iSize != src.m_iSize)
return;
// if all entries are still zero, we're already done
if(m_bAllZero) return;
for(int i=0; i<m_iSize; i++ )
{
int start = m_rowstart[i];
int length = m_rowstart[i+1] - start;
for(int j = 0; j<length; j++ )
{
// perform multiplication for nonzero matrix blocks
int iCol = m_col_lookup[start+j];
Matrix3x3 *temp = m_matBlock[start+j];
ret.m_block[i].x += temp->elt[0] * src.m_block[iCol].x + temp->elt[1] * src.m_block[iCol].y + temp->elt[2] * src.m_block[iCol].z;
ret.m_block[i].y += temp->elt[3] * src.m_block[iCol].x + temp->elt[4] * src.m_block[iCol].y + temp->elt[5] * src.m_block[iCol].z;
ret.m_block[i].z += temp->elt[6] * src.m_block[iCol].x + temp->elt[7] * src.m_block[iCol].y + temp->elt[8] * src.m_block[iCol].z;
// since we stored stuff in upper triangular form, we need to use the transpose of the appropriate matrix block for the complementary matrix-vector mult
// NOTE: symmetry does not hold for the supermatrix or the submatrices necessarily, but for the matrix obtained by embedding the submatrices into the supermatrix structure
if( i != iCol )
{
ret.m_block[iCol].x += temp->elt[0] * src.m_block[i].x + temp->elt[3] * src.m_block[i].y + temp->elt[6] * src.m_block[i].z;
ret.m_block[iCol].y += temp->elt[1] * src.m_block[i].x + temp->elt[4] * src.m_block[i].y + temp->elt[7] * src.m_block[i].z;
ret.m_block[iCol].z += temp->elt[2] * src.m_block[i].x + temp->elt[5] * src.m_block[i].y + temp->elt[8] * src.m_block[i].z;
}
}
}
}
void SpinAdapted::Linear::block_davidson(vector<Wavefunction>& b, DiagonalMatrix& h_diag, double normtol, const bool &warmUp, Davidson_functor& h_multiply, bool& useprecond, int currentRoot, std::vector<Wavefunction> &lowerStates)
{
pout.precision (12);
#ifndef SERIAL
mpi::communicator world;
#endif
int iter = 0;
double levelshift = 0.0;
int nroots = b.size();
//normalise all the guess roots
if(mpigetrank() == 0) {
for(int i=0; i<nroots; ++i)
{
for(int j=0; j<i; ++j)
{
double overlap = DotProduct(b[j], b[i]);
ScaleAdd(-overlap, b[j], b[i]);
}
Normalise(b[i]);
}
//if we are doing state specific, lowerstates has lower energy states
if (lowerStates.size() != 0) {
for (int i=0; i<lowerStates.size(); i++) {
double overlap = DotProduct(b[0], lowerStates[i]);
ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], b[0]);
}
Normalise(b[0]);
}
}
vector<Wavefunction> sigma;
int converged_roots = 0;
int maxiter = h_diag.Ncols() - lowerStates.size();
while(true)
{
if (dmrginp.outputlevel() > 0)
pout << "\t\t\t Davidson Iteration :: " << iter << endl;
++iter;
dmrginp.hmultiply -> start();
int sigmasize, bsize;
if (mpigetrank() == 0) {
sigmasize = sigma.size();
bsize = b.size();
}
#ifndef SERIAL
mpi::broadcast(world, sigmasize, 0);
mpi::broadcast(world, bsize, 0);
#endif
//multiply all guess vectors with hamiltonian c = Hv
for(int i=sigmasize; i<bsize; ++i) {
Wavefunction sigmai, bi;
Wavefunction* sigmaptr=&sigmai, *bptr = &bi;
if (mpigetrank() == 0) {
sigma.push_back(b[i]);
sigma[i].Clear();
sigmaptr = &sigma[i];
bptr = &b[i];
}
#ifndef SERIAL
mpi::broadcast(world, *bptr, 0);
#endif
if (mpigetrank() != 0) {
sigmai = bi;
sigmai.Clear();
}
h_multiply(*bptr, *sigmaptr);
//if (mpigetrank() == 0) {
// cout << *bptr << endl;
// cout << *sigmaptr << endl;
//}
}
dmrginp.hmultiply -> stop();
Wavefunction r;
DiagonalMatrix subspace_eigenvalues;
if (mpigetrank() == 0) {
Matrix subspace_h(b.size(), b.size());
for (int i = 0; i < b.size(); ++i)
for (int j = 0; j <= i; ++j) {
subspace_h.element(i, j) = DotProduct(b[i], sigma[j]);
subspace_h.element(j, i) = subspace_h.element(i, j);
}
Matrix alpha;
diagonalise(subspace_h, subspace_eigenvalues, alpha);
if (dmrginp.outputlevel() > 0) {
for (int i = 1; i <= subspace_eigenvalues.Ncols (); ++i)
pout << "\t\t\t " << i << " :: " << subspace_eigenvalues(i,i)+dmrginp.get_coreenergy() << endl;
}
//now calculate the ritz vectors which are approximate eigenvectors
vector<Wavefunction> btmp = b;
vector<Wavefunction> sigmatmp = sigma;
for (int i = 0; i < b.size(); ++i)
{
Scale(alpha.element(i, i), b[i]);
Scale(alpha.element(i, i), sigma[i]);
}
for (int i = 0; i < b.size(); ++i)
for (int j = 0; j < b.size(); ++j)
{
if (i != j)
{
ScaleAdd(alpha.element(i, j), btmp[i], b[j]);
ScaleAdd(alpha.element(i, j), sigmatmp[i], sigma[j]);
}
}
// build residual
for (int i=0; i<converged_roots; i++) {
r = sigma[i];
ScaleAdd(-subspace_eigenvalues(i+1), b[i], r);
double rnorm = DotProduct(r,r);
if (rnorm > normtol) {
converged_roots = i;
if (dmrginp.outputlevel() > 0)
pout << "\t\t\t going back to converged root "<<i<<" "<<rnorm<<" > "<<normtol<<endl;
continue;
}
}
r = sigma[converged_roots];
ScaleAdd(-subspace_eigenvalues(converged_roots+1), b[converged_roots], r);
if (lowerStates.size() != 0) {
for (int i=0; i<lowerStates.size(); i++) {
double overlap = DotProduct(r, lowerStates[i]);
ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], r);
//ScaleAdd(-overlap, lowerStates[i], r);
}
}
}
double rnorm;
if (mpigetrank() == 0)
rnorm = DotProduct(r,r);
#ifndef SERIAL
mpi::broadcast(world, converged_roots, 0);
mpi::broadcast(world, rnorm, 0);
#endif
if (useprecond && mpigetrank() == 0)
olsenPrecondition(r, b[converged_roots], subspace_eigenvalues(converged_roots+1), h_diag, levelshift);
if (dmrginp.outputlevel() > 0)
pout << "\t \t \t residual :: " << rnorm << endl;
if (rnorm < normtol)
{
if (dmrginp.outputlevel() > 0)
pout << "\t\t\t Converged root " << converged_roots << endl;
++converged_roots;
if (converged_roots == nroots)
{
if (mpigetrank() == 0) {
for (int i = 0; i < min((int)(b.size()), h_diag.Ncols()); ++i)
h_diag.element(i) = subspace_eigenvalues.element(i);
}
break;
}
}
else if (mpigetrank() == 0)
{
if(b.size() >= dmrginp.deflation_max_size())
{
if (dmrginp.outputlevel() > 0)
pout << "\t\t\t Deflating block Davidson...\n";
b.resize(dmrginp.deflation_min_size());
sigma.resize(dmrginp.deflation_min_size());
}
for (int j = 0; j < b.size(); ++j)
{
//Normalize
double normalization = DotProduct(r, r);
Scale(1./sqrt(normalization), r);
double overlap = DotProduct(r, b[j]);
ScaleAdd(-overlap, b[j], r);
}
//if we are doing state specific, lowerstates has lower energy states
if (lowerStates.size() != 0) {
for (int i=0; i<lowerStates.size(); i++) {
double overlap = DotProduct(r, lowerStates[i]);
ScaleAdd(-overlap/DotProduct(lowerStates[i], lowerStates[i]), lowerStates[i], r);
//ScaleAdd(-overlap, lowerStates[i], r);
}
}
//double tau2 = DotProduct(r,r);
Normalise(r);
b.push_back(r);
}
}
}
void Clean(DiagonalMatrix& A, Real c)
{
int nr = A.Nrows();
for (int i=1; i<=nr; i++)
{ Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; }
}
ReturnMatrix Returner7(const GenericMatrix& GM)
{ DiagonalMatrix M = GM*7; M.Release(); return M; }
void trymate()
{
Tracer et("Fourteenth test of Matrix package");
Tracer::PrintTrace();
{
Tracer et1("Stage 1");
Matrix A(8,5);
{
Real a[] = { 22, 10, 2, 3, 7,
14, 7, 10, 0, 8,
-1, 13, -1,-11, 3,
-3, -2, 13, -2, 4,
9, 8, 1, -2, 4,
9, 1, -7, 5, -1,
2, -6, 6, 5, 1,
4, 5, 0, -2, 2
};
int ai[] = { 22, 10, 2, 3, 7,
14, 7, 10, 0, 8,
-1, 13, -1,-11, 3,
-3, -2, 13, -2, 4,
9, 8, 1, -2, 4,
9, 1, -7, 5, -1,
2, -6, 6, 5, 1,
4, 5, 0, -2, 2
};
A << a;
Matrix AI(8,5);
AI << ai;
AI -= A;
Print(AI);
int b[] = { 13, -1,-11,
-2, 13, -2,
8, 1, -2,
1, -7, 5
};
Matrix B(8, 5);
B = 23;
B.SubMatrix(3,6,2,4) << b;
AI = A;
AI.Rows(1,2) = 23;
AI.Rows(7,8) = 23;
AI.Column(1) = 23;
AI.Column(5) = 23;
AI -= B;
Print(AI);
}
DiagonalMatrix D;
Matrix U;
Matrix V;
int anc = A.Ncols();
IdentityMatrix I(anc);
SymmetricMatrix S1;
S1 << A.t() * A;
SymmetricMatrix S2;
S2 << A * A.t();
Real zero = 0.0;
SVD(A+zero,D,U,V);
CheckIsSorted(D);
DiagonalMatrix D1;
SVD(A,D1);
CheckIsSorted(D1);
D1 -= D;
Clean(D1,0.000000001);
Print(D1);
Matrix W;
SVD(A, D1, W, W, true, false);
D1 -= D;
W -= U;
Clean(W,0.000000001);
Print(W);
Clean(D1,0.000000001);
Print(D1);
Matrix WX;
SVD(A, D1, WX, W, false, true);
D1 -= D;
W -= V;
Clean(W,0.000000001);
Print(W);
Clean(D1,0.000000001);
Print(D1);
Matrix SU = U.t() * U - I;
Clean(SU,0.000000001);
Print(SU);
Matrix SV = V.t() * V - I;
Clean(SV,0.000000001);
Print(SV);
Matrix B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
D1=0.0;
SVD(A,D1,A);
CheckIsSorted(D1);
A -= U;
Clean(A,0.000000001);
Print(A);
D(1) -= sqrt(1248.0);
D(2) -= 20;
D(3) -= sqrt(384.0);
Clean(D,0.000000001);
Print(D);
Jacobi(S1, D, V);
CheckIsSorted(D, true);
V = S1 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D = D.Reverse();
D(1)-=1248;
D(2)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
Jacobi(S1, D);
CheckIsSorted(D, true);
D = D.Reverse();
D(1)-=1248;
D(2)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
SymmetricMatrix JW(5);
Jacobi(S1, D, JW);
CheckIsSorted(D, true);
D = D.Reverse();
D(1)-=1248;
D(2)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
Jacobi(S2, D, V);
CheckIsSorted(D, true);
V = S2 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D = D.Reverse();
D(1)-=1248;
D(2)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
EigenValues(S1, D, V);
CheckIsSorted(D, true);
V = S1 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D(5)-=1248;
D(4)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
EigenValues(S2, D, V);
CheckIsSorted(D, true);
V = S2 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D(8)-=1248;
D(7)-=400;
D(6)-=384;
Clean(D,0.000000001);
Print(D);
EigenValues(S1, D);
CheckIsSorted(D, true);
D(5)-=1248;
D(4)-=400;
D(3)-=384;
Clean(D,0.000000001);
Print(D);
SymmetricMatrix EW(S2);
EigenValues(S2, D, EW);
CheckIsSorted(D, true);
D(8)-=1248;
D(7)-=400;
D(6)-=384;
Clean(D,0.000000001);
Print(D);
}
{
Tracer et1("Stage 2");
Matrix A(20,21);
int i,j;
for (i=1; i<=20; i++) for (j=1; j<=21; j++)
{
if (i>j) A(i,j) = 0;
else if (i==j) A(i,j) = 21-i;
else A(i,j) = -1;
}
A = A.t();
SymmetricMatrix S1;
S1 << A.t() * A;
SymmetricMatrix S2;
S2 << A * A.t();
DiagonalMatrix D;
Matrix U;
Matrix V;
DiagonalMatrix I(A.Ncols());
I=1.0;
SVD(A,D,U,V);
CheckIsSorted(D);
Matrix SU = U.t() * U - I;
Clean(SU,0.000000001);
Print(SU);
Matrix SV = V.t() * V - I;
Clean(SV,0.000000001);
Print(SV);
Matrix B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
for (i=1; i<=20; i++) D(i) -= sqrt((22.0-i)*(21.0-i));
Clean(D,0.000000001);
Print(D);
Jacobi(S1, D, V);
CheckIsSorted(D, true);
V = S1 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D = D.Reverse();
for (i=1; i<=20; i++) D(i) -= (22-i)*(21-i);
Clean(D,0.000000001);
Print(D);
Jacobi(S2, D, V);
CheckIsSorted(D, true);
V = S2 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
D = D.Reverse();
for (i=1; i<=20; i++) D(i) -= (22-i)*(21-i);
Clean(D,0.000000001);
Print(D);
EigenValues(S1, D, V);
CheckIsSorted(D, true);
V = S1 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
for (i=1; i<=20; i++) D(i) -= (i+1)*i;
Clean(D,0.000000001);
Print(D);
EigenValues(S2, D, V);
CheckIsSorted(D, true);
V = S2 - V * D * V.t();
Clean(V,0.000000001);
Print(V);
for (i=2; i<=21; i++) D(i) -= (i-1)*i;
Clean(D,0.000000001);
Print(D);
EigenValues(S1, D);
CheckIsSorted(D, true);
for (i=1; i<=20; i++) D(i) -= (i+1)*i;
Clean(D,0.000000001);
Print(D);
EigenValues(S2, D);
CheckIsSorted(D, true);
for (i=2; i<=21; i++) D(i) -= (i-1)*i;
Clean(D,0.000000001);
Print(D);
}
{
Tracer et1("Stage 3");
Matrix A(30,30);
int i,j;
for (i=1; i<=30; i++) for (j=1; j<=30; j++)
{
if (i>j) A(i,j) = 0;
else if (i==j) A(i,j) = 1;
else A(i,j) = -1;
}
Real d1 = A.LogDeterminant().Value();
DiagonalMatrix D;
Matrix U;
Matrix V;
DiagonalMatrix I(A.Ncols());
I=1.0;
SVD(A,D,U,V);
CheckIsSorted(D);
Matrix SU = U.t() * U - I;
Clean(SU,0.000000001);
Print(SU);
Matrix SV = V.t() * V - I;
Clean(SV,0.000000001);
Print(SV);
Real d2 = D.LogDeterminant().Value();
Matrix B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
Real d3 = D.LogDeterminant().Value();
ColumnVector Test(3);
Test(1) = d1 - 1;
Test(2) = d2 - 1;
Test(3) = d3 - 1;
Clean(Test,0.00000001);
Print(Test); // only 8 decimal figures
A.ReSize(2,2);
Real a = 1.5;
Real b = 2;
Real c = 2 * (a*a + b*b);
A << a << b << a << b;
I.ReSize(2);
I=1;
SVD(A,D,U,V);
CheckIsSorted(D);
SU = U.t() * U - I;
Clean(SU,0.000000001);
Print(SU);
SV = V.t() * V - I;
Clean(SV,0.000000001);
Print(SV);
B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
D = D*D;
SortDescending(D);
DiagonalMatrix D50(2);
D50 << c << 0;
D = D - D50;
Clean(D,0.000000001);
Print(D);
A << a << a << b << b;
SVD(A,D,U,V);
CheckIsSorted(D);
SU = U.t() * U - I;
Clean(SU,0.000000001);
Print(SU);
SV = V.t() * V - I;
Clean(SV,0.000000001);
Print(SV);
B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
D = D*D;
SortDescending(D);
D = D - D50;
Clean(D,0.000000001);
Print(D);
}
{
Tracer et1("Stage 4");
// test for bug found by Olof Runborg,
// Department of Numerical Analysis and Computer Science (NADA),
// KTH, Stockholm
Matrix A(22,20);
A = 0;
int a=1;
A(a+0,a+2) = 1;
A(a+0,a+18) = -1;
A(a+1,a+9) = 1;
A(a+1,a+12) = -1;
A(a+2,a+11) = 1;
A(a+2,a+12) = -1;
A(a+3,a+10) = 1;
A(a+3,a+19) = -1;
A(a+4,a+16) = 1;
A(a+4,a+19) = -1;
A(a+5,a+17) = 1;
A(a+5,a+18) = -1;
A(a+6,a+10) = 1;
A(a+6,a+4) = -1;
A(a+7,a+3) = 1;
A(a+7,a+2) = -1;
A(a+8,a+14) = 1;
A(a+8,a+15) = -1;
A(a+9,a+13) = 1;
A(a+9,a+16) = -1;
A(a+10,a+8) = 1;
A(a+10,a+9) = -1;
A(a+11,a+1) = 1;
A(a+11,a+15) = -1;
A(a+12,a+16) = 1;
A(a+12,a+4) = -1;
A(a+13,a+6) = 1;
A(a+13,a+9) = -1;
A(a+14,a+5) = 1;
A(a+14,a+4) = -1;
A(a+15,a+0) = 1;
A(a+15,a+1) = -1;
A(a+16,a+14) = 1;
A(a+16,a+0) = -1;
A(a+17,a+7) = 1;
A(a+17,a+6) = -1;
A(a+18,a+13) = 1;
A(a+18,a+5) = -1;
A(a+19,a+7) = 1;
A(a+19,a+8) = -1;
A(a+20,a+17) = 1;
A(a+20,a+3) = -1;
A(a+21,a+6) = 1;
A(a+21,a+11) = -1;
Matrix U, V;
DiagonalMatrix S;
SVD(A, S, U, V, true, true);
CheckIsSorted(S);
DiagonalMatrix D(20);
D = 1;
Matrix tmp = U.t() * U - D;
Clean(tmp,0.000000001);
Print(tmp);
tmp = V.t() * V - D;
Clean(tmp,0.000000001);
Print(tmp);
tmp = U * S * V.t() - A ;
Clean(tmp,0.000000001);
Print(tmp);
}
{
Tracer et1("Stage 5");
Matrix A(10,10);
A.Row(1) << 1.00 << 0.07 << 0.05 << 0.00 << 0.06
<< 0.09 << 0.03 << 0.02 << 0.02 << -0.03;
A.Row(2) << 0.07 << 1.00 << 0.05 << 0.05 << -0.03
<< 0.07 << 0.00 << 0.07 << 0.00 << 0.02;
A.Row(3) << 0.05 << 0.05 << 1.00 << 0.05 << 0.02
<< 0.01 << -0.05 << 0.04 << 0.05 << -0.03;
A.Row(4) << 0.00 << 0.05 << 0.05 << 1.00 << -0.05
<< 0.04 << 0.01 << 0.02 << -0.05 << 0.00;
A.Row(5) << 0.06 << -0.03 << 0.02 << -0.05 << 1.00
<< -0.03 << 0.02 << -0.02 << 0.04 << 0.00;
A.Row(6) << 0.09 << 0.07 << 0.01 << 0.04 << -0.03
<< 1.00 << -0.06 << 0.08 << -0.02 << -0.10;
A.Row(7) << 0.03 << 0.00 << -0.05 << 0.01 << 0.02
<< -0.06 << 1.00 << 0.09 << 0.12 << -0.03;
A.Row(8) << 0.02 << 0.07 << 0.04 << 0.02 << -0.02
<< 0.08 << 0.09 << 1.00 << 0.00 << -0.02;
A.Row(9) << 0.02 << 0.00 << 0.05 << -0.05 << 0.04
<< -0.02 << 0.12 << 0.00 << 1.00 << 0.02;
A.Row(10) << -0.03 << 0.02 << -0.03 << 0.00 << 0.00
<< -0.10 << -0.03 << -0.02 << 0.02 << 1.00;
SymmetricMatrix AS;
AS << A;
Matrix V;
DiagonalMatrix D, D1;
ColumnVector Check(6);
EigenValues(AS,D,V);
CheckIsSorted(D, true);
Check(1) = MaximumAbsoluteValue(A - V * D * V.t());
DiagonalMatrix I(10);
I = 1;
Check(2) = MaximumAbsoluteValue(V * V.t() - I);
Check(3) = MaximumAbsoluteValue(V.t() * V - I);
EigenValues(AS, D1);
CheckIsSorted(D1, true);
D -= D1;
Clean(D,0.000000001);
Print(D);
Jacobi(AS,D,V);
Check(4) = MaximumAbsoluteValue(A - V * D * V.t());
Check(5) = MaximumAbsoluteValue(V * V.t() - I);
Check(6) = MaximumAbsoluteValue(V.t() * V - I);
SortAscending(D);
D -= D1;
Clean(D,0.000000001);
Print(D);
Clean(Check,0.000000001);
Print(Check);
// Check loading rows
SymmetricMatrix B(10);
B.Row(1) << 1.00;
B.Row(2) << 0.07 << 1.00;
B.Row(3) << 0.05 << 0.05 << 1.00;
B.Row(4) << 0.00 << 0.05 << 0.05 << 1.00;
B.Row(5) << 0.06 << -0.03 << 0.02 << -0.05 << 1.00;
B.Row(6) << 0.09 << 0.07 << 0.01 << 0.04 << -0.03
<< 1.00;
B.Row(7) << 0.03 << 0.00 << -0.05 << 0.01 << 0.02
<< -0.06 << 1.00;
B.Row(8) << 0.02 << 0.07 << 0.04 << 0.02 << -0.02
<< 0.08 << 0.09 << 1.00;
B.Row(9) << 0.02 << 0.00 << 0.05 << -0.05 << 0.04
<< -0.02 << 0.12 << 0.00 << 1.00;
B.Row(10) << -0.03 << 0.02 << -0.03 << 0.00 << 0.00
<< -0.10 << -0.03 << -0.02 << 0.02 << 1.00;
B -= AS;
Print(B);
}
{
Tracer et1("Stage 6");
// badly scaled matrix
Matrix A(9,9);
A.Row(1) << 1.13324e+012 << 3.68788e+011 << 3.35163e+009
<< 3.50193e+011 << 1.25335e+011 << 1.02212e+009
<< 3.16602e+009 << 1.02418e+009 << 9.42959e+006;
A.Row(2) << 3.68788e+011 << 1.67128e+011 << 1.27449e+009
<< 1.25335e+011 << 6.05413e+010 << 4.34573e+008
<< 1.02418e+009 << 4.69192e+008 << 3.61098e+006;
A.Row(3) << 3.35163e+009 << 1.27449e+009 << 1.25571e+007
<< 1.02212e+009 << 4.34573e+008 << 3.69769e+006
<< 9.42959e+006 << 3.61098e+006 << 3.59450e+004;
A.Row(4) << 3.50193e+011 << 1.25335e+011 << 1.02212e+009
<< 1.43514e+011 << 5.42310e+010 << 4.15822e+008
<< 1.23068e+009 << 4.31545e+008 << 3.58714e+006;
A.Row(5) << 1.25335e+011 << 6.05413e+010 << 4.34573e+008
<< 5.42310e+010 << 2.76601e+010 << 1.89102e+008
<< 4.31545e+008 << 2.09778e+008 << 1.51083e+006;
A.Row(6) << 1.02212e+009 << 4.34573e+008 << 3.69769e+006
<< 4.15822e+008 << 1.89102e+008 << 1.47143e+006
<< 3.58714e+006 << 1.51083e+006 << 1.30165e+004;
A.Row(7) << 3.16602e+009 << 1.02418e+009 << 9.42959e+006
<< 1.23068e+009 << 4.31545e+008 << 3.58714e+006
<< 1.12335e+007 << 3.54778e+006 << 3.34311e+004;
A.Row(8) << 1.02418e+009 << 4.69192e+008 << 3.61098e+006
<< 4.31545e+008 << 2.09778e+008 << 1.51083e+006
<< 3.54778e+006 << 1.62552e+006 << 1.25885e+004;
A.Row(9) << 9.42959e+006 << 3.61098e+006 << 3.59450e+004
<< 3.58714e+006 << 1.51083e+006 << 1.30165e+004
<< 3.34311e+004 << 1.25885e+004 << 1.28000e+002;
SymmetricMatrix AS;
AS << A;
Matrix V;
DiagonalMatrix D, D1;
ColumnVector Check(6);
EigenValues(AS,D,V);
CheckIsSorted(D, true);
Check(1) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
DiagonalMatrix I(9);
I = 1;
Check(2) = MaximumAbsoluteValue(V * V.t() - I);
Check(3) = MaximumAbsoluteValue(V.t() * V - I);
EigenValues(AS, D1);
D -= D1;
Clean(D,0.001);
Print(D);
Jacobi(AS,D,V);
Check(4) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
Check(5) = MaximumAbsoluteValue(V * V.t() - I);
Check(6) = MaximumAbsoluteValue(V.t() * V - I);
SortAscending(D);
D -= D1;
Clean(D,0.001);
Print(D);
Clean(Check,0.0000001);
Print(Check);
}
{
Tracer et1("Stage 7");
// matrix with all singular values close to 1
Matrix A(8,8);
A.Row(1)<<-0.4343<<-0.0445<<-0.4582<<-0.1612<<-0.3191<<-0.6784<<0.1068<<0;
A.Row(2)<<0.5791<<0.5517<<0.2575<<-0.1055<<-0.0437<<-0.5282<<0.0442<<0;
A.Row(3)<<0.5709<<-0.5179<<-0.3275<<0.2598<<-0.196<<-0.1451<<-0.4143<<0;
A.Row(4)<<0.2785<<-0.5258<<0.1251<<-0.4382<<0.0514<<-0.0446<<0.6586<<0;
A.Row(5)<<0.2654<<0.3736<<-0.7436<<-0.0122<<0.0376<<0.3465<<0.3397<<0;
A.Row(6)<<0.0173<<-0.0056<<-0.1903<<-0.7027<<0.4863<<-0.0199<<-0.4825<<0;
A.Row(7)<<0.0434<<0.0966<<0.1083<<-0.4576<<-0.7857<<0.3425<<-0.1818<<0;
A.Row(8)<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<-1.0;
Matrix U,V;
DiagonalMatrix D;
SVD(A,D,U,V);
CheckIsSorted(D);
Matrix B = U * D * V.t() - A;
Clean(B,0.000000001);
Print(B);
DiagonalMatrix I(8);
I = 1;
D -= I;
Clean(D,0.0001);
Print(D);
U *= U.t();
U -= I;
Clean(U,0.000000001);
Print(U);
V *= V.t();
V -= I;
Clean(V,0.000000001);
Print(V);
}
{
Tracer et1("Stage 8");
// check SortSV functions
Matrix A(15, 10);
int i, j;
for (i = 1; i <= 15; ++i) for (j = 1; j <= 10; ++j)
A(i, j) = i + j / 1000.0;
DiagonalMatrix D(10);
D << 0.2 << 0.5 << 0.1 << 0.7 << 0.8 << 0.3 << 0.4 << 0.7 << 0.9 << 0.6;
Matrix U = A;
Matrix V = 10 - 2 * A;
Matrix Prod = U * D * V.t();
DiagonalMatrix D2 = D;
SortDescending(D2);
DiagonalMatrix D1 = D;
SortSV(D1, U, V);
Matrix X = D1 - D2;
Print(X);
X = Prod - U * D1 * V.t();
Clean(X,0.000000001);
Print(X);
U = A;
V = 10 - 2 * A;
D1 = D;
SortSV(D1, U);
X = D1 - D2;
Print(X);
D1 = D;
SortSV(D1, V);
X = D1 - D2;
Print(X);
X = Prod - U * D1 * V.t();
Clean(X,0.000000001);
Print(X);
D2 = D;
SortAscending(D2);
U = A;
V = 10 - 2 * A;
D1 = D;
SortSV(D1, U, V, true);
X = D1 - D2;
Print(X);
X = Prod - U * D1 * V.t();
Clean(X,0.000000001);
Print(X);
U = A;
V = 10 - 2 * A;
D1 = D;
SortSV(D1, U, true);
X = D1 - D2;
Print(X);
D1 = D;
SortSV(D1, V, true);
X = D1 - D2;
Print(X);
X = Prod - U * D1 * V.t();
Clean(X,0.000000001);
Print(X);
}
{
Tracer et1("Stage 9");
// Tom William's example
Matrix A(10,10);
Matrix U;
Matrix V;
DiagonalMatrix Sigma;
Real myVals[] =
{
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 0,
1, 1, 1, 1, 1, 1, 1, 1, 1, 0,
1, 1, 1, 1, 1, 1, 1, 1, 0, 0,
1, 1, 1, 1, 1, 1, 1, 0, 0, 0,
1, 1, 1, 1, 1, 0, 0, 0, 0, 0,
};
A << myVals;
SVD(A, Sigma, U, V);
CheckIsSorted(Sigma);
A -= U * Sigma * V.t();
Clean(A, 0.000000001);
Print(A);
}
}
void trymat8()
{
// cout << "\nEighth test of Matrix package\n";
Tracer et("Eighth test of Matrix package");
Tracer::PrintTrace();
int i;
DiagonalMatrix D(6);
for (i=1;i<=6;i++) D(i,i)=i*i+i-10;
DiagonalMatrix D2=D;
Matrix MD=D;
DiagonalMatrix D1(6); for (i=1;i<=6;i++) D1(i,i)=-100+i*i*i;
Matrix MD1=D1;
Print(Matrix(D*D1-MD*MD1));
Print(Matrix((-D)*D1+MD*MD1));
Print(Matrix(D*(-D1)+MD*MD1));
DiagonalMatrix DX=D;
{
Tracer et1("Stage 1");
DX=(DX+D1)*DX; Print(Matrix(DX-(MD+MD1)*MD));
DX=D;
DX=-DX*DX+(DX-(-D1))*((-D1)+DX);
// Matrix MX = Matrix(MD1);
// MD1=DX+(MX.t())*(MX.t()); Print(MD1);
MD1=DX+(Matrix(MD1).t())*(Matrix(MD1).t()); Print(MD1);
DX=D; DX=DX; DX=D2-DX; Print(DiagonalMatrix(DX));
DX=D;
}
{
Tracer et1("Stage 2");
D.Release(2);
D1=D; D2=D;
Print(DiagonalMatrix(D1-DX));
Print(DiagonalMatrix(D2-DX));
MD1=1.0;
Print(Matrix(MD1-1.0));
}
{
Tracer et1("Stage 3");
//GenericMatrix
LowerTriangularMatrix LT(4);
LT << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8 << 9 << 10;
UpperTriangularMatrix UT = LT.t() * 2.0;
GenericMatrix GM1 = LT;
LowerTriangularMatrix LT1 = GM1-LT; Print(LT1);
GenericMatrix GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
GM2 = GM1*2; LT1 = GM2; LT1 = LT1-LT*2; Print(LT1);
GM1.Release();
GM1=GM1; LT1=GM1-LT; Print(LT1); LT1=GM1-LT; Print(LT1);
GM1.Release();
GM1=GM1*4; LT1=GM1-LT*4; Print(LT1);
LT1=GM1-LT*4; Print(LT1); GM1.CleanUp();
GM1=LT; GM2=UT; GM1=GM1*GM2; Matrix M=GM1; M=M-LT*UT; Print(M);
Transposer(LT,GM2); LT1 = LT - GM2.t(); Print(LT1);
GM1=LT; Transposer(GM1,GM2); LT1 = LT - GM2.t(); Print(LT1);
GM1 = LT; GM1 = GM1 + GM1; LT1 = LT*2-GM1; Print(LT1);
DiagonalMatrix D; D << LT; GM1 = D; LT1 = GM1; LT1 -= D; Print(LT1);
UpperTriangularMatrix UT1 = GM1; UT1 -= D; Print(UT1);
}
{
Tracer et1("Stage 4");
// Another test of SVD
Matrix M(12,12); M = 0;
M(1,1) = M(2,2) = M(4,4) = M(6,6) =
M(7,7) = M(8,8) = M(10,10) = M(12,12) = -1;
M(1,6) = M(1,12) = -5.601594;
M(3,6) = M(3,12) = -0.000165;
M(7,6) = M(7,12) = -0.008294;
DiagonalMatrix D;
SVD(M,D);
SortDescending(D);
// answer given by matlab
DiagonalMatrix DX(12);
DX(1) = 8.0461;
DX(2) = DX(3) = DX(4) = DX(5) = DX(6) = DX(7) = 1;
DX(8) = 0.1243;
DX(9) = DX(10) = DX(11) = DX(12) = 0;
D -= DX; Clean(D,0.0001); Print(D);
}
#ifndef DONT_DO_NRIC
{
Tracer et1("Stage 5");
// test numerical recipes in C interface
DiagonalMatrix D(10);
D << 1 << 4 << 6 << 2 << 1 << 6 << 4 << 7 << 3 << 1;
ColumnVector C(10);
C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
RowVector R(6);
R << 2 << 3 << 5 << 7 << 11 << 13;
nricMatrix M(10, 6);
DCR( D.nric(), C.nric(), 10, R.nric(), 6, M.nric() );
M -= D * C * R; Print(M);
D.ReSize(5);
D << 1.25 << 4.75 << 9.5 << 1.25 << 3.75;
C.ReSize(5);
C << 1.5 << 7.5 << 4.25 << 0.0 << 7.25;
R.ReSize(9);
R << 2.5 << 3.25 << 5.5 << 7 << 11.25 << 13.5 << 0.0 << 1.5 << 3.5;
Matrix MX = D * C * R;
M.ReSize(MX);
DCR( D.nric(), C.nric(), 5, R.nric(), 9, M.nric() );
M -= MX; Print(M);
// test swap
nricMatrix A(3,4); nricMatrix B(4,5);
A.Row(1) << 2 << 7 << 3 << 6;
A.Row(2) << 6 << 2 << 5 << 9;
A.Row(3) << 1 << 0 << 1 << 6;
B.Row(1) << 2 << 8 << 4 << 5 << 3;
B.Row(2) << 1 << 7 << 5 << 3 << 9;
B.Row(3) << 7 << 8 << 2 << 1 << 6;
B.Row(4) << 5 << 2 << 9 << 0 << 9;
nricMatrix A1(1,2); nricMatrix B1;
nricMatrix X(3,5); Matrix X1 = A * B;
swap(A, A1); swap(B1, B);
for (int i = 1; i <= 3; ++i) for (int j = 1; j <= 5; ++j)
{
X.nric()[i][j] = 0.0;
for (int k = 1; k <= 4; ++k)
X.nric()[i][j] += A1.nric()[i][k] * B1.nric()[k][j];
}
X1 -= X; Print(X1);
}
#endif
{
Tracer et1("Stage 6");
// test dotproduct
DiagonalMatrix test(5); test = 1;
ColumnVector C(10);
C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
RowVector R(10);
R << 2 << 3 << 5 << 7 << 11 << 13 << -3 << -4 << 2 << 4;
test(1) = (R * C).AsScalar() - DotProduct(C, R);
test(2) = C.SumSquare() - DotProduct(C, C);
test(3) = 6.0 * (C.t() * R.t()).AsScalar() - DotProduct(2.0 * C, 3.0 * R);
Matrix MC = C.AsMatrix(2,5), MR = R.AsMatrix(5,2);
test(4) = DotProduct(MC, MR) - (R * C).AsScalar();
UpperTriangularMatrix UT(5);
UT << 3 << 5 << 2 << 1 << 7
<< 1 << 1 << 8 << 2
<< 7 << 0 << 1
<< 3 << 5
<< 6;
LowerTriangularMatrix LT(5);
LT << 5
<< 2 << 3
<< 1 << 0 << 7
<< 9 << 8 << 1 << 2
<< 0 << 2 << 1 << 9 << 2;
test(5) = DotProduct(UT, LT) - Sum(SP(UT, LT));
Print(test);
// check row-wise load;
LowerTriangularMatrix LT1(5);
LT1.Row(1) << 5;
LT1.Row(2) << 2 << 3;
LT1.Row(3) << 1 << 0 << 7;
LT1.Row(4) << 9 << 8 << 1 << 2;
LT1.Row(5) << 0 << 2 << 1 << 9 << 2;
Matrix M = LT1 - LT; Print(M);
// check solution with identity matrix
IdentityMatrix IM(5); IM *= 2;
LinearEquationSolver LES1(IM);
LowerTriangularMatrix LTX = LES1.i() * LT;
M = LTX * 2 - LT; Print(M);
DiagonalMatrix D = IM;
LinearEquationSolver LES2(IM);
LTX = LES2.i() * LT;
M = LTX * 2 - LT; Print(M);
UpperTriangularMatrix UTX = LES1.i() * UT;
M = UTX * 2 - UT; Print(M);
UTX = LES2.i() * UT;
M = UTX * 2 - UT; Print(M);
}
{
Tracer et1("Stage 7");
// Some more GenericMatrix stuff with *= |= &=
// but don't any additional checks
BandMatrix BM1(6,2,3);
BM1.Row(1) << 3 << 8 << 4 << 1;
BM1.Row(2) << 5 << 1 << 9 << 7 << 2;
BM1.Row(3) << 1 << 0 << 6 << 3 << 1 << 3;
BM1.Row(4) << 4 << 2 << 5 << 2 << 4;
BM1.Row(5) << 3 << 3 << 9 << 1;
BM1.Row(6) << 4 << 2 << 9;
BandMatrix BM2(6,1,1);
BM2.Row(1) << 2.5 << 7.5;
BM2.Row(2) << 1.5 << 3.0 << 8.5;
BM2.Row(3) << 6.0 << 6.5 << 7.0;
BM2.Row(4) << 2.5 << 2.0 << 8.0;
BM2.Row(5) << 0.5 << 4.5 << 3.5;
BM2.Row(6) << 9.5 << 7.5;
Matrix RM1 = BM1, RM2 = BM2;
Matrix X;
GenericMatrix GRM1 = RM1, GBM1 = BM1, GRM2 = RM2, GBM2 = BM2;
Matrix Z(6,0); Z = 5; Print(Z);
GRM1 |= Z; GBM1 |= Z; GRM2 &= Z.t(); GBM2 &= Z.t();
X = GRM1 - BM1; Print(X); X = GBM1 - BM1; Print(X);
X = GRM2 - BM2; Print(X); X = GBM2 - BM2; Print(X);
GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
GRM1 *= GRM2; GBM1 *= GBM2;
X = GRM1 - BM1 * BM2; Print(X);
X = RM1 * RM2 - GBM1; Print(X);
GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
GRM1 *= GBM2; GBM1 *= GRM2; // Bs and Rs swapped on LHS
X = GRM1 - BM1 * BM2; Print(X);
X = RM1 * RM2 - GBM1; Print(X);
X = BM1.t(); BandMatrix BM1X = BM1.t();
GRM1 = RM1; X -= GRM1.t(); Print(X); X = BM1X - BM1.t(); Print(X);
// check that linear equation solver works with Identity Matrix
IdentityMatrix IM(6); IM *= 2;
GBM1 = BM1; GBM1 *= 4; GRM1 = RM1; GRM1 *= 4;
DiagonalMatrix D = IM;
LinearEquationSolver LES1(D);
BandMatrix BX;
BX = LES1.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
LinearEquationSolver LES2(IM);
BX = LES2.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
BX = D.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
BX = IM.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
BX = IM.i(); BX *= GBM1; BX -= BM1 * 2; X = BX; Print(X);
// try symmetric band matrices
SymmetricBandMatrix SBM; SBM << SP(BM1, BM1.t());
SBM << IM.i() * SBM;
X = 2 * SBM - SP(RM1, RM1.t()); Print(X);
// Do this again with more general D
D << 2.5 << 7.5 << 2 << 5 << 4.5 << 7.5;
BX = D.i() * BM1; X = BX - D.i() * RM1;
Clean(X,0.00000001); Print(X);
BX = D.i(); BX *= BM1; X = BX - D.i() * RM1;
Clean(X,0.00000001); Print(X);
SBM << SP(BM1, BM1.t());
BX = D.i() * SBM; X = BX - D.i() * SP(RM1, RM1.t());
Clean(X,0.00000001); Print(X);
// test return
BX = TestReturn(BM1); X = BX - BM1;
if (BX.BandWidth() != BM1.BandWidth()) X = 5;
Print(X);
}
// cout << "\nEnd of eighth test\n";
}
void test2(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
cout << "\n\nTest 2 - traditional, OK\n";
// traditional sum of squares and products method of calculation
// with subtraction of means - less subject to round-off error
// than test1
// make matrix of predictor values
Matrix X(nobs,npred);
// load x1 and x2 into X
// [use << rather than = when loading arrays]
X.Column(1) << x1; X.Column(2) << x2;
// vector of Y values
ColumnVector Y(nobs); Y << y;
// make vector of 1s
ColumnVector Ones(nobs); Ones = 1.0;
// calculate means (averages) of x1 and x2 [ .t() takes transpose]
RowVector M = Ones.t() * X / nobs;
// and subtract means from x1 and x1
Matrix XC(nobs,npred);
XC = X - Ones * M;
// do the same to Y [use Sum to get sum of elements]
ColumnVector YC(nobs);
Real m = Sum(Y) / nobs; YC = Y - Ones * m;
// form sum of squares and product matrix
// [use << rather than = for copying Matrix into SymmetricMatrix]
SymmetricMatrix SSQ; SSQ << XC.t() * XC;
// calculate estimate
// [bracket last two terms to force this multiplication first]
// [ .i() means inverse, but inverse is not explicity calculated]
ColumnVector A = SSQ.i() * (XC.t() * YC);
// calculate estimate of constant term
// [AsScalar converts 1x1 matrix to Real]
Real a = m - (M * A).AsScalar();
// Get variances of estimates from diagonal elements of inverse of SSQ
// [ we are taking inverse of SSQ - we need it for finding D ]
Matrix ISSQ = SSQ.i(); DiagonalMatrix D; D << ISSQ;
ColumnVector V = D.AsColumn();
Real v = 1.0/nobs + (M * ISSQ * M.t()).AsScalar();
// for calc variance of const
// Calculate fitted values and residuals
int npred1 = npred+1;
ColumnVector Fitted = X * A + a;
ColumnVector Residual = Y - Fitted;
Real ResVar = Residual.SumSquare() / (nobs-npred1);
// Get diagonals of Hat matrix (an expensive way of doing this)
Matrix X1(nobs,npred1); X1.Column(1)<<Ones; X1.Columns(2,npred1)<<X;
DiagonalMatrix Hat; Hat << X1 * (X1.t() * X1).i() * X1.t();
// print out answers
cout << "\nEstimates and their standard errors\n\n";
cout.setf(ios::fixed, ios::floatfield);
cout << setw(11) << setprecision(5) << a << " ";
cout << setw(11) << setprecision(5) << sqrt(v*ResVar) << endl;
// make vector of standard errors
ColumnVector SE(npred);
for (int i=1; i<=npred; i++) SE(i) = sqrt(V(i)*ResVar);
// use concatenation function to form matrix and use matrix print
// to get two columns
cout << setw(11) << setprecision(5) << (A | SE) << endl;
cout << "\nObservations, fitted value, residual value, hat value\n";
cout << setw(9) << setprecision(3) <<
(X | Y | Fitted | Residual | Hat.AsColumn());
cout << "\n\n";
}
LP_InteriorPointSolver::Result LP_InteriorPointSolver::Solve()
{
if (verbose>=1) cout << "Solving LP_InteriorPoint:" << endl;
if (x0.n == 0) {
if (! FindFeasiblePoint()) {
if(verbose>=1) cout << "Couldn't find an initial feasible point in LP_InteriorPointSolver::solve()" << endl;
return Infeasible;
}
}
if (verbose>=2) { cout << "x0 = "<<VectorPrinter(x0)<<endl; }
// Make sure starting xopt is always set to x0 when we begin to solve.
xopt = x0;
// Already checked that the initial point is feasible.
//
// Algorithm parameters
//
// t - indicates how far the search is along the "central path"; basically,
// it just trades off how much we weight the goal vs. the inequality barriers.
double t=0.01;
// mu - multiple by which we increase t at each outer iteration.
double mu=15;
// alpha - ? - in backtracking line search, I think this indicates a goal decrement or something.
// beta - in backtracking line search, how much we back off at each iteration
double alpha=0.2;
double beta=0.5;
//
// Outer Loop
//
DiagonalMatrix d;
Matrix Hsub,H;
Vector g;
Vector dx;
Vector xcur;
//cout<<"A is "<<endl<<MatrixPrinter(A)<<endl;
//cout<<"b is "<<VectorPrinter(b)<<endl;
//getchar();
double gap=1.0;
int iter_outer=0;
while (gap>tol_outer) {
if(verbose >= 2) { cout << "Current xopt = "<<VectorPrinter(xopt)<<endl; }
++iter_outer;
if (iter_outer > kMaxIters_Outer) {
cout << "WARNING: LP_InteriorPoint outer loop did not converge within max iters" << endl;
return MaxItersReached;
}
// Update position along the central path
t *= mu;
if (verbose>=2) cout << " Outer iter " << iter_outer << ", t=" << t << endl;
//
// Inner Loop
//
double dec=1.0;
int iter_inner=0;
while ((fabs(dec)>tol_inner) && (iter_inner <= kMaxIters_Inner)) {
++iter_inner;
/*
if (iter_inner > kMaxIters_Inner) {
cout << "WARNING: LP_InteriorPoint inner loop did not converge within max iters" << endl;
return MaxItersReached;
}
*/
if (verbose>=3) cout << " Inner iter " << iter_inner << ", dec=" << dec << endl;
// (1) Compute the direction to descend
// - Newton direction is dx = -(Hessian^-1)*(gradient)
// - H = (Aineq'*(diag(d.^2))*Aineq)
// - g = (t*f) + (Aineq'*d)
//cout << "Size of Aineq is " << A.m << " x " << A.n << endl;
//d = (bineq-Aineq*xopt)^-1 component-wise
A.mul(xopt,d); d.inplaceNegative(); d += p;
d.inplacePseudoInverse();
//Hsub = diag(d)*Aineq
d.preMultiply(A,Hsub);
//H = Hsub'*Hsub
H.mulTransposeA(Hsub,Hsub);
//g = Aineq'*d+t*c
A.mulTranspose(d,g);
g.madd(c,t);
Matrix Htemp=H;
Vector gtemp=g;
// Solve the system to find Newton direction
// Try using a better numerically conditioned matrix
HConditioner Hinv(Htemp,gtemp);
//if(!Hinv.Solve_SVD(dx)) {
if(!Hinv.Solve_Cholesky(dx)) {
RobustSVD<Real> svd;
if(svd.set(Hinv.A)) {
svd.backSub(Hinv.b,dx);
if(verbose >= 1) cout<<"Solved by SVD"<<endl;
if(verbose >= 2) {
cout<<"Singular values "<<svd.svd.W<<endl;
cout<<"b "<<Hinv.b<<endl;
getchar();
}
}
else {
if(verbose >= 1) {
//cout<<"H Matrix "<<endl; H.print();
//cout<<"Scale is "; Hinv.S.print();
cout<<"Scaled H Matrix "<<endl<<MatrixPrinter(Hinv.A)<<endl;
cout<<"LP: Error performing H^-1*g!"<<endl;
}
return Error;
}
}
Hinv.Post(dx);
//getchar();
dx.inplaceNegative();
/*
// TEMPORARY!!!
cout << "H" << endl; H.print();
cout << "d" << endl; d.print();
cout << "g" << endl; g.print();
cout << "dx" << endl; dx.print();
*/
// (2) Compute decrement (a scalar) -> dec=abs(-g'*dx)
dec = dot(g,dx);
// (3) Line search along dx, using backtracking
double s=1.0;
Real obji_xopt = Objective_Ineq(xopt,t);
//cout<<"Starting search at "<< Objective(xopt)<<", "; xopt.print();
//cout<<" direction "; dx.print();
//can find max on s outside of the loop
{
Real smax = 2;
for(int i=0;i<A.m;i++) {
Real r = p(i) - A.dotRow(i,xopt);
Real q = A.dotRow(i,dx);
Assert(r >= Zero);
if(q > Zero)
smax = Min(smax,r/q);
}
Assert(smax >= 0.0);
if(smax <= 1.0)
s = smax*0.99;
}
int iter_backtrack=0;
bool done_backtrack=false;
while (! done_backtrack) {
++iter_backtrack;
if (verbose>=4) cout << " Backtrack iter " << iter_backtrack << ", s=" << s << ", dec=" << dec << endl;
xcur = xopt; xcur.madd(dx,s);
bool sat_ineq=SatisfiesInequalities(xcur);
//inequalities will be satisfied unless there's some numerical error
/*
if(!sat_ineq) {
Real maxViolation=0;
for(int i=0;i<A.m;i++) {
Real r = b(i) - A.dotRow(i,xcur);
maxViolation = Max(maxViolation,-r);
}
if(!FuzzyZero(maxViolation)) {
cout<<"WHAT?!?! s is "<<s<<" on iter "<<iter_backtrack<<endl;
cout<<"Maximum violation is "<<maxViolation<<endl;
cout<<"xcur "; xcur.print();
cout<<"dx "; dx.print();
getchar();
}
}
*/
if (sat_ineq && (Objective_Ineq(xcur,t)<=(obji_xopt+(alpha*s*dec)))) {
xopt=xcur;
done_backtrack = true;
} else {
s *= beta;
if(s < 1e-10) {
if(verbose >= 2) cout<<"s is really small, breaking"<<endl;
done_backtrack = true;
}
}
}
if (verbose>=3) cout << " Inner iter " << iter_inner << ", obj=" << Objective(xopt) << ", s=" << s << ", dec=" << dec << ", gap=" << ((double) p.n) / t << endl;
//if (verbose>=4) cout << " Backtrack iter " << iter_backtrack << ", s=" << s << ", dec=" << dec << endl;
/*
while ((f_objective(x+(s*dx),f,Aineq,bineq,t) > (f_objective(x,f,Aineq,bineq,t)+(alpha*s*g'*dx))) | (max((Aineq*(x+(s*dx)))-bineq) > 0))
iter_bt = iter_bt + 1;
if (diagnostics) disp([' Backtrack iter ' num2str(iter_bt)]); end
s = beta*s;
%if (max((A*(x+(s*dx)))>b) > 0)
% disp('Error: current iteration of x is not feasible when backtracking.');
% return;
%end
end
% (4) Update x with the value obtained from backtracking.
x = x + (s*dx);
*/
if (!IsInf(objectiveBreak) && (Objective(xopt) < objectiveBreak)) return SubOptimal;
if(s < 1e-10) break;
}
gap = ((double) p.n) / t;
}
if (verbose>=1) cout << "Optimization was successful." << endl;
// If we were going to stop when the objective is negative, check that the
// objective actually _became_ negative!
if (!IsInf(objectiveBreak) && (Objective(xopt) >= objectiveBreak)) {
if(verbose>=1) cout<<"The max objective is not achieved! "<<Objective(xopt)<<endl;
return OptimalNoBreak;
}
else return Optimal;
}
//==========================================================================
void make_implicit_svd(vector<vector<double> >& mat,
vector<double>& b, double& sigma_min)
//==========================================================================
{
int rows = (int)mat.size();
int cols = (int)mat[0].size();
// cout << "Rows = " << rows << endl
// << "Cols = " << cols << endl;
Matrix nmat;
nmat.ReSize(rows, cols);
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
nmat.element(i, j) = mat[i][j];
}
}
// Check if mat has enough rows. If not fill out with zeros.
if (rows < cols) {
RowVector zero(cols);
zero = 0.0; // Initializes zero to a null-vector.
for (int i = rows; i < cols; ++i) {
nmat &= zero; // & means horizontal concatenation in newmat
}
}
// Perform SVD.
// cout << "Running SVD..." << endl;
static DiagonalMatrix diag;
static Matrix V;
Try {
SVD(nmat, diag, nmat, V);
} CatchAll {
cout << Exception::what() << endl;
b = vector<double>(cols, 0.0);
sigma_min = -1.0;
return;
}
// // Write out singular values.
// cout << "Singular values:" << endl;
// for (int ik = 0; ik < cols; ik++)
// cout << ik << "\t" << diag.element(ik, ik) << endl;
// // Write out info about singular values
// double s_min = diag.element(cols-1, cols-1);
// double s_max = diag.element(0, 0);
// cout << "Implicitization:" << endl
// << "s_min = " << s_min << endl
// << "s_max = " << s_max << endl
// << "Ratio of s_min/s_max = " << s_min/s_max << endl;
// // Find square sum of singular values
// double sum = 0.0;
// for (int i = 0; i < cols; i++)
// sum += diag.element(i, i) * diag.element(i, i);
// sum = sqrt(sum);
// cout << "Square sum = " << sum << endl;
// Get the appropriate null-vector and corresponding singular value
const double eps = 1.0e-15;
double tol = cols * fabs(diag.element(0, 0)) * eps;
int nullvec = 0;
for (int i = 0; i < cols-1; ++i) {
if (fabs(diag.element(i, i)) > tol) {
++nullvec;
}
}
sigma_min = diag.element(nullvec, nullvec);
// cout << "Null-vector: " << nullvec << endl
// << "sigma_min = " << sigma_min << endl;
// Set the coefficients
b.resize(cols);
for (int jk = 0; jk < cols; ++jk)
b[jk] = V.element(jk, nullvec);
return;
}
void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A,
Matrix& V, bool eivec)
{
Real epsilon = FloatingPointPrecision::Epsilon();
Tracer et("Jacobi");
REPORT
int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.resize(n); A = X;
if (eivec) { REPORT V.resize(n,n); D = 1.0; V = D; }
B << A; D = B; Z = 0.0; A.Inject(Z);
bool converged = false;
for (int i=1; i<=50; i++)
{
Real sm=0.0; Real* a = A.Store(); int p = A.Storage();
while (p--) sm += fabs(*a++); // have previously zeroed diags
if (sm==0.0) { REPORT converged = true; break; }
Real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store();
for (p = 0; p < n; p++)
{
Real* ap1 = a + (p*(p+1))/2;
Real& zp = Z.element(p); Real& dp = D.element(p);
for (int q = p+1; q < n; q++)
{
Real* ap = ap1; Real* aq = a + (q*(q+1))/2;
Real& zq = Z.element(q); Real& dq = D.element(q);
Real& apq = A.element(q,p);
Real g = 100 * fabs(apq); Real adp = fabs(dp); Real adq = fabs(dq);
if (i>4 && g < epsilon*adp && g < epsilon*adq) { REPORT apq = 0.0; }
else if (fabs(apq) > tresh)
{
REPORT
Real t; Real h = dq - dp; Real ah = fabs(h);
if (g < epsilon*ah) { REPORT t = apq / h; }
else
{
REPORT
Real theta = 0.5 * h / apq;
t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) );
if (theta<0.0) { REPORT t = -t; }
}
Real c = 1.0 / sqrt(1.0 + square(t)); Real s = t * c;
Real tau = s / (1.0 + c); h = t * apq;
zp -= h; zq += h; dp -= h; dq += h; apq = 0.0;
int j = p;
while (j--)
{
g = *ap; h = *aq;
*ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
}
int ip = p+1; j = q-ip; ap += ip++; aq++;
while (j--)
{
g = *ap; h = *aq;
*ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
ap += ip++;
}
if (q < n-1) // last loop is non-empty
{
int iq = q+1; j = n-iq; ap += ip++; aq += iq++;
for (;;)
{
g = *ap; h = *aq;
*ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau);
if (!(--j)) break;
ap += ip++; aq += iq++;
}
}
if (eivec)
{
REPORT
RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q);
Rotate(VP, VQ, tau, s);
}
}
}
}
B = B + Z; D = B; Z = 0.0;
}
if (!converged) Throw(ConvergenceException(X));
if (eivec) SortSV(D, V, true);
else SortAscending(D);
}
/*
* Fits a weighted cubic regression on predictor(s)
*
* @param contrast - want to predict this value per snp
* @param strength - covariate of choice
* @param weights - weight of data points for this genotype
* @param Predictor - output, prediction function coefficients
* @param Predicted - output, predicted contrast per snp
*/
void
FitWeightedCubic(const std::vector<double> &contrast,
const std::vector<double> &strength,
const std::vector<double> &weights,
std::vector<double> &Predictor,
std::vector<double> &Predicted) {
// Singular value decomposition method
unsigned int i;
unsigned int nobs;
unsigned int npred;
npred = 3+1;
nobs= contrast.size();
// convert double into doubles to match newmat
vector<Real> tmp_vec(nobs);
Real* tmp_ptr = &tmp_vec[0];
vector<Real> obs_vec(nobs);
Real *obs_ptr = &obs_vec[0];
vector<Real> weight_vec(nobs);
Matrix covarMat(nobs,npred);
ColumnVector observedVec(nobs);
// fill in the data
// modified by weights
for (i=0; i<nobs; i++)
weight_vec[i] = sqrt(weights[i]);
// load data - 1s into col 1 of matrix
for (i=0; i<nobs; i++)
tmp_vec[i] = weight_vec[i];
covarMat.Column(1) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(2) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(3) << tmp_ptr;
for (i=0; i<nobs; i++)
tmp_vec[i] *= strength[i];
covarMat.Column(4) << tmp_ptr;
for (i=0; i<nobs; i++)
obs_vec[i] = contrast[i]*weight_vec[i];
observedVec << obs_ptr;
// do SVD
Matrix U, V;
DiagonalMatrix D;
ColumnVector Fitted(nobs);
ColumnVector A(npred);
SVD(covarMat,D,U,V);
Fitted = U.t() * observedVec;
A = V * ( D.i() * Fitted );
// this predicts "0" for low weights
// because of weighted regression
Fitted = U * Fitted;
// this is the predictor
Predictor.resize(npred);
for (i=0; i<npred; i++)
Predictor[i] = A.element(i);
// export data back to doubles
// and therefore this predicts "0" for low-weighted points
// which is >not< the desired outcome!!!!
// instead we need to predict all points at once
// >unweighted< as output
vector<double> Goofy;
Predicted.resize(nobs);
for (i = 0; i < nobs; ++i) {
Goofy.resize(npred);
Goofy[0] = 1;
Goofy[1] = strength[i];
Goofy[2] = strength[i]*Goofy[1];
Goofy[3] = strength[i]*Goofy[2];
Predicted[i] = vprod(Goofy,Predictor);
}
}
void NonLinearLeastSquares::GetHatDiagonal(DiagonalMatrix& Hat) const
{
Hat.ReSize(n_obs);
for (int i = 1; i<=n_obs; i++) Hat(i) = X.Row(i).SumSquare();
}