tsrif.cpp

1 gps基本算法

      if(test == 16) {
         srif.doRobust = true;
         srif.doWeight = false;
         srif.convergenceLimit = 1.e-2;
      }
      // ---------- OR weight it
      if(test == 15) {
         srif.doRobust = false;
         srif.doWeight = true;
         // compute mest of y15 to get weights
         double median,mad,mest;
         mad = Robust::MAD(y15,M,median,true);
         mest = Robust::MEstimate(y15,M,median,mad,w15);
         cov.resize(M,M);
         ident(cov);
         for(i=0; i<M; i++) cov(i,i) = 1.0/(w15[i]*w15[i]); // meas. cov.
         //Robust::StemLeafPlot(cout, y15, M, "raw dd phase data");
      }

      // initialize for doit()
      sol = 0.0;
      batch = 0;     // for LSFunc
      for(i=0; i<M; i++) {
         f(i) = double(n15[i]);     // f used as a temp, for PolyFit PF
         data(i) = y15[i];
      }

      // first compute regular polynomial fit to data, and compute statistics
      PF.Reset(3);
      PF.Add(data,f);
      res = data - PF.Evaluate(f);
      stat.Reset();
      stat.Add(res);
      cout << " Initial raw statistics on residuals of fit:\n "
         << scientific << setprecision(3) << stat << endl;

      // least squares
      doIt(data,sol,cov);

      // compute final weighted stats on residuals of fit
      LSFunc(sol,f,partials);    // f will be the fit evaluted at each point
      if(test == 15) {
         res = data; // leastSquares returns residuals in data Vector
         for(i=0; i<M; i++) {
            data(i)=y15[i];
            wt(i)=w15[i];
         }
      }
      if(test == 16) {
         wt = data;  // leastSquares(robust) returns weights in data Vec
         for(i=0; i<M; i++) data(i)=y15[i];
         res = data-f;
      }

      //cout << "Weights: " << wt << endl;
      stat.Reset();
      stat.Add(res,wt);
      cout << " Final weighted statistics on residuals of fit:\n "
         << scientific << setprecision(3) << stat << endl;

      // plot
      string filename;
      filename = "tsrif" + StringUtils::asString(test) + ".dat";
      ofstream ofs(filename.c_str(),ios::out);
      if(!ofs) {
         cout << " Could not open " << filename << " .. abort plot\n";
      }
      else {
         for(i=0; i<M; i++) {
            ofs << fixed << setw(4) << n15[i]
               << " " << f63 << data[i]
               << " " << f63 << f[i]
               << " " << f63 << res[i]
               << " " << f63 << wt[i]
               << " " << f63 << fabs(res[i])/stat.StdDev()
               << endl;
         }
         ofs.close();
         cout << " Created " << filename << endl;
         filename = "tsrif" + StringUtils::asString(test) + ".gp";
         ofs.open(filename.c_str(),ios::out);
         if(!ofs) {
            cout << " Could not open " << filename << " .. abort plot\n";
         }
         else {
            if(test == 15) ofs << "set title \"DD Phase data - wt'd fit of order "
               << N-1 << " tsrif(15," << dataset << ")\\n(wts from m-est of data)\"\n";
            if(test == 16) ofs << "set title \"DD Phase data - robust fit of order "
               << N-1 << " tsrif(16," << dataset << ")\"\n";
            ofs << "set xlabel \"Count\"\n";
            ofs << "set ylabel \"DDPhase(m)\"\n";
            ofs << "unset mouse\n";
#ifndef _WIN32
            ofs << "set term x11 enhanced font \"luxi sans,17\"\n";  // linux only
#endif
            ofs << "set key bottom right\n";
            ofs << "set autoscale y2\n";
            ofs << "set ytics nomirror\n";
            ofs << "set y2tics\n";
            if(test == 15) ofs << "set y2label \"Residual (m)\"\n";
            if(test == 16) ofs << "set y2label \"Weight\"\n";
            ofs << "#set xrange [40:60]\n";
            ofs << "#set yrange [250:290]\n";
            ofs << "plot \"tsrif" << test
               << ".dat\" using 1:2 t \"DDPh\" with points\n";
            ofs << "replot \"tsrif" << test
               << ".dat\" using 1:3 t \"fit\" with lines\n";
            ofs << "replot \"tsrif" << test
               << ".dat\" using 1:4 axes x1y2 t \"res\" with linespoints\n";
            ofs << "replot \"tsrif" << test
               << ".dat\" using 1:5 axes x1y2 t \"wt\" with linespoints\n";
            ofs.close();
            cout << "\n Created file tsrif" << test
               << ".gp .. try gnuplot tsrif" << test << ".gp\n";
         }
      }

      if(test == 16) {
         QSort(&wt[0],M);
         Robust::StemLeafPlot(cout, &wt[0], M, "weights");
      }
   }
   else {
      cout << " ... not implemented\n";
   }

   return 0;
}
catch(gpstk::Exception& e) {
   cerr << "tsrif caught an exception\n" << e << endl;
   return 0;
}
}

