#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.