matrixtest.cpp

GPSTK:做gpS的人都应当知道这个东西

#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)