estimation.cpp

linux的gps应用

#pragma ident "$Id: Estimation.cpp 185 2006-10-05 18:21:39Z btolman $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S. 
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software. 
//
//Pursuant to DoD Directive 523024 
//
// DISTRIBUTION STATEMENT A: This software has been approved for public 
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file Estimation.cpp
 * Solve estimation problem using linearized least squares, part of program DDBase.
 */

//------------------------------------------------------------------------------------
// TD Estimation.cpp SRIF convergence parameters -> input parameters
// TD Estimation.cpp For L3 : average DD WL range-phase to solve for widelane bias,
// TD Estimation.cpp  use this to solve for biases, then allow fixing of biases.
// TD Estimation.cpp Need to account for signs in single diff
// TD Estimation.cpp Singular problems in Solve

//------------------------------------------------------------------------------------
// system includes

// GPSTk
#include "Vector.hpp"
#include "Matrix.hpp"
#include "Namelist.hpp"
#include "SRIFilter.hpp"
#include "PreciseRange.hpp"
#include "Stats.hpp"
#include "RobustStats.hpp"
#include "geometry.hpp"

// DDBase
#include "DDBase.hpp"
#include "index.hpp"

//------------------------------------------------------------------------------------
using namespace std;
using namespace gpstk;

//------------------------------------------------------------------------------------
// prototypes -- this module only
   // called by ConfigureEstimation(), which is Configure(3)
void DefineStateVector(void);
void DefineLSProblem(void);
string ComposeName(const string& site1, const string& site2,
                   const GSatID& sat1, const GSatID& sat2);
string ComposeName(const DDid& ddid);
void DecomposeName(const string& label, string& site1, string& site2,
                    GSatID& sat1, GSatID& sat2);
   // called by Estimation() -- inside the loop
int EditDDdata(int n);
int ModifyState(int n);
int InitializeEstimator(void);
int aPrioriConstraints(void);
int FillDataVector(int count);
void StochasticModel(int count, Namelist& DNL, Matrix<double>& MCov);
void EvaluateLSEquation(Vector<double>& X,Vector<double>& f,Matrix<double>& P);
int MeasurementUpdate(Matrix<double>& P, Vector<double>& f, Matrix<double>& MC);
int Solve(void);
int UpdateNominalState(void);
void OutputIterationResults(bool final);
int IterationControl(int iter_n);
void OutputFinalResults(int iret);
double RMSResidualOfFit(int N, Vector<double>& dX, bool final=false);

//------------------------------------------------------------------------------------
// local data
static int N,M;                    // lengths of state and data
static Namelist StateNL;           // state vector namelist
static Vector<double> State;       // state vector
static Vector<double> dX;          // update to state vector
static Matrix<double> Cov;         // covariance matrix
static Namelist DataNL;            // data vector namelist
static Vector<double> Data;        // data vector
static Matrix<double> MeasCov;     // measurement covariance matrix
static Matrix<double> Partials;    // partials matrix
static bool Biasfix;               // if true, fix estimated biases and solve for
                                   // position states only -- NB used widely!
static SRIFilter srif;             // square root information filter for least squares
static double small,big;           // condition number in inversion = big/small
static int NEp,nDD;                // counters used in LS problem
static int Mmax;                   // largest M (data size) encountered
static int NState;                 // true length of the state vector
static Vector<double> BiasState;   // save the solution for biases, before bias fixing
static Matrix<double> BiasCov;     // save covariance for biases, before bias fixing
static Vector<double> NominalState;// save the nominal state to output with solution

