sri.hpp

linux的gps应用

#pragma ident "$Id: SRI.hpp 286 2006-11-08 02:21:51Z ocibu $"

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

//------------------------------------------------------------------------------------
// TD go back thru and add const and throw() everywhere, also in Namelist
// TD check that names CAN have different length than R and Z -- see zeroAll

//------------------------------------------------------------------------------------
#ifndef CLASS_SQUAREROOTINFORMATION_INCLUDE
#define CLASS_SQUAREROOTINFORMATION_INCLUDE

//------------------------------------------------------------------------------------
// system includes
#include <string>
#include <vector>
#include <algorithm>
#include <iomanip>
#include <ostream>
#include <sstream>
// GPSTk
#include "Matrix.hpp"
#include "Namelist.hpp"

namespace gpstk
{

//------------------------------------------------------------------------------------
/// constant (empty) Matrix used for default input arguments
extern const Matrix<double> SRINullMatrix;

//------------------------------------------------------------------------------------
// fundamental routines
/// Compute inverse of uppertriangular matrix, returning smallest and
/// largest eigenvalues.
/// @param UT upper triangular matrix to be inverted
/// @param ptrS pointer to <T> small, on output *ptrS contains smallest eigenvalue.
/// @param ptrB pointer to <T> small, on output *ptrB contains largest eigenvalue.
/// @return inverse of input matrix.
/// @throw MatrixException if input is not square (assumed upper triangular as well).
/// @throw SingularMatrixException if input is singular.
template <class T>
Matrix<T> inverseUT(const Matrix<T>& UT,
                    T *ptrSmall,
                    T *ptrBig)
   throw(MatrixException);

/// Compute the product of an upper triangular matrix and its transpose.
/// @param UT upper triangular matrix
/// @return product UT * transpose(UT)
/// @throw MatrixException if input is not square (assumed upper triangular as well).
template <class T>
Matrix<T> UTtimesTranspose(const Matrix<T>& UT)
   throw(MatrixException);

/// Square root information measurement update
template <class T>
void SrifMU(Matrix<T>& R,
            Vector<T>& Z,
            Matrix<T>& H,
            Vector<T>& D,
            unsigned int M=0)
   throw(MatrixException);

/// Square root information measurement update, with new data in the form of a
/// single matrix of H and D concatenated: A = H || D.
template <class T>
void SrifMU(Matrix<T>& R,
            Vector<T>& Z,
            Matrix<T>& A,
            unsigned int M=0)
   throw(MatrixException);

//------------------------------------------------------------------------------------
/// class SRI encapsulates all the information associated with the solution of a set
/// of simultaneous linear equations. It is used in least squares estimation (linear
/// and linearized) and is the basis of the preferred implementation of Kalman
/// filtering. An SRI consists of just three things:
/// (1) 'R', the 'information matrix', which is an upper triangular matrix of
/// dimension N, equal to the inverse of the square root (or Cholesky decomposition)
/// of the solution covariance matrix,
/// (2) 'Z', the 'SRI state vector' of length N (parallels the components of R),
/// (not to be confused with the regular state vector X), and
/// (3) 'names', a Namelist used to label the elements of R and Z (parallels and
/// labels rows and columns of R and elements of Z). A Namelist is part of class SRI
/// because the manipulations of SRI (see functions below) requires a consistent way
/// of manipulating the different individual elements of R and Z, in addition it
/// allows the user to attach 'human-readable' labels to the elements of the state
/// vector, which is useful in adding, dropping and bumping states, and it makes
/// printed results more readable (see the LabelledMatrix class in Namelist.hpp).
///
/// The set of simultaneous equations represented by an SRI is R * X = Z, where X is
/// the (unknown) state vector (the conventional solution vector) also of dimension N.
/// The state X is solved for as X = inverse(R) * Z, and the covariance matrix of the
/// state X is equal to transpose(inverse(R))*inverse(R).
///
/// Least squares estimation via SRI is very simple and efficient; it uses the
/// Householder transformation to convert the problem to upper triangular form, and
/// then uses very efficient algorithms to invert the information matrix to find the
/// solution and its covariance. The usual matrix equation is H * X = D,
/// where H is the 'design matrix' or the 'partials matrix', of dimension M x N,
/// X is the (unknown) solution vector of length N, and D is the 'data' or
/// 'measurement' vector of length M. In the least squares 'update' of the SRI,
/// this set of information {H,D} is concatenated with the existing SRI {R,Z} to
/// form an (N+M x N+1) matrix Q which has R in the upper left, Z upper right,
/// H lower left and D lower right. This extended matrix is then subjected to a
/// Householder transformation (see class Matrix), which will put (at least the
/// first N columns of) Q into upper triangular form. The result is a new, updated
/// SRI (R and Z) in the place of the old, while in place of D are residuals of fit
/// corresponding to the measurements in D (the H part of Q is trashed). This result,
/// in fact (see the reference), produces an updated SRI which gives precisely the
/// usual least squares solution for the combined 'a priori SRI + new data' problem.
/// This algorithm is called a 'measurement update' of the SRI.
///
/// It is most enlightening to think of the SRI and this process in terms of
/// 'information'. The SRI contains all the 'information' which has come from
/// updates that have been made to it using (H,D) pairs. Initially, the SRI is all
/// zeros, which corresponds to 'no information'. This overcomes one serious problem
/// with conventional least squares and the Kalman algorithm, namely that a
/// 'zero information' starting value cannot be correctly expressed, because in that
/// case the covariance matrix is singular and the state vector is indeterminate;
/// in the SRI method this is perfectly consistent - the covariance matrix is
/// singular because the information matrix (R) is zero, and thus the state
/// is entirely indeterminate. As new 'information' (in the form of data D and
/// partials matrix H pairs) is added to the SRI (via the Householder algorithm),
/// the 'information' stored in R and Z is increased and they become non-zero.
/// (By the way note that the number of rows in the {H,D} information is arbitrary -
/// information can be added in 'batches' - M large - or one - M=1 - piece at a time.)
/// When there is enough information, R becomes non-singular, and so can be inverted
/// and the solution and covariance can be computed. As the amount of information
/// becomes large, elements of R become large, and thus elements of the covariance
/// (think of covariance as a measure of uncertainty - the opposite or inverse of
/// information) become small.
///
/// The structure of the SRI method allows some powerful techniques to be used in 
/// manipulating, combining and separating state elements and the information
/// associated with them in SRIs. For example, if the measurement updates have
/// failed to increase the information about one particular state element, then
/// that element, and its information, may be removed from the problem by deleting
/// that element's row and column of R, and its element of Z (and then
/// re-triangularizing the SRI). In general, any subset of an SRI may be separated,
/// or the SRI split (see the routine of that name below - note the caveats) into
/// two separate SRIs. For another example, SRI allows the information of a each
/// state element to be selectively reduced or even zeroed, simply by multiplying
/// the corresponding elements of R and Z by a factor; in Kalman filtering this
/// is called a 'Q bump' of the element and is very important in some filtering
/// applications. There are methods (see below) consistently to merge (operator+()),
/// split, and permute elements of, SRIs.
///
/// Kalman filtering is an important application of SRI methods (actually it is
/// called 'square root information filtering' or SRIF - 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 measurment update described above (which is actually just linear least
/// squares) is half of the SRIF (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!).
/// There are even SRI smoothing algorithms, corresponding to Kalman smoothers,
/// which consist of a 'backwards' filter, implemented by applying a
/// 'smoother update' to the SRI at each point in reverse order.
///
/// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential Estimation,"
///      Academic Press, 1977.
class SRI {
public:
      /// empty constructor
   SRI(void) throw() { }