//------------------------------------------------------------------------------------
void doIt(Vector<double>& data, Vector<double>& sol, Matrix<double>& cov)
{
try {
   int i,pre=6,wid=12;
   format fmts(wid,pre,2);

   srif.doVerbose = verbose;
   cout << " Start at x = (" << fixed;
   for(i=0; i<sol.size(); i++) cout << (i==0 ? "":",") << sol[i];
         cout << ")" << endl;
   //cout << " Data is (" << data.size() << "):" << setprecision(pre) << data << endl;

   i = srif.leastSquaresEstimation(data,sol,cov,&LSFunc);
   if(i) cout << " LS failed (" << i << ") "
         << (i==-1 ? "Underdetermined" :
            (i==-2 ? "Singular" : 
            (i==-3 ? "Failed to converge" : "Diverged")))
         << endl;
   cout << " SRIFilter is" << (srif.isValid() ? "":" not") << " valid" << endl;

   Namelist NL=srif.getNames();
   LabelledVector LT(NL,truth);
   LT.setw(wid).setprecision(pre);
   LT.message(" Truth:");
   cout << LT << endl;
   Vector<double> delta(sol-truth);
   LabelledVector LR(NL,delta);
   LR.setw(wid).setprecision(pre);
   LR.message(" Residuals:");
   cout << LR << endl;
   cout << " RMS residuals of fit: " << fmts << RMS(data) << endl;
   LabelledMatrix LC(NL,cov);
   LC.setw(wid).setprecision(pre);
   LC.message(" Covariance:");
   cout << LC << endl;
   cout << " Condition number is " << fmts << srif.ConditionNumber() << endl;
   if(srif.doLinearize || srif.doRobust) {
      cout << " There were " << srif.Iterations() << " iterations,";
      cout << " and convergence was " << fmts << srif.Convergence() << endl;
   }
   //cout << " Data residuals (" << data.size() << ") : " << scientific
      //<< setprecision(pre) << data << endl;
}
catch(gpstk::Exception& e) { GPSTK_RETHROW(e); }
}

