srifilter.cpp

1 gps基本算法

         R = Rapriori;
         Z = Zapriori;
      }

      // update information with simple MU
      if(doVerbose) {
         cout << " Meas Cov:";
         for(i=0; i<M; i++) cout << " " << MeasCov(i,i);
         cout << endl;
         cout << " Partials:\n" << Partials << endl;
      }
      if(doRobust || doWeight)
         measurementUpdate(Partials,Res,MeasCov);
      else
         measurementUpdate(Partials,Res);

      if(doVerbose) {
         cout << " Updated information matrix\n" << LabelledMatrix(names,R) << endl;
         cout << " Updated information vector\n" << LabelledVector(names,Z) << endl;
      }

      // invert
      try { getStateAndCovariance(Xsol,Cov,&small,&big); }
      catch(SingularMatrixException& sme) {
         iret = -2;
         break;
      }
      condition_number = big/small;
      if(doVerbose) {
         cout << " Condition number: " << scientific << condition_number
            << fixed << endl;
         cout << " Post-fit data residuals:  "
            << fixed << setprecision(6) << Res << endl;
      }

      // update X: when linearized, solution = dX
      if(doLinearize) {
         Xsol += NominalX;
      }
      if(doVerbose) {
         LabelledVector LXsol(names,Xsol);
         LXsol.message(" Updated X:");
         cout << LXsol << endl;
      }

      // linear non-robust is done..
      if(!doLinearize && !doRobust) break;

      // test for convergence of linearization
      if(doLinearize) {
         rms_convergence = RMS(Xsol - NominalX);
         if(doVerbose) {
            cout << " RMS convergence : "
               << scientific << rms_convergence << fixed << endl;
         }
      }

      // test for convergence of robust weighting, and compute new weights
      if(doRobust) {
         // must de-weight post-fit residuals
         LSF(Xsol,f,Partials);
         Res = D-f;

         // compute a new set of weights
         double mad,median;
         //for(mad=0.0,i=0; i<M; i++)
         //   mad += Wts(i)*Res(i)*Res(i);
         //mad = sqrt(mad)/sqrt(Robust::TuningA*(M-1));
         mad = Robust::MedianAbsoluteDeviation(&(Res[0]),Res.size(),median);

         OldWts = Wts;
         for(i=0; i<M; i++) {
            if(Res(i) < -RobustTuningT*mad)
               Wts(i) = -RobustTuningT*mad/Res(i);
            else if(Res(i) > RobustTuningT*mad)
               Wts(i) = RobustTuningT*mad/Res(i);
            else
               Wts(i) = 1.0;
         }

         // test for convergence
         rms_convergence = RMS(OldWts - Wts);
         if(doVerbose) cout << " Convergence: "
            << scientific << setprecision(3) << rms_convergence << endl;
      }

      // failures
      if(rms_convergence > divergenceLimit) iret=-4;
      if(number_iterations >= iterationsLimit) iret=-3;
      if(iret) {
         if(doSequential) {
            R = Rapriori;
            Z = Zapriori;
         }
         break; // return iret;
      }

      // success
      if(number_iterations > 1 && rms_convergence < convergenceLimit) break;

      // prepare for another iteration
      if(doLinearize)
         NominalX = Xsol;
      if(doRobust)
         NominalX = X;

   } while(1); // end iteration loop

   number_batches++;
   if(doVerbose) cout << "Return from SRIFilter::leastSquaresUpdate\n\n";

   if(iret) return iret;

   // output the solution
   Xsave = X = Xsol;

   // put residuals of fit into data vector, or weights if Robust
   if(doRobust)
      D = OldWts;
   else
      D = Res;

   valid = true;
   return iret;
}

//------------------------------------------------------------------------------------
// SRIF (Kalman) time update see SrifTU for doc.
void SRIFilter::timeUpdate(Matrix<double>& Phi,
                           Matrix<double>& Rw,
                           Matrix<double>& G,
                           Vector<double>& Zw,
                           Matrix<double>& Rwx)
   throw(MatrixException)
{
   try { SrifTU(R, Z, Phi, Rw, G, Zw, Rwx); }
   catch(MatrixException& me) { GPSTK_RETHROW(me); }
}

//------------------------------------------------------------------------------------
// SRIF (Kalman) smoother update see SrifSU for doc.
void SRIFilter::smootherUpdate(Matrix<double>& Phi,
                               Matrix<double>& Rw,
                               Matrix<double>& G,
                               Vector<double>& Zw,
                               Matrix<double>& Rwx)
   throw(MatrixException)
{
   try { SrifSU(R, Z, Phi, Rw, G, Zw, Rwx); }
   catch(MatrixException& me) { GPSTK_RETHROW(me); }
}

