srifilter.hpp

一个gps小工具包

#pragma ident "$Id: SRIFilter.hpp 293 2006-11-10 16:39:56Z rickmach $"

//============================================================================
//
//  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 SRIFilter.hpp
 * Include file defining class SRIFilter.
 * class SRIFilter implements the square root information matrix form of the
 * Kalman filter and smoother.
 *
 * Reference: "Factorization Methods for Discrete Sequential Estimation,"
 *             G.J. Bierman, Academic Press, 1977.
 */

//------------------------------------------------------------------------------------
#ifndef CLASS_SQUAREROOT_INFORMATION_FILTER_INCLUDE
#define CLASS_SQUAREROOT_INFORMATION_FILTER_INCLUDE

//------------------------------------------------------------------------------------
// system
#include <ostream>
// GPSTk
#include "Vector.hpp"
#include "Matrix.hpp"
#include "SRI.hpp"

namespace gpstk
{

//------------------------------------------------------------------------------------
/** class SRIFilter inherits SRI and implements a square root information filter,
 * which is the square root formulation of the Kalman filter algorithm. SRIFilter may
 * be used for Kalman filtering, smoothing, or for simple least squares, including
 * weighted, linear or linearized, robust and/or sequential algorithms.
 *
 * 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)), or
 * R = upper triangular square root (Cholesky decomposition) of the inverse of P,
 * and z = R * X.
 *
 * The SRIFilter implements Kalman filter algorithm, which includes sequential least
 * squares (measurement update), dynamic propagation (time update), and smoothing
 * (technically the term 'Kalman filter algorithm' is reserved for the classical
 * algorithm just as Kalman presented it, in terms of a state vector and its
 * covariance matrix).
 *
 * The SRIFilter measurment update (which is actually just linear least squares) is
 * half of the SRIFilter (Kalman filter) - there is a 'time update' that propagates
 * the SRI (and thus the state and covariance) forward in time using the dynamical
 * model of the filter. These are algebraically equivalent to the classical Kalman
 * algorithm, but are more efficient and numerically stable (actually the Kalman
 * algorithm has been shown to be numerically unstable!). The SRIFilter smoothing
 * algorithms consists of a 'backwards' filter, implemented by applying a
 * 'smoother update' to the SRIFilter at each point in reverse order.
 *
 *  Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential Estimation,"
 *       Academic Press, 1977.
 */
class SRIFilter : public SRI {
public:
      /// empty constructor
   SRIFilter(void) throw();

      /// constructor given the dimension N.
      /// @param N dimension of the SRIFilter.
   SRIFilter(const unsigned int N)
      throw();

      /// constructor given a Namelist; its dimension determines the SRI dimension.
      /// @param NL Namelist for the SRIFilter.
   SRIFilter(const Namelist& NL)
      throw();

      /// explicit constructor - throw if the dimensions are inconsistent.
      /// @param R  Initial information matrix, an upper triangular matrix of dim N.
      /// @param Z  Initial information data vector, of length N.
      /// @param NL Namelist for the SRIFilter, also of length N.
      /// @throw MatrixException if dimensions are not consistent.
   SRIFilter(const Matrix<double>& R,
             const Vector<double>& Z,
             const Namelist& NL)
      throw(MatrixException);

      /// copy constructor
      /// @param right SRIFilter to be copied
   SRIFilter(const SRIFilter& right)
      throw()
      { *this = right; }

      /// operator=
      /// @param right SRIFilter to be copied
   SRIFilter& operator=(const SRIFilter& right)
      throw();

      /// SRIF (Kalman) simple linear measurement update with optional weight matrix
      /// @param H  Partials matrix, dimension MxN.
      /// @param D  Data vector, length M; on output D is post-fit residuals.
      /// @param CM Measurement covariance matrix, dimension MxM.
      /// @throw if dimension N does not match dimension of SRI, or if other
      ///        dimensions are inconsistent, or if CM is singular.
   void measurementUpdate(const Matrix<double>& H,
                         Vector<double>& D,
                         const Matrix<double>& CM=SRINullMatrix)
   throw(MatrixException,VectorException);

      /// SRIF (Kalman) measurement update, or least squares update
      /// Given data and measurement covariance, compute a solution and
      /// covariance using the appropriate least squares algorithm.
      /// @param D   Data vector, length M
      ///               Input:  raw data
      ///               Output: post-fit residuals
      /// @param X   Solution vector, length N
      ///               Input:  nominal solution X0 (zero when doLinearized is false)
      ///               Output: final solution
      /// @param Cov Covariance matrix, dimension (N,N)
      ///               Input:  (If doWeight is true) inverse measurement covariance
      ///                       or weight matrix(M,M)
      ///               Output: Solution covariance matrix (N,N)
      /// @param LSF Pointer to a function which is used to define the equation
      ///            to be solved.
      ///            LSF arguments are:
      ///            X  Nominal solution (input)
      ///            f  Values of the equation f(X) (length M) (output)
      ///            P  Partials matrix df/dX evaluated at X (dimension M,N) (output)
      ///        When doLinearize is false, LSF ignores X and returns the (constant)
      ///        partials matrix P and zero for f(X).
      /// @throw MatrixException if the input is inconsistent
      /// Return values: 0 ok
      ///               -1 Problem is underdetermined (M<N) // TD -- naturalized sol?
      ///               -2 Problem is singular
      ///               -3 Algorithm failed to converge
      ///               -4 Algorithm diverged
   int leastSquaresEstimation(Vector<double>& D,
                              Vector<double>& X,
                              Matrix<double>& Cov,
                              void (LSF)(Vector<double>& X,
                                         Vector<double>& f,
                                         Matrix<double>& P)
      )
      throw(MatrixException);

      /// SRIF (Kalman) time update
      /// This routine uses the Householder transformation to propagate the SRIFilter
      /// state and covariance through a time 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>
      ///        Inverse of 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
      /// @param G Matrix<double>
      ///        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.
      /// @param Zw Vector<double>
      ///        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). Used for output.
      /// @param Rwx Matrix<double>
      ///        An ns by n matrix which is set to zero by this routine 
      ///        and 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.
      /// 
      /// @throw MatrixException if the input is inconsistent
      /// @return void
      /// 
      /// 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 GQtranspose(G) 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.  [Ref. 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 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.
      ///
      ///   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.
   void timeUpdate(Matrix<double>& Phi,
                   Matrix<double>& Rw,
                   Matrix<double>& G,
                   Vector<double>& zw,