#include "Vector.hpp"#include "Matrix.hpp"#include "PolyFit.hpp"#include "Stats.hpp"#include <iostream>#include <iomanip>#include <fstream>   // for copyfmt/** * @file MatrixTest.cpp * */using namespace std;int ReadMatrix(gpstk::Matrix<double>& M, string& file){   ifstream infile(file.c_str());   if(!infile) return -1;   enum Type{GEN,LOW,UPT,DIA,SYM,SQU};   string labels[6]={"GEN","LOW","UPT","DIA","SYM","SQU"};   bool dimmed=false;   char ch;   int r=0,c=0,i=0,j=0;   string buffer;   Type type=GEN;   while(infile >> buffer) {      if(buffer[0] == '#') {        // skip to end of line         while(infile.get(ch)) { if(ch=='\n') break; }      }      else {         string::size_type in;         if((in=buffer.find("r=")) != string::npos) {            r = strtol(buffer.substr(in+2).c_str(),0,10);            //cout << "Found r = " << r << endl;            if(type != GEN) c=r;         }         else if((in=buffer.find("c=")) != string::npos) {            c = strtol(buffer.substr(in+2).c_str(),0,10);            //cout << "Found c = " << c << endl;            if(type != GEN) r=c;         }         else if((in=buffer.find("t=")) != string::npos) {            if((in=buffer.find("LOW")) != string::npos) type=LOW;            if((in=buffer.find("UPT")) != string::npos) type=UPT;            if((in=buffer.find("DIA")) != string::npos) type=DIA;            if((in=buffer.find("SYM")) != string::npos) type=SYM;            if((in=buffer.find("SQU")) != string::npos) type=SQU;            //cout << "Found type " << labels[type] << endl;         }         else if(buffer == string(":::")) {            if(r*c == 0) return 0;            M = gpstk::Matrix<double>(r,c,double(0));            dimmed = true;         }         else {            if(!dimmed) continue;            M(i,j) = strtod(buffer.c_str(),0);            j++;            switch(type) {               case LOW: if(j>i) { i++; j=0; } break;               case UPT: if(j>=c) { i++; j=i; } break;               case DIA: i=j; break;               case SYM: M(j-1,i) = M(i,j-1); if(j>i) { i++; j=0; } break;               case SQU: case GEN: default: if(j>=c) { i++; j=0; } break;            }         }      }   }   infile.close();   if(!dimmed) return -2;   return 0;}void VectorTest(void){   cout << "\n -------------- Vector Test ---------------------------------\n";   gpstk::Vector<double> V(10);   V += 3.1415;   cout << "V = " << V << endl;   double dat[10]={1.1,2.2,3.3,4.4,5.5,6.6,7.7,8.8,9.9,10.1};   V = dat;   cout << "V = " << V << endl;   //cout << "Median of V " << median(V) << endl;   cout << "V min " << min(V) << ", max " << max(V) << ", sum " << sum(V) << endl;      // slice is (init,number,stride)   gpstk::VectorSlice<double> Vodd(V,std::slice(0,5,2));   gpstk::ConstVectorSlice<double> Veve(V,std::slice(1,5,2));   cout << "Vodd = " << Vodd << endl;   cout << "Veve = " << Veve << endl;   Vodd[1] = Vodd[3] = 0;   //Veve[1] = Veve[4] = 99; compiler won't let you have Veve as l-value   V[3] = V[7] = 99;    // but you can change V, and that will show in Veve   cout << "Vodd = " << Vodd << endl;   cout << "Veve = " << Veve << endl;   cout << "Minkowski of Vodd and Veve " << Minkowski(Vodd,Veve) << endl;   cout << "dot of Vodd and Veve " << dot(Vodd,Veve) << endl;   cout << "V    = " << V << endl;   cout << "V min " << min(V) << ", max " << max(V) << ", sum " << sum(V) << ", norm " << norm(V) << endl;   gpstk::Vector<double> W=V;   V.zeroTolerance = 1.e-15;   cout << "Zero tolerance for V is " << V.zeroTolerance << endl;   V.zeroTolerance = 1.e-5;   cout << "Zero tolerance for W is " << W.zeroTolerance << endl;   W += V;   cout << "Here is W the usual way :\n";   cout << ' ' << fixed << setfill('.') << setw(8) << setprecision(3) << W << endl;   cout << "Here is W the saved way :\n";   cout << fixed << setfill('.') << setw(8) << setprecision(3);   ofstream savefmt;   savefmt.copyfmt(cout);   for(int i=0; i<W.size(); i++) {      cout.copyfmt(savefmt);      cout << W[i];   }   cout << setfill(' ') << endl;   gpstk::Vector<double> Sum;   Sum = V + W;   cout << "Sum = " << setw(8) << Sum << endl;   gpstk::ConstVectorSlice<double> CVS(V,std::slice(0,V.size(),1));   cout << "CVS = " << setw(13) << CVS << endl;}void MatrixTest1(int argc, char **argv){   cout << "\n -------------- Matrix Test 1 ---------------------------------\n";   gpstk::Matrix<double> MD(2,5);   double dat[10]={1.1,2.2,3.3,4.4,5.5,6.6,7.7,8.8,9.9,10.1};   MD = dat;   cout << "Matrix (" << MD.rows() << "," << MD.cols() << ") from double* :\n" << fixed << setprecision(1) << setw(5) << MD << endl;   string filename(argv[5]);   gpstk::Matrix<double> MF;   int iret=ReadMatrix(MF,filename);   if(iret < 0) {      cerr << "Error: could not open file " << filename << endl;      return;   }   //cout << "Matrix (" << MF.rows() << "," << MF.cols() << ") from file :\n" << fixed << setprecision(1) << setw(7) << MF << endl;      // pick off last column   gpstk::Vector<double> B(MF.colCopy(MF.cols()-1));      // copy all but last column   gpstk::Matrix<double> A(MF,0,0,MF.rows(),MF.cols()-1);   cout << "Partials Matrix (" << A.rows() << "," << A.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << A << endl;   cout << "Data vector (" << B.size() << ") :\n" << fixed << setprecision(3) << setw(10) << B << endl;      // take ATA, then invert   gpstk::Matrix<double> AT,ATA;   AT = transpose(A);   //cout << "Transposed matrix (" << AT.rows() << "," << AT.cols() << ") :\n" << fixed << setprecision(1) << setw(7) << AT << endl;   ATA = AT * A;   //cout << "ATA matrix (" << ATA.rows() << "," << ATA.cols() << ") :\n" << fixed << setprecision(3) << setw(13) << ATA << endl;   gpstk::Matrix<double> Ainv;   try { Ainv=inverse(ATA); }   catch (gpstk::Exception& e) { cout << e << endl; return;}   cout << "Covariance matrix (" << Ainv.rows() << "," << Ainv.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << Ainv << endl;   gpstk::Vector<double> Sol;   Sol = Ainv * AT * B;   cout << "Solution vector (" << Sol.size() << ") :\n" << fixed << setprecision(3) << setw(10) << Sol << endl;   gpstk::Vector<double> Two;   Two = B - A*Sol;   cout << "Residual vector (" << Two.size() << ") :\n" << scientific << setprecision(3) << setw(10) << Two << endl;   Two = Sol + Sol;   cout << "2*Solution vector (" << Two.size() << ") :\n" << fixed << setprecision(3) << setw(10) << Two << endl;   gpstk::Matrix<double> TA;   TA = A + A;   cout << "2*Partials Matrix (" << TA.rows() << "," << TA.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << TA << endl;   int i;   gpstk::Vector<double> c(8),b(7);   gpstk::Matrix<double> W;   for(i=0; i<8; i++) c(i)=3*i;   for(i=0; i<7; i++) b(i)=i+1;   W = outer(c,b);   cout << "Vector c(" << c.size() << ") :\n" << fixed << setprecision(3) << setw(7) << c << endl;   cout << "Vector b(" << b.size() << ") :\n" << fixed << setprecision(3) << setw(7) << b << endl;   cout << "Their outer product (" << W.rows() << "," << W.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << W << endl;   cout << "Their norms " << norm(b) << " and " << norm(c) << " and dot " << dot(c,b) << " and cosine " << cosVec(c,b) << endl;}void MatrixTest2(void){   cout << "\n -------------- Matrix Test 2 ---------------------------------\n";   for(int n=2; n<13; n++) {      double big,small,cond;      gpstk::Matrix<double> MM(n,n),Minv,Prod;      for(int i=0; i<n; i++) {         for(int j=0; j<n; j++) {            MM(i,j) = 1.0/(i+j+1.0);         }      }      cout << "Tough matrix (" << MM.rows() << "," << MM.cols() << ") :\n" << fixed << setprecision(6) << setw(9) << MM << endl;      Minv = inverse(MM);      try { Minv=inverse(MM); }      catch (gpstk::Exception& e) { cout << e << endl; break;}      cond = condNum(MM,big,small);      cout << "Condition number for " << n << " is " << fixed << setprecision(3) << big << "/" << scientific << setprecision(3) << small;      if(small > 0.0) cout << " = " << setprecision(3) << big/small;      cout << endl;      cout << "Inverse matrix (" << Minv.rows() << "," << Minv.cols() << ") :\n" << fixed << setprecision(3) << setw(10+(n>5?n:0)) << Minv << endl;      Prod = Minv * MM;      gpstk::Vector<double>V;      V.zeroTolerance = 1.e-3;      Prod.zeroize();      cout << "Unity matrix (" << Prod.rows() << "," << Prod.cols() << ") ? :\n" << fixed << setprecision(9) << setw(12) << Prod << endl;   }}void MatrixTest3(int argc, char **argv){   cout << "\n -------------- Matrix Test 3 ---------------------------------\n";   cout << "Read and print matrix from file\n";   double cond,big,small;   gpstk::Matrix<double> A,Ainv,P;   for(int i=1; i<argc; i++) {      string filename=argv[i];      cout << "File " << filename;      int iret=ReadMatrix(A,filename);      cout << " (" << iret << ") Matrix(" << A.rows() << "," << A.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << A << endl;      if(A.rows() != A.cols()) {         gpstk::Matrix<double> AT;         AT = gpstk::transpose(A);         A = AT*A;         cout << " ATA Matrix(" << A.rows() << "," << A.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << A << endl;      }      cond = condNum(A,big,small);      cout << "Condition number is " << fixed << setprecision(3) << big << "/" << scientific << setprecision(3) << small;      cout << " = " << fixed << big/small << endl;      try { Ainv=gpstk::inverse(A); }      catch (gpstk::Exception& e) { cout << e << endl; continue;}      cout << "Inverse matrix (" << Ainv.rows() << "," << Ainv.cols() << ") :\n" << fixed << setprecision(3) << setw(10) << Ainv << endl;      P = Ainv * A;      cout << "Unity matrix (" << P.rows() << "," << P.cols() << ") ? :\n" << fixed << setprecision(9) << setw(12) << P << endl;   }}void MatrixTest4(void){   cout << "\n -------------- Matrix Test 4 ---------------------------------\n";   const int N=7; // rows = columns   double mat[N*N]={       // storage by columns      8.317,  6.212,  2.574,  5.317,  2.080, -9.133, -2.755,      0.212,  3.292,  1.574,  1.028,  3.370, -2.077, -2.739,      5.740,  1.574,  1.911,  1.390,  8.544,  8.930,  9.216,      4.317,  1.028,  1.039,  7.126,  4.512,  8.538,  5.226,      -1.109,  7.438,  7.236,  6.783,  0.356, -9.509, -0.109,      0.174,  5.408, -9.503, -6.527, -6.589, -6.375, -7.239,      1.960,  6.592,  9.440,  4.428, -4.531,  5.084,  4.296   };   double dat[N]={ 14.289,  9.284, -1.128,  8.389, -6.929,  4.664,  7.590 };   int i,j;   gpstk::Matrix<double> A(N,N);   gpstk::Vector<double> b(N);   A = mat;   A = transpose(A);   b = dat;   cout << "Matrix A(" << A.rows() << "," << A.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << A << endl;   cout << "Vector b(" << b.size() << ") :\n" << fixed << setprecision(3) << setw(7) << b << endl;   cout << "\nNow solve using SVD\n";   gpstk::SVD<double> Asvd;   gpstk::Matrix<double> U,V;   gpstk::Vector<double> S;   Asvd(A);   U = Asvd.U;   V = Asvd.V;   S = Asvd.S;   cout << "Singular Values (" << S.size() << ") :\n" << fixed << setprecision(3) << setw(7) << S << endl;   cout << "Matrix U(" << U.rows() << "," << U.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << U << endl;   cout << "Matrix V(" << V.rows() << "," << V.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << V << endl;   gpstk::Matrix<double> P,W(A);   W = 0.0;   for(i=0; i<S.size(); i++) W(i,i)=S(i);   P = U * W * transpose(V);   P = P - A;   P.zeroize();   cout << "Difference (" << P.rows() << "," << P.cols() << ") :\n" << scientific << setprecision(3) << setw(10) << P << endl;   cout << "Determinant of A = " << scientific << setprecision(3) << Asvd.det();   double d=1;   for(i=0; i<N; i++) d *= S(i);   cout << " -- Compare to " << scientific << setprecision(3) << d << endl;   gpstk::Vector<double> X(b);   Asvd.backSub(X);   cout << "Solution via backsubstitution (" << X.size() << ") :\n" << fixed << setprecision(3) << setw(7) << X << endl;   S = b-A*X;   cout << "Solution residuals (" << S.size() << ") :\n" << scientific << setprecision(3) << setw(7) << S << endl;   cout << "\nSort in ascending order\n";   Asvd.sort(false);   U = Asvd.U;   V = Asvd.V;   S = Asvd.S;   cout << "Singular Values (" << S.size() << ") :\n" << fixed << setprecision(3) << setw(7) << S << endl;   cout << "Matrix U(" << U.rows() << "," << U.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << U << endl;   cout << "Matrix V(" << V.rows() << "," << V.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << V << endl;   // now chop off the last column of A and repeat   cout << "\n\nNow reduce A by one column and repeat\n";   A = gpstk::Matrix<double>(A,0,0,A.rows(),A.cols()-1);   cout << "Matrix A(" << A.rows() << "," << A.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << A << endl;   Asvd(A);   S = Asvd.S;   U = Asvd.U;   V = Asvd.V;   cout << "Singular Values (" << S.size() << ") :\n" << fixed << setprecision(3) << setw(7) << S << endl;   cout << "Matrix U(" << U.rows() << "," << U.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << U << endl;   cout << "Matrix V(" << V.rows() << "," << V.cols() << ") :\n" << fixed << setprecision(3) << setw(7) << V << endl;   W = A;   W = 0.0;   for(i=0; i<S.size(); i++) W(i,i)=S(i);   P = U * W * transpose(V);   P = P - A;   P.zeroize();   cout << "Difference (" << P.rows() << "," << P.cols() << ") :\n" << scientific << setprecision(3) << setw(10) << P << endl;}void MatrixTest5(void)