//------------------------------------------------------------------------------------
void SRIFilter::DMsmootherUpdate(Matrix<double>& P,
                                 Vector<double>& X,
                                 Matrix<double>& Phinv,
                                 Matrix<double>& Rw,
                                 Matrix<double>& G,
                                 Vector<double>& Zw,
                                 Matrix<double>& Rwx)
   throw(MatrixException)
{
   try { SrifSU_DM(P, X, Phinv, Rw, G, Zw, Rwx); }
   catch(MatrixException& me) { GPSTK_RETHROW(me); }
}

//------------------------------------------------------------------------------------
// output operator
ostream& operator<<(ostream& os,
                    const SRIFilter& srif)
{
   Namelist NL(srif.names);
   NL += string("State");
   Matrix<double> A;
   A = srif.R || srif.Z;
   LabelledMatrix LM(NL,A);
   LM.setw(os.width());
   LM.setprecision(os.precision());
   os << LM;
   return os;
}

//------------------------------------------------------------------------------------
// reset the computation, i.e. remove all stored information
void SRIFilter::zeroAll(void)
{
   SRI::zeroAll();
   Xsave = 0.0;
   number_batches = 0;
}

//------------------------------------------------------------------------------------
// reset the computation, i.e. remove all stored information, and
// optionally change the dimension. If N is not input, the
// dimension is not changed.
// @param N new SRIFilter dimension (optional).
void SRIFilter::Reset(const int N)
{
   if(N > 0 && N != R.rows()) {
      R.resize(N,N,0.0);
      Z.resize(N,0.0);
   }
   else
      SRI::zeroAll(N);
   if(N > 0) Xsave.resize(N);
   Xsave = 0.0;
   number_batches = 0;
}

