srifilter.hpp

一个gps小工具包

                   Matrix<double>& Rwx)
      throw(MatrixException);

      /// SRIF (Kalman) smoother update
      /// This routine uses the Householder transformation to propagate the SRIF
      /// state and covariance through a smoother (backward filter) step.
      /// If the existing SRI state is of dimension N, and the number of noise
      /// parameter is Ns, then the inputs must be dimensioned as indicated.
      /// @param Phi Matrix<double>
      ///        State transition matrix, an N by N matrix.
      ///        Phi is destroyed on output.
      /// @param Rw Matrix<double>
      ///        A priori square root information matrix for the process
      ///        noise, an Ns by Ns upper triangular matrix (which has 
      ///        Ns(Ns+1)/2 elements), output of the time update.
      /// @param G Matrix<double>
      ///        The N by Ns matrix associated with process noise.  The 
      ///        process noise covariance is GQtranspose(G) where inverse(Q)
      ///        is Rw(trans)*Rw, also input to the time update.   // TD
      /// @param zw Vector<double>
      ///        A priori 'state' associated with the process noise,
      ///        a vector with Ns elements, output of the time update.
      /// @param Rwx Matrix<double> An Ns by N matrix, output of the time update.
      ///
      /// The inputs Rw,zw,Rwx are the output of the SRIF time update, and these and
      /// Phi and G are associated with the same timestep. All the inputs are trashed
      /// on output.
      /// 
      /// @throw MatrixException if the input is inconsistent
      /// @return void
      /// 
      /// Method:
      ///   The fixed interval square root information smoother (SRIS) is 
      /// composed of two Kalman filters, one identical with the square root 
      /// information filter (SRIF), the other similar but operating on the
      /// data in reverse order and combining the current (smoothed) state
      /// with elements output by the SRIF in its forward run and saved.
      /// Thus a smoother is composed of a forward filter which saves all of
      /// its output, followed by a backward filter which makes use of that
      /// saved information.
      ///   This form of the SRIF backward filter algorithm is equivalent to the
      /// Dyer-McReynolds SRIS algorithm, which uses less computer resources, but
      /// propagates the state and covariance rather than the SRI (R,z). [As always,
      /// at any point the state X and covariance P are related to the SRI by
      /// X = inverse(R) * z , P = inverse(R) * inverse(transpose(R)).]
      ///   For startup of the backward filter, the state after the final 
      /// measurement update of the SRIF is given another time update, the
      /// output of which is identified with the a priori values for the 
      /// backward filter.  Backward filtering proceeds from there, the N+1st
      /// point, toward the first point.
      ///
      ///   In this implementation of the backward filter, the Householder
      /// transformation is applied to the following matrix
      /// [dimensions are shown in ()]:
      /// 
      ///      _  (Ns)     (N)      (1) _          _                  _
      /// (Ns) |  Rw+Rwx*G  Rwx*Phi  zw   |   ==>  |   Rw   Rwx   zw    |
      /// (N)  |  R*G       R*Phi    z    |   ==>  |   0     R    z     | .
      ///      -                        -          -                  -
      /// The SRI matricies R and Rw remain upper triangular.
      ///
      /// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
      ///      Estimation," Academic Press, 1977.
   void smootherUpdate(Matrix<double>& Phi,
                       Matrix<double>& Rw,
                       Matrix<double>& G,
                       Vector<double>& zw,
                       Matrix<double>& Rwx)
      throw(MatrixException);

      /// Covariance/State version of the Kalman smoother update (Dyer-McReynolds).
      /// This routine implements the Dyer-McReynolds form of the state and covariance
      /// recursions which constitute the backward filter of the Square Root
      /// Information Smoother; it is equivalent to the SRI form implemented in
      /// SRIFilter::smootherUpdate().
      /// 
      /// @param X Vector<double> X(N)
      ///          A priori state, derived from SRI (R*X=Z)
      /// @param P Matrix<double> P(N,N)
      ///          A priori covariance, derived from SRI (P=R^-1*R^-T)
      /// @param Rw Matrix<double> Rw(Ns,Ns)
      ///          Process noise covariance (UT), output of SRIF TU
      /// @param Rwx Matrix<double> Rwx(Ns,N)
      ///          PN 'cross term', output of SRIF TU
      /// @param Zw Vector<double> Zw(Ns)
      ///          Process noise state, output of SRIF TU
      /// @param Phinv Matrix<double> Phinv(N,N)
      ///          Inverse of state transition, saved at SRIF TU
      /// @param G Matrix<double> G(N,Ns)
      ///          Noise coupling matrix, saved at SRIF TU
      /// @throw MatrixException if the input is inconsistent
      /// @return void
      /// On return, X and P are the updated state and covariance, and the
      /// other inputs are trashed.
      /// 
      /// Method:
      ///   The fixed interval square root information smoother (SRIS) is 
      /// composed of two Kalman filters, one identical with the square root 
      /// information filter (SRIF), the other similar but operating on the
      /// data in reverse order and combining the current (smoothed) state
      /// with elements output by the SRIF in its forward run and saved.
      /// Thus a smoother is composed of a forward filter which saves all of
      /// its output, followed by a backward filter which makes use of that
      /// saved information.
      ///   This form of the SRIS algorithm is equivalent to the SRIS backward
      /// filter Householder transformation algorithm, but uses less computer
      /// resources. It is not necessary to update both the state and the
      /// covariance, although doing both at once is less expensive than
      /// doing them separately. (This routine does both.)
      ///   For startup of the backward filter, the state after the final 
      /// measurement update of the SRIF is given another time update, the
      /// output of which is identified with the a priori values for the 
      /// backward filter.  Backward filtering proceeds from there, the N+1st
      /// point, toward the first point.
      ///
      /// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
      ///      Estimation," Academic Press, 1977.
   static void DMsmootherUpdate(Matrix<double>& P,
                                Vector<double>& X,
                                Matrix<double>& Phinv,
                                Matrix<double>& Rw,
                                Matrix<double>& G,
                                Vector<double>& Zw,
                                Matrix<double>& Rwx)
      throw(MatrixException);

      /// output operator
   friend std::ostream& operator<<(std::ostream& s,
                                   const SRIFilter& srif);

      /// Get the current solution vector
      /// @return current solution vector
   Vector<double> Solution(void) { return Xsave; }

      /// Get the number of iterations used in last call to leastSquaresEstimation()
      /// @return the number of iterations
   int Iterations() { return number_iterations; }

      /// Get the convergence value found in last call to leastSquaresEstimation()
      /// @return the convergence value
   double Convergence() { return rms_convergence; }

      /// Get the condition number of the covariance matrix from last calls
      /// to leastSquaresEstimation() (Larger means 'closer to singular' except
      /// zero means condition number is infinite)
   double ConditionNumber() { return condition_number; }

      /// Return true if the algorithm succeeded.
      /// Currently used only by leastSquaresEstimation(). TD - do in TU and SU
   bool isValid() { return valid; }

      /// remove all stored information by setting the SRI to zero
      /// (does not re-dimension).
   void zeroAll(void);

      /// 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 Reset(const int N=0);

   // ------------- member data ---------------
      /// limit on the number of iterations
   int iterationsLimit;
      /// limit on the RSS change in solution which produces success
   double convergenceLimit;
      /// upper limit on the RSS change in solution which produces an abort
   double divergenceLimit;
      /// if true, weight the equation using the inverse of covariance matrix
      /// on input - default is false
   bool doWeight;
      /// if true, weight the equation using robust statistical techniques
      /// - default is false
   bool doRobust;
      /// if true, save information for a sequential solution - default is false
   bool doSequential;
      /// if true, equation F(X)=D is non-linear, the algorithm will be iterated,
      /// and LSF must return partials matrix and F(X). - default is false
   bool doLinearize;
      /// if true, output intermediate results
   bool doVerbose;

