sri.cpp

linux的gps应用

   void SRI::reshape(const Namelist& NL)
      throw(MatrixException,VectorException)
   {
      try {
         if(names == NL) return;
         Namelist keep(names);
         keep &= NL;                // keep only those in both names and NL
         //Namelist drop(names);    // (drop is unneeded - split would do it)
         //drop ^= keep;            // lose those in names but not in keep
         Namelist add(NL);
         add ^= keep;               // add those in NL but not in keep
         SRI Sdrop;                 // make a new SRI to hold the losers
         // would like to allow caller access to Sdrop..
         split(keep,Sdrop);         // split off only the losers
                                    // NB names = drop | keep; drop & keep is empty
         *this += add;              // add the new ones
         this->permute(NL);         // permute it to match NL
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // merge this SRI with the given input SRI. ? should this be operator&=() ?
   // NB may reorder the names in the resulting Namelist.
   SRI& SRI::operator+=(const SRI& S)
      throw(MatrixException,VectorException)
   {
      try {
         Namelist all(names);
         all |= S.names;      // assumes Namelist::op|= adds unique S.names to _end_

         //all.sort();        // TEMP - for testing with old version

            // stack the (R|Z)'s from both in one matrix;
            // all determines the columns, plus last column is for Z
         unsigned int i,j,k,n,m,sm;
         n = all.labels.size();
         m = R.rows();
         sm = S.R.rows();
         Matrix<double> A(m+sm,n+1,0.0);

            // copy R into A, permuting columns as names differs from all
            // loop over columns of R; do Z at the same time using j=row
         for(j=0; j<m; j++) {
               // find where this column of R goes in A
               // (should never throw..)
            k = all.index(names.labels[j]);
            if(k == -1) {
               MatrixException me("Algorithm error 1");
               GPSTK_THROW(me);
            }

               // copy this col of R into A (R is UT)
            for(i=0; i<=j; i++) A(i,k) = R(i,j);
               // also the jth element of Z
            A(j,n) = Z(j);
         }
            // now do the same for S, but put S.R|S.Z below R|Z
         for(j=0; j<sm; j++) {
            k = all.index(S.names.labels[j]);
            if(k == -1) {
               MatrixException me("Algorithm error 2");
               GPSTK_THROW(me);
            }
            for(i=0; i<=j; i++) A(m+i,k) = S.R(i,j);
            A(m+j,n) = S.Z(j);
         }

            // now triangularize A and pull out the new R and Z
         Householder<double> HA;
         HA(A);
         // submatrix args are matrix,toprow,topcol,numrows,numcols
         R = Matrix<double>(HA.A,0,0,n,n);
         Vector<double> T = Vector<double>(HA.A.colCopy(n));
         Z = Vector<double>(T,0,n);
         names = all;

         return *this;
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // merge two SRIs to produce a third. ? should this be operator&() ?
   SRI operator+(const SRI& Sleft,
                 const SRI& Sright)
      throw(MatrixException,VectorException)
   {
      try {
         SRI S(Sleft);
         S += Sright;
         return S;
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // Zero out the nth row of R and the nth element of Z, removing all
   // information about that element.
   void SRI::zeroOne(const unsigned int n)
      throw()
   {
      if(n >= R.rows())
         return;

      //TD this is not right -- you should permute the element
      //to the first row, then zero
      for(unsigned int j=n; j<R.cols(); j++) 
         R(n,j) = 0.0;
      Z(n) = 0.0;
   }

   // --------------------------------------------------------------------------------
   // Zero out all the first n rows of R and elements of Z, removing all
   // information about those elements. Default value of the input is 0,
   // meaning zero out the entire SRI.
   void SRI::zeroAll(const int n)
      throw()
   {
      if(n <= 0) {
         R = 0.0;
         Z = 0.0;
         return;
      }

      if(n >= R.rows())
         return;

      for(unsigned int i=0; i<n; i++) {
         for(unsigned int j=i; j<R.cols(); j++) 
            R(i,j) = 0.0;
         Z(i) = 0.0;
      }
   }

   // --------------------------------------------------------------------------------
   // Shift the state vector by a constant vector X0; does not change information
   // i.e. let R * X = Z => R * (X-X0) = Z'
   // throw on invalid input dimension
   void SRI::shift(const Vector<double>& X0)
      throw(MatrixException)
   {
      if(X0.size() != R.cols()) {
         using namespace StringUtils;
         
         MatrixException me("Invalid input dimension: SRI has dimension "
               + asString<int>(R.rows()) + " while input has length "
               + asString<int>(X0.size())
               );
         GPSTK_THROW(me);
      }
      Z = Z - R * X0;
   }

   // --------------------------------------------------------------------------------
   // Transform this SRI with the transformation matrix T;
   // i.e. R -> T * R * inverse(T) and Z -> T * Z. The matrix inverse(T)
   // may optionally be supplied as input, otherwise it is computed from
   // T. NB names in this SRI are most likely changed; but this routine does
   // not change the Namelist. Throw MatrixException if the input has
   // the wrong dimension or cannot be inverted.
   void SRI::transform(const Matrix<double>& T,
                       const Matrix<double>& invT)
      throw(MatrixException,VectorException)
   {
      if(T.rows() != R.rows() ||
         T.cols() != R.cols() ||
         (&invT != &SRINullMatrix && (invT.rows() != R.rows() ||
         invT.cols() != R.cols()))) {
            using namespace StringUtils;
         
            MatrixException me("Invalid input dimension:\n  SRI has dimension "
               + asString<int>(R.rows()) + " while T has dimension "
               + asString<int>(T.rows()) + "x"
               + asString<int>(T.cols()));
            if(&invT != &SRINullMatrix) me.addText("\n  and invT has dimension "
                           + asString<int>(invT.rows()) + "x"
                           + asString<int>(invT.cols()));
            GPSTK_THROW(me);
      }

      try {
            // get the inverse matrix
         Matrix<double> Ti(T);
         if(&invT == &SRINullMatrix)
            Ti = inverseSVD(T);
         else
            Ti = invT;

            // transform
         Matrix<double> B = T * R * Ti;
         Vector<double> Q = T * Z;

            // re-triangularize
         R = 0.0;
         Z = 0.0;
         SrifMU(R,Z,B,Q);
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // Transform the state by the transformation matrix T; i.e. X -> T*X,
   // without transforming the SRI; this is done by right multiplying R by
   // inverse(T), which is the input. Thus R -> R*inverse(T),
   // so R*inverse(T)*T*X = Z.  Input is the _inverse_ of the transformation.
   // throw MatrixException if input dimensions are wrong.
   void SRI::transformState(const Matrix<double>& invT)
      throw(MatrixException)
   {
      if(invT.rows() != R.rows() || invT.cols() != R.rows()) {
         using namespace StringUtils;
         MatrixException me("Invalid input dimension: SRI has dimension "
            + asString<int>(R.rows()) + " while invT has dimension "
            + asString<int>(invT.rows()) + "x"
            + asString<int>(invT.cols()));
         GPSTK_THROW(me);
      }

         // transform
      Matrix<double> A = R * invT;
         // re-triangularize
      Householder<double> HA;
      HA(A);
      R = HA.A;
   }

   // --------------------------------------------------------------------------------
   // Decrease the information in this SRI for, or 'Q bump', the element
   // with the input index.  This means that the uncertainty and the state
   // element given by the index are divided by the input factor q; the
   // default input is zero, which means zero out the information (q = infinite).
   // A Q bump by factor q is equivalent to 'de-weighting' the element by q.
   // No effect if input index is out of range.
   //
   // Use a specialized form of the time update, with Phi=unity, G(N x 1) = 0
   // except 1 for the element (in) getting bumped, and Rw(1 x 1) = 1 / q.
   // Note that this bump of the covariance for element k results in
   // Cov(k,k) += q (plus, not times!).
   // if q is 0, replace q with 1/q, i.e. lose all information, covariance
   // goes singular; this is equivalent to (1) permute so that the 'in'
   // element is first, (2) zero out the first row of R and the first element
   // of Z, (3) permute the first row back to in.
   void SRI::Qbump(const unsigned int& in,
                   const double& q)
      throw(MatrixException,VectorException)
   {
      try {
         if(in >= R.rows()) return;
         double factor=0.0;
         if(q != 0.0) factor=1.0/q;

         unsigned int ns=1,i,j,n=R.rows();

         Matrix<double> A(n+ns,n+ns+1,0.0), G(n,ns,0.0);
         A(0,0) = factor;           // Rw, dimension ns x ns = 1 x 1
         G(in,0) = 1.0;
         G = R * G;                 // R*Phi*G
         for(i=0; i<n; i++) {
            A(ns+i,0) = -G(i,0);               //     A =   Rw       0       zw=0
            for(j=0; j<n; j++)                 //          -R*Phi*G  R*Phi   Z
               if(i<=j) A(ns+i,ns+j) = R(i,j); //
            A(ns+i,ns+n) = Z(i);
         }

            // triangularize and pull out the new R and Z
         Householder<double> HA;                //    A  =  Rw  Rwx  zw
         HA(A);                                 //          0    R   z
         R = Matrix<double>(HA.A,ns,ns,n,n);
         Vector<double> T=HA.A.colCopy(ns+n);
         Z = Vector<double>(T,ns,n);
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // Fix the state element with the input index to the input value, and
   // collapse the SRI by removing that element.
   // No effect if index is out of range.
   void SRI::biasFix(const unsigned int& in,
                     const double& bias)
      throw(MatrixException,VectorException)
   {
      if(in >= R.rows()) return;

      try {
         unsigned int i,j,ii,jj,n=R.rows();
         Vector<double> Znew(n-1,0.0);
         Matrix<double> Rnew(n-1,n-1,0.0);
            // move the X(in) terms to the data vector on the RHS
         for(i=0; i<=in; i++) Z(i) -= R(i,in)*bias;
         for(i=0,ii=0; i<=n; i++) {
            if(i == in) continue;
            Znew(ii) = Z(i);
            for(j=i,jj=ii; j<n; j++) {
               if(j == in) continue;
               Rnew(ii,jj) = R(i,j);