//------------------------------------------------------------------------------------
// currently the estimation problem is designed like this:
// start with state of length np
// loop over data epochs
//    fill data vector for this epoch, length nd
//    fill measurement covariance matrix, nd x nd
//    compute partials and nominal data vector, P(nd x np), f(nd)
//    update srif with P(nd x np), data - f (nd), mc(nd x nd)
// end loop over data epochs
// invert to get solution
//
// inupt batch size : number of epochs / batch (nepb)
// 
// start with state of length np, nepb
// loop over data epochs
//    fill a data vector for this epoch, length nd
//    fill a measurement covariance matrix for this epoch, nd x nd
//    compute a partials and a nominal data vector, P(nd x np), f(nd)
//    add to current totals:  PP = PP && P   ff = ff && f
//     PP =  (P1)  ff = (f1)  MC = (mc1)  0    0
//           (P2)       (f2)         0  (mc2)  0
//           (P3)       (f3)         0    0  (mc3)
//    (PP grows only in rows, MC grows in rows and columns)
//    also fill in correlation (off-diagonal) parts of MC
//    if(its the end || nepb has been reached)
//        update srif with PP, Data-ff and MC
//        if(its the end) break
//        optional - invert to get solution
//        clear out PP, ff, MC
//    endif
// end loop over data epochs
// invert to get solution
int Estimation(void)
{
try {
   if(CI.Verbose) oflog << "BEGIN Estimation()" << endl;
   if(CI.noEstimate) {
      oflog << "Option --noEstimate was chosen .. terminate.\n";
      return 0;
   }
   if(CI.Screen) cout << "BEGIN Estimation..." << endl;

   bool final=false;
   int iret,n,curr;
   Vector<double> NomData,RHS;

      // iterative loop for linearized least squares estimation
   for(n=0; ; n++) {

      if(CI.Verbose) oflog << "BEGIN LLS Iteration #" << n+1
         << "------------------------------------------------------------------\n";
      if(CI.Screen) cout << "BEGIN LLS Iteration #" << n+1
         << "------------------------------------------------------------------\n";

         // edit DD data
      if((iret = EditDDdata(n))) break;

         // modify state - e.g. fix biases:
         // if user has chosen to fix biases, Biasfix is set true on last iteration
      if((iret = ModifyState(n))) break;

         //
      if((iret = InitializeEstimator())) break;

         //
      if((iret = aPrioriConstraints())) break;

         // ------------------ loop over epochs in the DD buffers
         // build the data vector and Namelist
         // build the measurement covariance matrix
         // update the SRI filter
      curr = -1;           // current value of count
      NEp = nDD = 0;
      while(1) {
         curr++;
         if(curr > maxCount) break;

            // this needed by EvaluateLSEquation, and is used in output
         SolutionEpoch = FirstEpoch + curr*CI.DataInterval;

            // get the data and the data namelist
         M = FillDataVector(curr);
            // no data -- but don't assume this implies the last epoch
         if(M == 0) continue;
         nDD += M;

            // compute the measurement covariance matrix
         StochasticModel(curr,DataNL,MeasCov);

            // get nominal data = NomData(nominal state) and partials
            // NB position components of state not used in here..
         EvaluateLSEquation(State,NomData,Partials);

         if(CI.Debug)
            oflog << "EvaluateLSEquation returns vector\n" << fixed
            << setw(8) << setprecision(3) << NomData
            << "\n diff with data " << setw(8) << setprecision(3) << (Data-NomData)
            << "\n partials matrix\n" << setw(8) << setprecision(3) << Partials
            << "\n State\n" << setw(8) << setprecision(3) << State << endl;

         RHS = Data - NomData;              // RHS of linearized LS equation

            // update the SRI filter
         if((iret = MeasurementUpdate(Partials,RHS,MeasCov))) break;

         NEp++;

      }  // end while loop over data epochs
      if(iret) break;

      if((iret = Solve())) break;

      if((iret = UpdateNominalState())) break;

      // return -1  quit now
      //         0   go on
      //         1   reached convergence and don't fix biases
      //         2   reached last iteration and don't fix biases
      //         4   1 and/or 2 and fix biases, i.e. fix the biases then quit
      iret = IterationControl(n+1);

      oflog << endl;

      //if(iret == -1) {        // biases have been fixed
      //   iret = 0;
      //   break;
      //}
      //else if(iret == 4)      // one more, with biases fixed
      //   final = true;
      //else if(iret) {           // quit now
      //   iret = 0;
      //   break;
      //}
      if(iret && iret != 4) final = true;

      OutputIterationResults(final);

      if(iret && iret != 4) {
         iret = 0;
         break;
      }

   }  // end iterative loop

   // iret is -2 (singular) or 0

   OutputFinalResults(iret);

   return iret;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}   // end Estimation()

//------------------------------------------------------------------------------------
// called by Configure(3)
int ConfigureEstimation(void)
{
try {
   if(CI.Verbose) oflog << "BEGIN ConfigureEstimation()" << endl;

      // find the mean time, get Earth orientation parameters
   MedianEpoch = FirstEpoch;
   MedianEpoch += (LastEpoch-FirstEpoch)/2.;
   eorient = EOPList.getEOP(MedianEpoch);
   if(CI.Verbose) {
      oflog << "Earth orientation parameters at median time " << MedianEpoch << " :"
            << endl << "  xp, yp, UT1mUTC*Wearth (all radians) =" << fixed
            << " " << setprecision(9) << eorient.xp*DEG_TO_RAD/3600.0
            << ", " << setprecision(9) << eorient.yp*DEG_TO_RAD/3600.0
            << ", " << setprecision(9) << eorient.UT1mUTC * 7.2921151467e-5 << endl;
   }

      // define the initial State vector
   DefineStateVector();

      // Configure the SRIF for the estimation
   DefineLSProblem();

      // initial value
   Biasfix = false;

   return 0;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// called by ConfigureEstimation()
void DefineStateVector(void)
{
try {
      // set up the names of the state vector
      // set up the initial value of the nominal state
      // State = offset from nominal position, stored in Stations[].pos
      // plus offset from nominal biases
      // NB biases MUST be last, after all other states. This b/c inside
      // LSIterationLoop(), dX will be State - bias states when Biasfix == true

   int i;

   // add position states for all the non-fixed stations
   // add residual zenith delay states (per site)
   map<string,Station>::const_iterator it;
   for(it=Stations.begin(); it != Stations.end(); it++) {
      if(!(it->second.fixed)) {
         StateNL += it->first + string("-X");
         StateNL += it->first + string("-Y");
         StateNL += it->first + string("-Z");
      }
      if(CI.NRZDintervals > 0) {
         for(i=0; i<CI.NRZDintervals; i++) {
            StateNL += it->first + string("-RZD") + StringUtils::asString(i);
         }
      }
   }

   // add bias states
   map<DDid,DDData>::iterator jt;
   for(jt=DDDataMap.begin(); jt != DDDataMap.end(); jt++) {
      // += adds it only if it is unique..
      StateNL += ComposeName(jt->first);
   }

   // temp - sanity check
   for(int i=0; i<StateNL.size(); i++) {
      string site1,site2;
      GSatID sat1,sat2;
      DecomposeName(StateNL.getName(i), site1, site2, sat1, sat2);
      oflog << "State name (" << setw(2) << i << ") decomposes as "
         << site1 << " " << site2 << " " << sat1 << " " << sat2;

         // interpret it
      oflog << " [ " << site1;
      if(site2 == string("X") || site2 == string("Y") || site2 == string("Z")) {
         oflog << " : " << site2 << "-component position";
      }
      else if(site2.substr(0,3) == "RZD") {
         oflog << " : trop delay #" << site2.substr(3,site2.size()-3);
      }
      else if(Stations.find(site2) != Stations.end() &&
              sat1.id != -1 &&
              sat2.id != -1) {
         oflog << " " << site2 << " " << sat1 << " " << sat2 << " : bias";
      }
      else
         oflog << " : unknown!";

      oflog << " ]" << endl;
   }

   // dimensions
   // state N, data M, NState=N but if biases are fixed N=non-bias states only
   // State and StateNL always has length NState
   // actual state may shrink to N when biases fixed,
   // but then LSIterationLoop() uses temporaries
   NState = StateNL.size();
   State = Vector<double>(NState,0.0);
   Mmax = DDDataMap.size();            // the largest M (Data.size()) could be

}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// called by ConfigureEstimation()
void DefineLSProblem(void)
{
try {
      // define the least squares processor
   srif.iterationsLimit = CI.nIter;
   srif.convergenceLimit = CI.convergence;
   srif.divergenceLimit = 1.e10;    // TD input parameter
   srif.doWeight = false;
   srif.doRobust = false;
   srif.doLinearize = false;
   srif.doSequential = true;

}