private:
      /// SRIF time update (non-SRI version); SRIFilter::timeUpdate for doc.
   template <class T>
   static void SrifTU(Matrix<T>& R,
                      Vector<T>& Z,
                      Matrix<T>& Phi,
                      Matrix<T>& Rw,
                      Matrix<T>& G,
                      Vector<T>& Zw,
                      Matrix<T>& Rwx)
      throw(MatrixException);

      /// SRIF smoother update (non-SRI version); SRIFilter::smootherUpdate for doc.
   template <class T>
   static void SrifSU(Matrix<T>& R,
                      Vector<T>& Z,
                      Matrix<T>& Phi,
                      Matrix<T>& Rw,
                      Matrix<T>& G,
                      Vector<T>& Zw,
                      Matrix<T>& Rwx)
      throw(MatrixException);

      /// SRIF smoother update in covariance / state form;
      /// see SRIFilter::DMsmootherUpdate() for doc.
   template <class T>
   static void SrifSU_DM(Matrix<T>& P,
                         Vector<T>& X,
                         Matrix<T>& Phinv,
                         Matrix<T>& Rw,
                         Matrix<T>& G,
                         Vector<T>& Zw,
                         Matrix<T>& Rwx)
      throw(MatrixException);

      /// initialization used by constructors
   void defaults(void) throw()
   {
      iterationsLimit = 10;
      convergenceLimit = 1.e-8;
      divergenceLimit = 1.e10;
      doWeight = false;
      doRobust = false;
      doLinearize = false;
      doSequential = false;
      doVerbose = false;
      valid = false;
      number_iterations = number_batches = 0;
      rms_convergence = condition_number = 0.0;
   }

   // private member data - inherits from SRI
      // inherit SRI Information matrix, an upper triangular (square) matrix
   //Matrix<double> R;
      // inherit SRI state vector, of length equal to dimension (row and col) of R.
   //Vector<double> Z;
      // inherit SRI Namelist parallel to R and Z, labelling elements of state vector.
   //Namelist names;
   /// indicates if filter is valid - set false when inversion finds singularity.
   bool valid;
   /// current number of iterations
   int number_iterations;
   /// current number of batches seen
   int number_batches;
   /// RMS change in state, used for convergence test
   double rms_convergence;
   /// condition number, defined in inversion to get state and covariance
   double condition_number;
   /// solution X consistent with current information RX=z
   Vector<double> Xsave;

}; // end class SRIFilter

} // end namespace gpstk

//------------------------------------------------------------------------------------
#endif