//------------------------------------------------------------------------------------
void LSFunc(Vector<double>& X, Vector<double>& f, Matrix<double>& P)
{
try {
   double t;
   if(test == 1) {
      // f(X) = [ x0 + x1*t + x2*t*t]
      // partials = [ 1  t  t*t ]
      // t = i * 0.32;
      int M=1;
      for(int i=0; i<batch; i++) {
         t = i * 0.32;
         f(M*i) = 0.0;  // its linear X(0) + X(1)*t + X(2)*t*t;
         P(M*i,0)=1.0;  P(M*i,1)=t;  P(M*i,2)=t*t;
      }
   }
   else if(test == 2) {
      // f(X) = [ x0*sin(t) + sin(x1)*cos(t) ]
      // partials = [ sin(t)   cos(x1)*cos(t) ]
      // t = i * 0.32;
      int M=1;
      for(int i=0; i<batch; i++) {
         t = i * 0.32;
         f(M*i) = X(0)*sin(t) + sin(X(1))*cos(t);
         P(M*i,0)=sin(t);  P(M*i,1)=cos(X(1))*cos(t);
      }
   }
   else if(test == 3) {
      //        [ cos(x1)*sin(t) - 10*sin(x2)*cos(t)  ]
      // f(X) = [ 2*sin(x0)*cos(t) + 4*cos(x3)        ]
      //        [ x0*sin(x1)*t*t - x2*cos(x3)*tan(t)  ]
      //
      // partials = 
      // [ 0              -sin(x1)*sin(t)  -10*cos(x2)*cos(t) 0              ]
      // [ 2*cos(x0)*cos(t)  0               0                -4*sin(x3)     ]
      // [ sin(x1)*t*t    cos(x1)*t*t      -cos(x3)*tan(t)    sin(x3)*tan(t) ]
      //
      int M=3;
      for(int i=0; i<batch; i++) {     // loop over batch
         t = i * 0.32;
         f(M*i)   = cos(X(1))*sin(t) - 10*sin(X(2))*cos(t);
         f(M*i+1) = 2*sin(X(0))*cos(t) + 4*cos(X(3));
         f(M*i+2) = X(0)*sin(X(1))*t*t - X(2)*cos(X(3))*tan(t);

         P(M*i+0,0)=0.0;
         P(M*i+0,1)=-sin(X(1))*sin(t);
         P(M*i+0,2)=-10.0*cos(X(2))*cos(t);
         P(M*i+0,3)=0.0;

         P(M*i+1,0)=2*cos(X(0))*cos(t);
         P(M*i+1,1)=0.0;
         P(M*i+1,2)=0.0;
         P(M*i+1,3)=4.0;

         P(M*i+2,0)=sin(X(1))*t*t;
         P(M*i+2,1)=X(0)*cos(X(1))*t*t;
         P(M*i+2,2)=-cos(X(3))*tan(t);
         P(M*i+2,3)=X(2)*sin(X(3))*tan(t);
      }
   }
   else if(test == 4) {
      double x,y,z,r,tb,th,ph;
      for(int i=0; i<batch; i++) {     // loop over batch
         t = i * 0.32;
         for(int j=0; j<4; j++) {      // loop over satellites
            th = Theta[j]+OmegaTheta[j]*t;
            ph = Phi[j]+OmegaPhi[j]*t;
            x = RSV[j]*sin(th)*sin(ph);
            y = RSV[j]*sin(th)*cos(ph);
            z = RSV[j]*cos(th);
            tb = Bias[j];  //+Drift[j]*t;
            r = sqrt( (X(0)-x)*(X(0)-x) + (X(1)-y)*(X(1)-y) + (X(2)-z)*(X(2)-z) );
            f(4*i+j) = r-(tb-X(3));
            P(4*i+j,0) = (X(0)-x)/r;
            P(4*i+j,1) = (X(1)-y)/r;
            P(4*i+j,2) = (X(2)-z)/r;
            P(4*i+j,3) = 1.0;
         }
      }
   }
   else if(test == 5) {
      double x,y,z,r,tb,th,ph;
      t = batch * 0.32;
      for(int j=0; j<4; j++) {      // loop over satellites
         th = Theta[j]+OmegaTheta[j]*t;
         ph = Phi[j]+OmegaPhi[j]*t;
         x = RSV[j]*sin(th)*sin(ph);
         y = RSV[j]*sin(th)*cos(ph);
         z = RSV[j]*cos(th);
         tb = Bias[j];  //+Drift[j]*t;
         r = sqrt( (X(0)-x)*(X(0)-x) + (X(1)-y)*(X(1)-y) + (X(2)-z)*(X(2)-z) );
         f(j) = r-(tb-X(3));
         P(j,0) = (X(0)-x)/r;
         P(j,1) = (X(1)-y)/r;
         P(j,2) = (X(2)-z)/r;
         P(j,3) = 1.0;
      }
   }
   else if(test == 6) {
      // P6 is random but fixed
      Vector<double> f4;
      f4 = P6*X;
      for(int i=0; i<batch; i++) {
         for(int j=0; j<4; j++) {
            f(4*i+j) = f4(j);
            for(int k=0; k<4; k++) P(4*i+j,k) = P6(j,k);
         }
      }
   }
   else if(test == 7) {
      P = P6;
      f = P6*X;
   }
   else if(test == 8) {
         // log(L) = log(x0) + x1*log(n) + x2*log(w) + x3*log(d) + x4*log(D)
      for(int i=0; i<f.size(); i++) {
         f(i) = 0.0;
         P(i,0) = 1.0;
         P(i,1) = log(n8[i]);
         P(i,2) = log(w8[i]);
         P(i,3) = log(d8[i]);
         P(i,4) = log(D8[i]);
      }
   }
   else if(test == 9) {
         // f(x) = exp( x0 + x1*log(n) + x2*log(w) + x3*log(d) + x4*log(D) ) = L
      for(int i=0; i<f.size(); i++) {
         f(i) = exp(X(0) + X(1)*log(n8[i]) + X(2)*log(w8[i]) + X(3)*log(d8[i])
                         + X(4)*log(D8[i]));
         P(i,0) = f(i);
         P(i,1) = f(i)*log(n8[i]);
         P(i,2) = f(i)*log(w8[i]);
         P(i,3) = f(i)*log(d8[i]);
         P(i,4) = f(i)*log(D8[i]);
      }
   }
   else if(test == 10) {
      for(int i=0; i<f.size(); i++) {
         f(i) = sqrt((X(0)-x10[i])*(X(0)-x10[i]) + (X(1)-y10[i])*(X(1)-y10[i]));
         P(i,0) = (X(0)-x10[i])/f(i);
         P(i,1) = (X(1)-y10[i])/f(i);
      }
   }
   else if(test == 11 || test == 12) {
      double t;
      for(int i=0; i<batchlen; i++) {
         t = (years11[batch*batchsize+i] - 1856.0)/144.;
         P(i,0) = 1.0;
         for(int j=1; j<X.size(); j++) P(i,j) = P(i,j-1)*t;
      }
      f = P * X;
   }
   else if(test == 13 || test == 14) {
      P = 0.0;
      int n;
      for(int i=0; i<batchlen; i++) {
         n = (batch*batchsize+i)/(batchsize/X.size());
         P(i,n) = 1.0;
      }
      f = P * X;
   }
   else if(test == 15 || test == 16) {
      for(int i=0; i<batchlen; i++) {
         t = n15[batch*batchsize+i] - n15[0];
         P(i,0) = 1.0;
         for(int j=1; j<X.size(); j++) P(i,j) = P(i,j-1)*t;
      }
      f = P * X;
   }
}
catch(gpstk::Exception& e) { GPSTK_RETHROW(e); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------