tsrif.cpp

1 gps基本算法

      if(dataset < 1 || dataset > 4) {
         cout << "Error: -d [1234] only. Abort\n";
         return -1;
      }
   }
   else {
      cout << "Test least squares using class SRIFilter\n";
      cout << "  Usage tsrif <n> [-v] [-t tau] [-s sig] [-d n]" << endl;
      cout << "    where -v turns on verbose output inside SRIFilter" << endl;
      cout << "    and -t and -s are for choice 14 (see below)" << endl;
      cout << "    and -d is for choices 15 & 16, n=1,2,3,or4 (see below)" << endl;
      cout << "    and n is:" << endl;
      cout << "  1   simple 1-d linear problem" << endl;
      cout << "  2   simple 1-d linearized problem" << endl;
      cout << "  3   multi-dimensional non-linear problem" << endl;
      cout << "  4   a non-linear batch test; a ranging problem" << endl;
      cout << "  5   test 4 done sequentially" << endl;
      cout << "  6   a batch linear test with random partials and data" << endl;
      cout << "  7   same as 6, but sequential" << endl;
      cout << "  8   a very non-linear equation made linear" << endl;
      cout << "  9   direct linearized solution of test 8" << endl;
      cout << " 10   a 2-d ranging problem with a false minimum" << endl;
      cout << " 11   global warming trends - polynomial fit in batchs" << endl;
      cout << " 12   global warming trends - polynomial fit in one batch" << endl;
      cout << " 13   global warming trends - piecewise fit in one batch" << endl;
      cout << " 14   global warming trends - piecewise fit constrained by "
            << "Gauss-Markov process\n         characterized by sigma and tau "
            << "(input or defaults: tau=50,sig=0.1)." << endl;
      cout << " 15   weighted LS using DD phase data (for dataset n use -d n)"<< endl;
      cout << " 16   robust LS using DD phase data of problem 15" << endl;
      return 0;
   }

   cout << "\n========================= Test " << test
      <<    " =========================" << endl;
   if(test == 1) {
      cout << " linear model : \n    f(X) = [ x0 + x1*t + x2*t*t]" << endl;
      cout << " partials     : \n         = [  1 ,    t ,    t*t]" << endl;
      cout << " add 0.025 gaussian noise to data" << endl;
      M=1; batch=10; N=3;
      data.resize(M*batch);
      truth.resize(N); truth(0) = 1.0; truth(1) = 2.0; truth(2) = 4.0;

      NL += "x0"; NL += "x1"; NL += "x2";
      srif = SRIFilter(NL);
      srif.doLinearize = false;
      sol.resize(N); sol = 1.5;
      for(i=0; i<batch; i++) {
         t = i * 0.32;
         data(M*i) = RandNorm(0.025) + truth(0) + truth(1)*t + truth(2)*t*t;
      }
      doIt(data,sol,cov);
   }
   else if(test == 2) {
      cout << " linearized :\n   f(X) = [ x0*sin(t) + sin(x1)*cos(t) ]" << endl;
      cout << " partials   :\n          [    sin(t),  cos(x1)*cos(t) ]" << endl;
      cout << " add 0.025 gaussian noise to data" << endl;
      M=1; batch=20; N=2;
      data.resize(M*batch);
      truth.resize(N); truth(0) = 1.0; truth(1) = -0.5;

      NL += "x0"; NL += "x1";
      srif = SRIFilter(NL);
      srif.doLinearize = true;
      sol.resize(N); sol = -1.;
      for(i=0; i<batch; i++) {
         t = i * 0.32;
         data(M*i) = RandNorm(0.025) + truth(0)*sin(t) + sin(truth(1))*cos(t);
      }
      doIt(data,sol,cov);
   }
   else if(test == 3) {
      cout << " A multi-dimensional non-linear problem" << endl;
      cout << "        [ cos(x1)*sin(t) - 10*sin(x2)*cos(t)  ]" << endl;
      cout << " f(X) = [ 2*sin(x0)*cos(t) + 4*cos(x3)        ]" << endl;
      cout << "        [ x0*sin(x1)*t*t - x2*cos(x3)*tan(t)  ]" << endl;
      cout << " partials = " << endl;
      cout<<" [0             -sin(x1)*sin(t) -10*cos(x2)*cos(t)  0             ]"
         << endl;
      cout<<" [2*cos(x0)*cos(t) 0            0                -4*sin(x3)       ]"
         << endl;
      cout<<" [sin(x1)*t*t   x0*cos(x1)*t*t  -cos(x3)*tan(t)  x2*sin(x3)*tan(t)]"
         << endl;
      M=3; batch=10; N=4;
      data.resize(M*batch);
      sol.resize(N);
      truth.resize(N);
      truth(0) = 1.0;   sol(0) = 1.1;
      truth(1) = -2.0;  sol(1) = -2.1;
      truth(2) = 3.0;   sol(2) = 3.1;
      truth(3) = -4.0;  sol(3) = -4.1;
         // generate fake data with noise
      for(i=0; i<batch; i++) {
         t = i * 0.32;
         data(M*i)   = RandNorm(.005) + cos(truth(1))*sin(t)
                                    - 10*sin(truth(2))*cos(t);
         data(M*i+1) = RandNorm(.025) + 2*sin(truth(0))*cos(t) + 4*cos(truth(3));
         data(M*i+2) = RandNorm(.015) + truth(0)*sin(truth(1))*t*t
                                    - truth(2)*cos(truth(3))*tan(t);
      }

      NL += "x0"; NL += "x1"; NL += "x2"; NL += "x3";
      srif = SRIFilter(NL);
      srif.doLinearize = true;

      doIt(data,sol,cov);
   }
   else if(test == 4) {
      cout << " a non-linear batch test; a ranging problem" << endl;
      cout << " f(X) = [ sqrt(d0*d0 + d1*d1 + d2*d2) - d3 ]" << endl;
      cout << " where di = XSV[j][i]-xi for each of 4 sats j" << endl;
      cout << endl;
      cout << " partials = [ d0/r  d1/r  d2/r  -1 ]" << endl;
      cout << " where r = 1/sqrt(d0*d0 + d1*d1 + d2*d2)" << endl;
      cout << "" << endl;
      cout << " add gaussian noise at 0.01 to the data" << endl;
      M=4; batch=5; N=4;
      data.resize(M*batch);
      sol.resize(N);
      truth.resize(N);
      partials.resize(M*batch,N);
      truth(0) = 1.0;
      truth(1) = -2.0;
      truth(2) = 3.0;
      truth(3) = -4.0;
      sol = 0.0;
         // generate fake data with noise
      LSFunc(truth,data,partials);
      for(i=0; i<M*batch; i++) data(i) += RandNorm(0.01);

      NL += "X"; NL += "Y"; NL += "Z"; NL += "cdT";
      srif = SRIFilter(NL);
      srif.doLinearize = true;

      doIt(data,sol,cov);
   }
   else if(test == 5) {
      cout << " do test 4 again but sequentially, one batch at a time.\n";
      //cout << " add normal noise at 0.001 to the data\n";
      cout << " this problem is very sensitive to the noise on the data\n";
      cout << endl;
      cout << " NB doing this is very different than the batch solution when\n";
      cout << " doLinearize is set. In this case the solution depends strongly on\n";
      cout << " the initial nominal solution: bad X0 => bad Inf0 => bad X1 => ...\n";
      cout << " A problem like this is best handled with a very good initial X,\n";
      cout << " or a big batch for first sequential step.\n";
      cout << " model\n";
      cout << "       f(X) = [ sqrt(d0*d0 + d1*d1 + d2*d2) - d3 ]\n";
      cout << "                where di = XSV[j][i]-xi for each of 4 sats j\n";
      cout << "       partials = [ d0/r  d1/r  d2/r  -1 ]\n";
      cout << "                where r = 1/sqrt(d0*d0 + d1*d1 + d2*d2)\n";
      M=4; N=4; batch=5;
      data.resize(M);
      sol.resize(N);
      truth.resize(N);
      partials.resize(M,N);
         // true solution
      truth(0) = 1.0;
      truth(1) = -2.0;
      truth(2) = 3.0;
      truth(3) = -4.0;
         // need a good starting point - let it be solution from test 4
      sol(0) = 0.964780;
      sol(1) = -1.974532;
      sol(2) = 2.936646;
      sol(3) = -4.058416;

      NL += "X"; NL += "Y"; NL += "Z"; NL += "cT";
      srif = SRIFilter(NL);
      srif.doLinearize = true;
      srif.doSequential = true;

      for(batch=0; batch<5; batch++) {
         cout << "\n------ Batch = " << batch << " ------" << endl;
            // generate fake data with noise
         LSFunc(truth,data,partials);
         //for(i=0; i<M; i++) data(i) += RandNorm(0.001);
         doIt(data,sol,cov);
      }
   }
   else if(test == 6 || test == 7) {
      batch=5;
      M=4; N=4;
      sol.resize(N);
      truth.resize(N);
         // true solution - arbitrary: it will be used to generate data
      truth(0) = 1.0;
      truth(1) = -2.0;
      truth(2) = 3.0;
      truth(3) = -4.0;
         // P6 is used to generate data
      P6.resize(M,N);
      for(i=0; i<M; i++) for(j=0; j<N; j++) P6(i,j)=RandNorm(10.0);
      cout << " Random partials matrix P6 is\n" << P6 << endl;
         // generate fake data with noise
      D6.resize(batch*M);
      partials.resize(M*batch,N);
      i = test; test = 6; LSFunc(truth,D6,partials); test = i;
      for(i=0; i<M*batch; i++) D6(i) += RandNorm(1.0);

      NL += "A"; NL += "B"; NL += "C"; NL += "D";
      srif = SRIFilter(NL);
      srif.doLinearize = false;
      sol = 0.0;

      if(test == 6) {
         cout << " a batch linear test using random data (same as 7):\n";
         srif.doSequential = false;
         data.resize(M*batch);
         data = D6;
         doIt(data,sol,cov);
      }
      else if(test == 7) {
         cout << " a sequential linear test using random data (same as 6):\n";
         srif.doSequential = true;
         data.resize(M);
         partials.resize(M,N);
         for(batch=0; batch<5; batch++) {
            cout << "\n ------------- Batch = " << batch << " --------------" << endl;
            for(i=0; i<M; i++) data(i) = D6(batch*M+i);
            doIt(data,sol,cov);
         }
      }
   }
   else if(test == 8) {
      cout << " solve this very non-linear equation:\n";
      cout << " L = exp(x0) * n^x1 * w^x2 * d^x3 * D^x4\n";
      cout << "  by making it linear:\n";
      cout << " log(L) = x0 + x1*log(n) + x2*log(w) + x3*log(d) + x4*log(D)\n";
      cout << " (test 9 will solve the non-linear equation.)\n";
      cout << endl;
      M=50; N=5;
      data.resize(M);
      partials.resize(M,N);
      sol.resize(N);
      truth.resize(N);
      for(i=0; i<M; i++) data(i) = log(L8[i]);
      sol = 0.0;
      // I don't have the real truth...
      truth(0) = -7.254122;
      truth(1) = 1.379365;
      truth(2) = -0.480604;
      truth(3) = 0.275616;
      truth(4) = 1.213172;

      NL += "x0"; NL += "x1"; NL += "x2"; NL += "x3"; NL += "x4";
      srif = SRIFilter(NL);
      srif.doLinearize = false;
      srif.doSequential = false;

      doIt(data,sol,cov);
   }
   else if(test == 9) {
      cout << " solve this very non-linear equation:\n";
      cout << " L = exp(x0) * n^x1 * w^x2 * d^x3 * D^x4\n";
      cout << " (test 8 solves the log of this equation (linear).)\n";
      cout << endl;
      M=50; N=5;
      partials.resize(M,N);
      sol.resize(N);
      truth.resize(N);
      data.resize(M);
      data = L8;
      // I don't have the real truth...
      truth(0) = -7.254122;
      truth(1) = 1.379365;
      truth(2) = -0.480604;
      truth(3) = 0.275616;
      truth(4) = 1.213172;
      sol = truth; // 0.0;

      NL += "x0"; NL += "x1"; NL += "x2"; NL += "x3"; NL += "x4";
      srif = SRIFilter(NL);
      srif.doLinearize = true;
      srif.doSequential = false;

      doIt(data,sol,cov);
   }
   else if(test == 10) {
      cout << " a 2-d ranging problem with a false minimum.\n";
      cout << " generate data from truth, adding 0.002 gaussian noise.\n";
      cout << " look at the plot, which is the potential well.\n";
      cout << " it has a minimum at (1,1), which is the true solution,\n";
      cout << " but also another local minimum, at (2.81,2.46).\n";
      cout << " try starting a (0,0), then again starting at (4,4)\n";
      cout << endl;
      M=5; N=2;
      Vector<double> f(M),D(M);
      data.resize(M);
      truth.resize(N);
      sol.resize(N);
      partials.resize(M,N);
      truth(0) = truth(1) = 1.0;
         // generate the data from truth + noise
      LSFunc(truth,data,partials);

         // generate a contour plot of the data
      while(1) {
         double x,y,z,r;
         ofstream ofs("tsrif10.dat",ios::out);
         if(!ofs) {
            cout << " Could not open tsrif10.dat .. abort contour plot\n";
            break;
         }
         for(i=0; i<40; i++) {
            sol(0) = x = i*0.10;
            for(j=0; j<40; j++) {
               sol(1) = y = j*0.10;
               LSFunc(sol,f,partials);
               z = norm(f-data);    // ie RSS
               ofs << f63 << x << " " << f63 << y << " " << f63 << z*z << endl;
            }
            ofs << "  " << endl;    // gnuplot wants a blank line
         }
         ofs.close();
         cout << " Created file tsrif10.dat\n";

         ofs.open("tsrif10.gp",ios::out);
         if(!ofs) {
            cout << " Could not open tsrif10.gp .. abort contour plot\n";
            break;
         }
         ofs << "set title \"tsrif 10 - ranging problem with false minimum\"\n";
         ofs << "set xlabel \"X\"\n";
         ofs << "set ylabel \"Y\"\n";
         ofs << "set zlabel \"Potential = |f-d|^2\"\n";
         ofs << "#set xrange [40:60]\n";
         ofs << "#set yrange [250:290]\n";
         ofs << "#set zrange [0:12]\n";
         ofs << "#set cbrange [0:12]\n";
         ofs << "set pm3d\n";
#ifndef _WIN32
         ofs << "set term x11 enhanced font \"luxi sans,17\"\n";  // linux only
#endif
         ofs << "unset key\n";
         ofs << "set data style lines\n";
         ofs << "set contour base\n";
         ofs << "set cntrparam levels incremental 0.0,0.25,12.0\n";
         ofs << "# comment out next two to see only contours\n";
         ofs << "set hidden3d\n";
         ofs << "set view 60,45\n";
         ofs << "# un-comment out next two to see only contours\n";
         ofs << "#set nosurface\n";
         ofs << "#set view 0,0\n";
         ofs << "set label 1 \"potential well - note true minimum at (1,1) "
             << "and local minimum at (2.81,2.46)\" at screen 0.5,0.88 center\n";
         ofs << "splot \"tsrif10.dat\" using 1:2:3\n";
         ofs.close();
         cout << " Created file tsrif10.gp .. try gnuplot tsrif10.gp\n\n";
         break;
      }

      for(k=0; k<5; k++) data(k) += RandNorm(0.002);
      D = data;      // save

      NL += "X"; NL += "Y";
      srif = SRIFilter(NL);
      srif.doLinearize = true;