sri.cpp

linux的gps应用

#pragma ident "$Id: SRI.cpp 312 2006-11-27 22:14:50Z pben $"

//============================================================================
//
//  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.cpp
 * Implementation of 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.
 */

// -----------------------------------------------------------------------------------
// system
#include <string>
#include <vector>
#include <algorithm>
#include <ostream>
// GPSTk
#include "SRI.hpp"
#include "StringUtils.hpp"

using namespace std;

namespace gpstk
{
   // --------------------------------------------------------------------------------
   // used to mark optional input
   const Matrix<double> SRINullMatrix;

   //---------------------------------------------------------------------------------
   // constructor given the dimension N.
   SRI::SRI(const unsigned int N)
      throw()
   {
      R = Matrix<double>(N,N,0.0);
      Z = Vector<double>(N,0.0);
      names = Namelist(N);
   }

   // --------------------------------------------------------------------------------
   // constructor given a Namelist, its dimension determines the SRI dimension.
   SRI::SRI(const Namelist& nl)
      throw()
   {
      if(nl.size() <= 0) return;
      R = Matrix<double>(nl.size(),nl.size(),0.0);
      Z = Vector<double>(nl.size(),0.0);
      names = nl;
   }

   // --------------------------------------------------------------------------------
   // explicit constructor - throw if the dimensions are inconsistent.
   SRI::SRI(const Matrix<double>& r,
            const Vector<double>& z,
            const Namelist& nl)
      throw(MatrixException)
   {
      if(r.rows() != r.cols() || r.rows() != z.size() || r.rows() != nl.size()) {
         using namespace StringUtils;
         
         MatrixException me("Invalid dimensions in explicit SRI constructor:\n R is "
               + asString<int>(r.rows()) + "x"
               + asString<int>(r.cols()) + ", Z has length "
               + asString<int>(z.size()) + " and NL has length "
               + asString<int>(nl.size())
               );
         GPSTK_THROW(me);
      }
      if(r.rows() <= 0) return;
      R = r;
      Z = z;
      names = nl;
   }

   // --------------------------------------------------------------------------------
   // copy constructor
   SRI::SRI(const SRI& s)
      throw()
   {
      R = s.R;
      Z = s.Z;
      names = s.names;
   }

   // --------------------------------------------------------------------------------
   // operator=
   SRI& SRI::operator=(const SRI& right)
      throw()
   {
      R = right.R;
      Z = right.Z;
      names = right.names;
      return *this;
   }