//------------------------------------------------------------------------------------
// private beyond this
//------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------
// Kalman time update.
// This routine uses the Householder transformation to propagate the SRIFilter
// state and covariance through a time step.
// Input:
// R     A priori square root information (SRI) matrix (an n by n 
//          upper triangular matrix)
// Z     a priori SRIF state vector, of length n (state is X, Z = R*X).
// Phi   Inverse of state transition matrix, an n by n matrix.
//          Phi is destroyed on output.
// Rw    a priori square root information matrix for the process
//          noise, an ns by ns upper triangular matrix
// G     The n by ns matrix associated with process noise.  The 
//          process noise covariance is G*Q*transpose(G) where inverse(Q)
//          is transpose(Rw)*Rw.  G is destroyed on output.
// Zw    a priori 'state' associated with the process noise,
//          a vector with ns elements.  Usually set to zero by
//          the calling routine (for unbiased process noise).
// Rwx   An ns by n matrix which is set to zero by this routine 
//          but is used for output.
// 
// Output:
//    The updated square root information matrix and SRIF state (R,Z) and
// the matrices which are used in smoothing: Rw, Zw, Rwx.
// Note that Phi and G are trashed, and that Rw and Zw are modified.
// 
// Return values:
//    SrifTU returns void, but throws exceptions if the input matrices
// or vectors have incompatible dimensions or incorrect types.
// 
// Method:
//    This SRIF time update method treats the process noise and mapping
// information as a separate data equation, and applies a Householder
// transformation to the (appended) equations to solve for an updated
// state.  Thus there is another 'state' variable associated with 
// whatever state variables have process noise.  The matrix G relates
// the process noise variables to the regular state variables, and 
// appears in the term GQG(trans) of the covariance.  If all n state
// variables have process noise, then ns=n and G is an n by n matrix.
// Since some (or all) of the state variables may not have process 
// noise, ns may be zero.  [Bierman ftnt pg 122 seems to indicate that
// variables with zero process noise can be handled by ns=n & setting a
// column of G=0.  But note that the case of the matrix G=0 is the
// same as ns=0, because the first ns columns would be zero below the
// diagonal in that case anyway, so the HH transformation would be 
// null.]
//    For startup, all of the a priori information and state arrays may
// be zero.  That is, "no information" would imply that R and Z are zero,
// as well as Rw and Zw.  A priori information (covariance) and state
// are handled by setting P = inverse(R)*transpose(inverse((R)), Z = R*X.
//    There are three ways to handle non-zero process noise covariance.
// (1) If Q is the (known) a priori process noise covariance Q, then
// set Q=Rw(-1)*Rw(-T), and G=1.
// (2) Transform process noise covariance matrix to UDU form, Q=UDU,
// then set G=U  and Rw = (D)**-1/2.
// (3) Take the sqrt of process noise covariance matrix Q, then set
// G=this sqrt and Rw = 1.  [2 and 3 have been tested.]
//    The routine applies a Householder transformation to a large
// matrix formed by addending the input matricies.  Two preliminary 
// steps are to form Rd = R*Phi (stored in Phi) and -Rd*G (stored in 
// G) by matrix multiplication, and to set Rwx to the zero matrix.  
// Then the Householder transformation is applied to the following
// matrix, dimensions are shown in ():
//       _  (ns)   (n)   (1)  _          _                  _
// (ns) |    Rw     0     Zw   |   ==>  |   Rw   Rwx   Zw    |
// (n)  |  -Rd*G   Rd     Z    |   ==>  |   0     R    Z     | .
//       -                    -          -                  -
// The SRI matricies R and Rw remain upper triangular.
//
//    For the programmer:  after Rwx is set to zero, G is made into 
// -Rd*G and Phi is made into R*Phi, the transformation is applied 
// to the matrix:
//       _   (ns)   (n)   (1) _
// (ns) |    Rw    Rwx    Zw   |
// (n)  |     G    Phi    Z    |
//       -                    -
// then the (upper triangular) matrix R is copied out of Phi into R.
// -------------------------------------------------------------------
//    The matrix Rwx is related to the sensitivity of the state
// estimate to the unmodeled parameters in Zw.  The sensitivity matrix
// is          Sen = -inverse(Rw)*Rwx,
// where perturbation in model X = 
//             Sen * diagonal(a priori sigmas of parameter uncertainties).
// -------------------------------------------------------------------
//    The quantities Rw, Rwx and Zw on output are to be saved and used
// in the sqrt information fixed interval smoother (SRIS), during the
// backward filter process.
// -------------------------------------------------------------------
// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
//      Estimation," Academic Press, 1977.
// -------------------------------------------------------------------
template <class T>
void SRIFilter::SrifTU(Matrix<T>& R,
                       Vector<T>& Z,
                       Matrix<T>& Phi,
                       Matrix<T>& Rw,
                       Matrix<T>& G,
                       Vector<T>& Zw,
                       Matrix<T>& Rwx)
   throw(MatrixException)
{
   const T EPS=-T(1.e-200);
   unsigned int n=R.rows(),ns=Rw.rows();
   unsigned int i,j,k;
   T sum, beta, delta, dum;

   if(Phi.rows() < n || Phi.cols() < n ||
      G.rows() < n || G.cols() < ns ||
      R.cols() != n ||
      Rwx.rows() < ns || Rwx.cols() < n ||
      Z.size() < n || Zw.size() < ns) {
      MatrixException me("Invalid input dimensions:\n  R is "
         + asString<int>(R.rows()) + "x"
         + asString<int>(R.cols()) + ", Z has length "
         + asString<int>(Z.size()) + "\n  Phi is "
         + asString<int>(Phi.rows()) + "x"
         + asString<int>(Phi.cols()) + "\n  Rw is "
         + asString<int>(Rw.rows()) + "x"
         + asString<int>(Rw.cols()) + "\n  G is "
         + asString<int>(G.rows()) + "x"
         + asString<int>(G.cols()) + "\n  Zw has length "
         + asString<int>(Zw.size()) + "\n  Rwx is "
         + asString<int>(Rwx.rows()) + "x"
         + asString<int>(Rwx.cols())
         );
      GPSTK_THROW(me);
   }

   try {
      Phi = R * Phi;                      // set Phi = Rd = R*Phi
      Rwx = T(0);
      G = -Phi * G;                       // set G = -Rd*G

      //---------------------------------------------------------------
      for(j=0; j<ns; j++) {               // loop over first ns columns
         sum = T(0);
         for(i=0; i<n; i++)               // rows of -Rd*G
            sum += G(i,j)*G(i,j);
         dum = Rw(j,j);
         sum += dum*dum;
         sum = (dum > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
         delta = dum - sum;
         Rw(j,j) = sum;

         beta = sum * delta;
         if(beta > EPS) continue;
         beta = T(1)/beta;

            // apply jth Householder transformation
            // to submatrix below and right of (j,j)
         if(j+1 < ns) {                   // apply to G
            for(k=j+1; k<ns; k++) {       // columns to right of diagonal
               sum = delta * Rw(j,k);
               for(i=0; i<n; i++)         // rows of G
                  sum += G(i,j)*G(i,k);
               if(sum == T(0)) continue;
               sum *= beta;
               Rw(j,k) += sum*delta;
               for(i=0; i<n; i++)         // rows of G again
                  G(i,k) += sum * G(i,j);
            }
         }

            // apply jth Householder transformation
            // to Rwx and Phi
         for(k=0; k<n; k++) {             // columns of Rwx and Phi
            sum = delta * Rwx(j,k);
            for(i=0; i<n; i++)            // rows of Phi and G
               sum += Phi(i,k) * G(i,j);
            if(sum == T(0)) continue;
            sum *= beta;
            Rwx(j,k) += sum*delta;
            for(i=0; i<n; i++)            // rows of Phi and G
               Phi(i,k) += sum * G(i,j);
         }                                // end loop over columns of Rwx and Phi

            // apply jth Householder transformation
            // to Zw and Z
         sum = delta * Zw(j);
         for(i=0; i<n; i++)               // rows of G and elements of Z
            sum += Z(i) * G(i,j);
         if(sum == T(0)) continue;
         sum *= beta;
         Zw(j) += sum * delta;
         for(i=0; i<n; i++)               // rows of G and elements of Z
            Z(i) += sum * G(i,j);
      }                                   // end loop over first ns columns