   // ---------------------------------------------------------------------------
   // modify SRIs
   // --------------------------------------------------------------------------------
   // Permute the SRI elements to match the input Namelist, which may differ with
   // the SRI Namelist by AT MOST A PERMUTATION, throw if this is not true.
   void SRI::permute(const Namelist& nl)
      throw(MatrixException,VectorException)
   {
      if(identical(names,nl)) return;
      if(names != nl) {
         MatrixException me("Invalid input: Namelists must be == to permute");
         GPSTK_THROW(me);
      }

      try {
         unsigned int i,j;
         // build a permutation matrix
         Matrix<double> P(R.rows(),R.rows(),0.0);
         for(i=0; i<R.rows(); i++) {
            j = nl.index(names.getName(i));
            P(j,i) = 1;
         }

         Matrix<double> B;
         Vector<double> Q;
         B = P * R * transpose(P);
         Q = P * Z;

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

   // --------------------------------------------------------------------------------
   // Split this SRI (call it S) into two others, S1 and Sleft, where S1 has
   // a Namelist identical to the input Namelist (NL); set *this = S1 at the
   // end. NL must be a non-empty subset of names, and (names ^ NL) also must
   // be non-empty; throw MatrixException if this is not true. The second
   // output SRI, Sleft, will have the same names as S, but perhaps permuted.
   //
   // The routine works by first permuting S so that its Namelist if of the
   // form {N2,NL}, where N2 = (names ^ NL); this is possible only if NL is
   // a non-trivial subset of names. Then, the rows of S (rows of R and elements
   // of Z) naturally separate into the two component SRIs, with zeros in the
   // elements of the first SRI which correspond to N2, and those in Sleft
   // which correspond to NL.
   //
   //    Example:    S.name = A B C D E F G and NL = D E F G.
   // (Obviously, S may be permuted into such an order whenever this is needed.)
   // Note that here the R,Z pair is written in a format reminiscent of the
   // set of equations implied by R*X=Z, i.e. 1A+2B+3C+4D+5E+6F+7G=a, etc.
   //
   //          S (R Z)       =         S1            +         Sleft
   // with    names                       NL                  names
   //     A B C D E F G           . . . D E F G           A B C D E F G   
   //     - - - - - - -  -        - - - - - - -  -        - - - - - - -  -
   //     1 2 3 4 5 6 7  a   =    . . . . . . .  .   +    1 2 3 4 5 6 7  a
   //       8 9 1 2 3 4  b          . . . . . .  .          8 9 1 2 3 4  b
   //         5 6 7 8 9  c            . . . . .  .            5 6 7 8 9  c
   //           1 2 3 4  d              1 2 3 4  d              . . . .  d
   //             5 6 7  e                5 6 7  e                . . .  e
   //               8 9  f                  8 9  f                  . .  f
   //                 1  g                    1  g                    .  g
   //
   // where "." denotes a zero.  The split is simply separating the linear
   // equations which make up R*X=Z into two groups; because of the ordering,
   // one of the groups of equations (S1) depends only on a particular subset
   // of the elements of the state vector, i.e. the elements labelled by the
   // Namelist NL.
   //
   // The equation shown here is an information equation; if the two SRIs S1
   // and Sleft were merged again, none of the information would be lost.
   // Note that S1 has no dependence on A B C (hence the .'s), and therefore
   // its size can be reduced. However S2 still depends on the full names
   // Namelist. Sleft is necessarily singular, but S1 is not.
   //
   // Note that the SRI contains information about both the solution and
   // the covariance, i.e. state and noise, and therefore one must be very careful
   // in interpreting the results of split and merge (operator+=). [Be especially
   // careful about the idea that a merge might be reversible with a split() or
   // vice-versa - strictly this is never possible unless the Namelists are
   // mutually exclusive - two separate problems.]
   //
   // For example, suppose two different SRI's, which have some elements in common,
   // are merged. The combined SRI will have more information (it can't have less)
   // about the common elements, and therefore the solution will be 'better'
   // (assuming the underlying model equations for those elements are identical).
   // However the noises will also be combined, and the results you get might be
   // surprising. Also, note that if you then split the combined SRI again, the
   // solution won't change but the noises will be very different; in particular
   // the new split part will take all the information with it, so the common states
   // will have lower noise than they did in the original SRI.
   // See the test program tsri.cpp
   //
   void SRI::split(const Namelist& NL, SRI& Sleft)
      throw(MatrixException,VectorException)
   {
      try {
         Sleft = SRI(0);
         unsigned int n,m;
         n = NL.size();
         m = names.size();
         if(n >= m) {
            MatrixException me("Input Namelist must be a subset of this one");
            GPSTK_THROW(me);
         }

         unsigned int i,j;
            // copy names and permute it so that its end matches NL 
         Namelist N0(names);
         for(i=1; i<=n; i++) {           // loop (backwards) over names in NL
            for(j=1; j<=m; j++) {        // search (backwards) in NO for a match
               if(NL.labels[n-i] == N0.labels[m-j]) {  // if found a match
                  N0.swap(m-i,m-j);      // then move matching name to end
                  break;                 // and go on to next name in NL
               }
            }
            if(j > m) {
               MatrixException me("Input Namelist is not a non-trivial subset");
               GPSTK_THROW(me);
            }
         }

            // copy *this into Sleft, then do the permutation
         Sleft = *this;
         Sleft.permute(N0);

            // copy parts of Sleft into S1, and then zero out those parts of Sleft
         SRI S1(NL);
         S1.R = Matrix<double>(Sleft.R,m-n,m-n,n,n);
         //S1.Z = Vector<double>(Sleft.Z,m-n,n);
         S1.Z.resize(n);
         for(i=0; i<n; i++) S1.Z(i) = Sleft.Z(m-n+i);
         for(i=m-n; i<m; i++) Sleft.zeroOne(i);

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

   // --------------------------------------------------------------------------------
   // extend this SRI to include the given Namelist, with no added information;
   // names in the input namelist which are not unique are ignored.
   SRI& SRI::operator+=(const Namelist& NL)
      throw(MatrixException,VectorException)
   {
      try {
         Namelist B(names);
            // NB assume that Namelist::operator|=() adds at the _end_
         B |= NL;
            // NB assume that this zeros A.R and A.Z
         SRI A(B);
            // should do this with slices..
            // copy into the new SRI
         for(unsigned int i=0; i<R.rows(); i++) {
            A.Z(i) = Z(i);
            for(unsigned int j=0; j<R.cols(); j++) A.R(i,j) = R(i,j);
         }
         *this = A;
         return *this;
      }
      catch(MatrixException& me) {
         GPSTK_RETHROW(me);
      }
      catch(VectorException& ve) {
         GPSTK_RETHROW(ve);
      }
   }

   // --------------------------------------------------------------------------------
   // reshape this SRI to match the input Namelist, by calling other member
   // functions, including split(), operator+() and permute()
   // Given this SRI and a new Namelist NL, if NL does not match names,
   // transform names to match it, using (1) drop elements (this is probably
   // optional - you can always keep 'dead' elements), (2) add new elements
   // (with zero information), and (3) permute to match NL.