SRI.cpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk 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 3.0 of the License, or
//  any later version.
//
//  The GNSSTk 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 GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was 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.
//
//==============================================================================
 
// -----------------------------------------------------------------------------------
// system
#include <algorithm>
#include <ostream>
#include <string>
#include <vector>
// geomatics
#include "Namelist.hpp"
#include "SRI.hpp"
// GNSSTk
#include "StringUtils.hpp"
 
using namespace std;
 
namespace gnsstk
{
   using namespace StringUtils;
 
      // -----------------------------------------------------------------------------
      // used to mark optional input
   const Matrix<double> SRINullMatrix;
   const SparseMatrix<double> SRINullSparseMatrix;
 
   //---------------------------------------------------------------------------------
      // constructor given the dimension N.
   SRI::SRI(const unsigned int N)
   {
      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)
   {
      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)
   {
      if (r.rows() != r.cols() || r.rows() != z.size() || r.rows() != nl.size())
      {
         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()));
         GNSSTK_THROW(me);
      }
      if (r.rows() <= 0)
      {
         return;
      }
      R     = r;
      Z     = z;
      names = nl;
   }
 
      // -----------------------------------------------------------------------------
      // define from covariance and state
   void SRI::setFromCovState(const Matrix<double>& Cov,
                             const Vector<double>& State, const Namelist& NL)
   {
      if (Cov.rows() != Cov.cols() || Cov.rows() != State.size() ||
          Cov.rows() != NL.size())
      {
         MatrixException me(
            "Invalid dimensions in SRI constructor from Cov,State:\n"
            " Cov is " +
            asString<int>(Cov.rows()) + "x" + asString<int>(Cov.cols()) +
            ", State has length " + asString<int>(State.size()) +
            " and NL has length " + asString<int>(NL.size()));
         GNSSTK_THROW(me);
      }
      R     = inverseUT(upperCholesky(Cov));
      Z     = R * State;
      names = NL;
   }
 
      // -----------------------------------------------------------------------------
      // copy constructor
   SRI::SRI(const SRI& s)
   {
      R     = s.R;
      Z     = s.Z;
      names = s.names;
   }
 
      // -----------------------------------------------------------------------------
      // operator=
   SRI& SRI::operator=(const SRI& right)
   {
      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)
   {
      if (identical(names, nl))
      {
         return;
      }
      if (names != nl)
      {
         MatrixException me(
            "Invalid input: Namelists must be == to w/in permute");
         GNSSTK_THROW(me);
      }
 
      try
      {
         const unsigned int n(R.rows());
         unsigned int i, j;
            // build a permutation matrix
         Matrix<double> P(n, n, 0.0);
         for (i = 0; i < n; i++)
         {
            j       = nl.index(names.getName(i));
            P(j, i) = 1;
         }
 
            // inverse of P is transpose of P
         retriangularize(R * transpose(P), Z);
         names = nl;
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_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 labeled 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 Sleft 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)
   {
      try
      {
         Sleft = SRI(0);
         unsigned int n, m;
         n = NL.size();
         m = names.size();
         if (n >= m)
         {
            MatrixException me(
               "split: Input Namelist must be a subset of this one");
            GNSSTK_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(
                  "split: Input Namelist is not non-trivial subset");
               GNSSTK_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.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)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_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)
   {
      try
      {
         Namelist B(names);
            // NB assume that Namelist::operator|=() adds at the _end_
            // NB if there are duplicate names, |= will not add them
         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)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_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. */
   void SRI::reshape(const Namelist& NL)
   {
      try
      {
         if (names == NL)
         {
            return;
         }
         Namelist keep(names);
         keep &= NL; // keep only those in both names and NL
            // Namelist drop(names);    // (drop is unneeded - split would do it)
            // drop ^= keep;            // lose those in names but not in keep
         Namelist add(NL);
         add ^= keep; // add those in NL but not in keep
         SRI Sdrop;   // make a new SRI to hold the losers
            // would like to allow caller access to Sdrop..
         split(keep, Sdrop); // split off only the losers
                                // NB names = drop | keep; drop & keep is empty
         *this += add;       // add the new ones
         this->permute(NL);  // permute it to match NL
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         append an SRI onto this SRI. Similar to opertor+= but simpler; input SRI
         is simply appended, first using operator+=(Namelist), then filling the new
         portions of R and Z, all without final Householder transformation of
         result. Do not allow a name that is already present to be added: throw. */
   SRI& SRI::append(const SRI& S)
   {
      try
      {
            // do not allow duplicates
         if ((names & S.names).size() > 0)
         {
            Exception e("Cannot append duplicate names");
            GNSSTK_THROW(e);
         }
 
            // append to names at the end, and to R Z, zero filling
         const size_t I(names.size());
         *this += S.names;
 
            // just in case...to avoid overflow in loop below
         if (I + S.names.size() != names.size())
         {
            Exception e("Append failed");
            GNSSTK_THROW(e);
         }
 
            // loop over new names, copying data from input into the new SRI
         for (size_t i = 0; i < S.names.size(); i++)
         {
            Z(I + i) = S.Z(i);
            for (size_t j = 0; j < S.names.size(); j++)
               R(I + i, I + j) = S.R(i, j);
         }
 
         return *this;
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         merge this SRI with the given input SRI. ? should this be operator&=() ?
         NB may reorder the names in the resulting Namelist. */
   SRI& SRI::operator+=(const SRI& S)
   {
      try
      {
         Namelist all(names);
         all |= S.names; // assumes Namelist::op|= adds unique S.names to _end_
 
            // all.sort();        // TEMP - for testing with old version
 
            // stack the (R|Z)'s from both in one matrix;
            // all determines the columns, plus last column is for Z
         unsigned int i, j, n, m, sm;
         n  = all.labels.size();
         m  = R.rows();
         sm = S.R.rows();
         Matrix<double> A(m + sm, n + 1, 0.0);
 
            // copy R into A, permuting columns as names differs from all
            // loop over columns of R; do Z at the same time using j=row
         for (j = 0; j < m; j++)
         {
               // find where this column of R goes in A
               // (should never throw..)
            int k = all.index(names.labels[j]);
            if (k == -1)
            {
               MatrixException me("Algorithm error 1");
               GNSSTK_THROW(me);
            }
 
               // copy this col of R into A (R is UT)
            for (i = 0; i <= j; i++)
               A(i, k) = R(i, j);
               // also the jth element of Z
            A(j, n) = Z(j);
         }
 
            // now do the same for S, but put S.R|S.Z below R|Z
         for (j = 0; j < sm; j++)
         {
            int k = all.index(S.names.labels[j]);
            if (k == -1)
            {
               MatrixException me("Algorithm error 2");
               GNSSTK_THROW(me);
            }
            for (i = 0; i <= j; i++)
               A(m + i, k) = S.R(i, j);
            A(m + j, n) = S.Z(j);
         }
 
            // now triangularize A and pull out the new R and Z
            // DONT - dimensions change - retriangularize(A);
         Householder<double> HA;
         HA(A);
            // submatrix args are matrix,toprow,topcol,numrows,numcols
         R = Matrix<double>(HA.A, 0, 0, n, n);
         Z = Vector<double>(HA.A.colCopy(n));
         Z.resize(n);
         names = all;
 
         return *this;
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         merge two SRIs to produce a third. ? should this be operator&() ? */
   SRI operator+(const SRI& Sleft, const SRI& Sright)
   {
      try
      {
         SRI S(Sleft);
         S += Sright;
         return S;
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         Zero out the nth row of R and the nth element of Z, removing all
         information about that element. */
   void SRI::zeroOne(const unsigned int n)
   {
      if (n >= R.rows())
      {
         return;
      }
 
      for (unsigned int j = n; j < R.cols(); j++) {
         R(n, j) = 0.0;
      }
      Z(n) = 0.0;
   }
 
      /* -----------------------------------------------------------------------------
         Zero out all the first n rows of R and elements of Z, removing all
         information about those elements. Default value of the input is 0,
         meaning zero out the entire SRI. */
   void SRI::zeroAll(const unsigned int n)
   {
      if (n <= 0)
      {
         R = 0.0;
         Z = 0.0;
         return;
      }
 
      if (n >= int(R.rows()))
      {
         return;
      }
 
      for (unsigned int i = 0; i < n; i++)
      {
         for (unsigned int j = i; j < R.cols(); j++) {
            R(i, j) = 0.0;
         }
         Z(i) = 0.0;
      }
   }
 
      /* -----------------------------------------------------------------------------
         Shift the state vector by a constant vector X0; does not change
         information i.e. let R * X = Z => R * (X-X0) = Z' throw on invalid input
         dimension */
   void SRI::shift(const Vector<double>& X0)
   {
      if (X0.size() != R.cols())
      {
         MatrixException me("Invalid input dimension: SRI has dimension " +
                            asString<int>(R.rows()) +
                            " while input has length " +
                            asString<int>(X0.size()));
         GNSSTK_THROW(me);
      }
      Z = Z - R * X0;
   }
 
      /* -----------------------------------------------------------------------------
         Shift the SRI state vector (Z) by a constant vector Z0;
         does not change information. i.e. let Z => Z-Z0
         throw on invalid input dimension */
   void SRI::shiftZ(const Vector<double>& Z0)
   {
      if (Z0.size() != R.cols())
      {
         MatrixException me("Invalid input dimension: SRI has dimension " +
                            asString<int>(R.rows()) +
                            " while input has length " +
                            asString<int>(Z0.size()));
         GNSSTK_THROW(me);
      }
      Z = Z - Z0;
   }
 
      /* -----------------------------------------------------------------------------
         Retriangularize the SRI, when it has been modified to a non-UT matrix
         (e.g. by transform()). Given the matrix A=[R||Z], apply HH transforms
         to retriagularize it and pull out new R and Z.
         param A Matrix<double> which is [R || Z] to be retriangularizied.
         throw if dimensions are wrong. */
   void SRI::retriangularize(const Matrix<double>& A)
   {
      const unsigned int n(R.rows());
      if (A.rows() != n || A.cols() != n + 1)
      {
         MatrixException me("Invalid input dimension: SRI has dimension " +
                            asString<int>(n) + " while input has dimension " +
                            asString<int>(A.rows()) + "x" +
                            asString<int>(A.cols()));
         GNSSTK_THROW(me);
      }
 
      Householder<double> HA;
      HA(A);
         // submatrix args are matrix,toprow,topcol,numrows,numcols
      R = Matrix<double>(HA.A, 0, 0, n, n);
      Z = Vector<double>(HA.A.colCopy(n));
         // NB names cannot be changed - caller must do it.
   }
 
      /* Retriangularize the SRI, that is assuming R has been modified to a non-UT
         matrix (e.g. by transform()). Given RR and ZZ, apply HH transforms to
         retriangularize, and store as R,Z.
         @param R Matrix<double> input the modified (non-UT) R
         @param Z Vector<double> input the (potentially) modified Z
         @throw if dimensions are wrong. */
   void SRI::retriangularize(Matrix<double> RR, Vector<double> ZZ)
   {
      const unsigned int n(R.rows());
      if (RR.rows() != n || RR.cols() != n || ZZ.size() != n)
      {
         MatrixException me(
            "Invalid input dimension: SRI has dimension " + asString<int>(n) +
            " while input has dimension " + asString<int>(RR.rows()) + "x" +
            asString<int>(RR.cols()) + " and " + asString<int>(ZZ.size()));
         GNSSTK_THROW(me);
      }
 
      Matrix<double> A(RR || ZZ);
      retriangularize(A);
   }
 
      /* -----------------------------------------------------------------------------
         Apply transformation matrix T to the SRI; i.e. X -> T*X, Cov -> T*Cov*T^T
         X' = T*X, C' = T*C*T^T = (TR^-1)(TR^-1)^T = R'^-1*R'^-T;
         but then Z' = R'*X' = R'TX = RT^-1TX = RX = Z
         This implies right multiplying R by inverse(T), which is the input, and
         then using HH to retriangularize. Input is the _inverse_ of the
         transformation. SRI.names is set to input Namelist throw MatrixException
         if input dimensions are wrong. */
   void SRI::transform(const Matrix<double>& invT, const Namelist& NL)
   {
      const unsigned int n(R.rows());
      if (invT.rows() != n || invT.cols() != n)
      {
         MatrixException me("Invalid input dimension: SRI has dimension " +
                            asString<int>(n) + " while invT has dimension " +
                            asString<int>(invT.rows()) + "x" +
                            asString<int>(invT.cols()));
         GNSSTK_THROW(me);
      }
 
      R = R * invT;
      retriangularize(R, Z);
      names = NL;
   }
 
      /* -----------------------------------------------------------------------------
         Decrease the information in this SRI for, or 'Q bump', the element
         with the input index.  This means that the uncertainty and the state
         element given by the index are divided by the input factor q; the
         default input is zero, which means zero out the information (q =
         infinite). A Q bump by factor q is equivalent to 'de-weighting' the
         element by q. No effect if input index is out of range.
 
         Use a specialized form of the time update, with Phi=unity, G(N x 1) = 0
         except 1 for the element (in) getting bumped, and Rw(1 x 1) = 1 / q.
         Note that this bump of the covariance for element k results in
         Cov(k,k) += q (plus, not times!).
         if q is 0, replace q with 1/q, i.e. lose all information, covariance
         goes singular; this is equivalent to (1) permute so that the 'in'
         element is first, (2) zero out the first row of R and the first element
         of Z, (3) permute the first row back to in. */
   void SRI::Qbump(const unsigned int in, double q)
   {
      try
      {
         if (in >= R.rows())
         {
            return;
         }
         double factor = 0.0;
         if (q != 0.0)
         {
            factor = 1.0 / q;
         }
 
         unsigned int ns = 1, i, j, n = R.rows();
 
         Matrix<double> A(n + ns, n + ns + 1, 0.0), G(n, ns, 0.0);
         A(0, 0)  = factor; // Rw, dimension ns x ns = 1 x 1
         G(in, 0) = 1.0;
         G        = R * G; // R*Phi*G
         for (i = 0; i < n; i++)
         {
            A(ns + i, 0) = -G(i, 0); //     A =   Rw       0       zw=0
            for (j = 0; j < n; j++)  //          -R*Phi*G  R*Phi   Z
               if (i <= j)
               {
                  A(ns + i, ns + j) = R(i, j); //
               }
            A(ns + i, ns + n) = Z(i);
         }
 
            /* triangularize and pull out the new R and Z
               NB do not call retriangularize() here! */
         Householder<double> HA; //    A  =  Rw  Rwx  zw
         HA(A);                  //          0    R   z
         R                = Matrix<double>(HA.A, ns, ns, n, n);
         Vector<double> T = HA.A.colCopy(ns + n);
         Z                = Vector<double>(T, ns, n);
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         Fix one state element (with the given name) to a given value, and set the
         information for that element (== 1/sigma) to a given value.
         No effect if name is not found
         param name string labeling the state in Namelist names
         param value to which the state element is fixed
         param sigma (1/information) assigned to the element */
   void SRI::stateFix(const std::string& name, double value,
                      double sigma, bool restore)
   {
      int index = names.index(name);
      if (index == -1)
      {
         return;
      }
      stateFix((unsigned int)(index), value, sigma, restore);
   }
 
      /* -----------------------------------------------------------------------------
         Fix one state element (at the given index) to a given value, and set the
         information for that element (== 1/sigma) to a given value.
         No effect if index is out of range.
         param index of the element to fix
         param value to which the state element is fixed
         param sigma (1/information) assigned to the element */
   void SRI::stateFix(const unsigned int index, double value,
                      double sigma, bool restore)
   {
      if (index >= names.size())
      {
         GNSSTK_THROW(Exception("Index out of range"));
      }
      const unsigned int N(names.size());
 
         // make a namelist with the desired element last
      Namelist NL(names);
      if (index != N - 1)
      {
         NL.swap(index, N - 1);
         permute(NL);
      }
 
         // fix the element and information
      Z(N - 1)        = value / sigma;
      R(N - 1, N - 1) = 1.0 / sigma;
 
         // permute back, if the caller wants
      if (restore && index != N - 1)
      {
         NL = names;
         NL.swap(index, N - 1);
         permute(NL);
      }
   }
 
      /* -----------------------------------------------------------------------------
         Fix the state element with the input index to the input value, and
         collapse the SRI by removing that element.
         No effect if index is out of range. */
   void SRI::stateFixAndRemove(const unsigned int in, double bias)
   {
      if (in >= R.rows())
      {
         return;
      }
 
      try
      {
         unsigned int i, j, ii, jj, n = R.rows();
         Vector<double> Znew(n - 1, 0.0);
         Matrix<double> Rnew(n - 1, n - 1, 0.0);
            // move the X(in) terms to the data vector on the RHS
         for (i = 0; i < in; i++)
            Z(i) -= R(i, in) * bias;
            // remove row/col in and collapse
         for (i = 0, ii = 0; i < n; i++)
         {
            if (i == in)
            {
               continue;
            }
            Znew(ii) = Z(i);
            for (j = i, jj = ii; j < n; j++)
            {
               if (j == in)
               {
                  continue;
               }
               Rnew(ii, jj) = R(i, j);
               jj++;
            }
            ii++;
         }
         R = Rnew;
         Z = Znew;
         names -= names.labels[in];
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /* -----------------------------------------------------------------------------
         Vector version of stateFixAndRemove with several states given in a
         Namelist. */
   void SRI::stateFixAndRemove(const Namelist& dropNL,
                               const Vector<double>& values_in)
   {
      try
      {
         if (dropNL.size() != values_in.size())
         {
            VectorException e("Input has inconsistent lengths");
            GNSSTK_THROW(e);
         }
 
         /*
                     // build a vector of indexes to keep
                  int i,j;
                  vector<int> indexes;
                  for(i=0; i<names.size(); i++) {
                     j = dropNL.index(names.getName(i)); // index in dropNL,
            this state if(j == -1) indexes.push_back(i);// not found in dropNL,
            so keep
                  }
 
                  const int n=indexes.size();         // new dimension
                  if(n == 0) {
                     Exception e("Cannot drop all states");
                     GNSSTK_THROW(e);
                  }
 
                  Vector<double> X,newX(n);
                  Matrix<double> C,newC(n,n);
                  Namelist newNL;
 
                  double big,small;
                  getStateAndCovariance(X,C,&small,&big);
 
                  for(i=0; i<n; i++) {
                     newX(i) = X(indexes[i]);
                     for(j=0; j<n; j++) newC(i,j) = C(indexes[i],indexes[j]);
                     newNL += names.getName(indexes[i]);
                  }
 
                  R = Matrix<double>(inverseUT(upperCholesky(newC)));
                  Z = Vector<double>(R*newX);
                  names = newNL;
         */
         size_t i, j, k;
            // create a vector of indexes and corresponding values
         vector<int> indx;
         vector<double> value;
         for (i = 0; i < dropNL.size(); i++)
         {
            int in =
               names.index(dropNL.getName(i)); // in must be allowed to be -1
            if (in > -1)
            {
               indx.push_back(in);
               value.push_back(values_in(i));
            }
               // else nothing happens
         }
         const unsigned int m = indx.size();
         const unsigned int n = R.rows();
         if (m == 0)
         {
            return;
         }
         if (m == n)
         {
            *this = SRI(0);
            return;
         }
            // move the X(in) terms to the data vector on the RHS
         for (k = 0; k < m; k++)
         {
            for (i = 0; i < indx[k]; i++)
            {
               Z(i) -= R(i, indx[k]) * value[k];
            }
         }
 
            // first remove the rows in indx
         bool skip;
         Vector<double> Ztmp(n - m, 0.0);
         Matrix<double> Rtmp(n - m, n, 0.0);
         for (i = 0, k = 0; i < n; i++)
         {
            skip = false;
            for (j = 0; j < m; j++)
            {
               if ((int)i == indx[j])
               {
                  skip = true;
                  break;
               }
            }
            if (skip)
            {
               continue; // skip row to be dropped
            }
 
            Ztmp(k) = Z(i);
            for (j = i; j < n; j++)
            {
               Rtmp(k, j) = R(i, j);
            }
            k++;
         }
 
            // Z is now done
         Z = Ztmp;
 
            // now remove columns in indx
         R = Matrix<double>(n - m, n - m, 0.0);
         for (j = 0, k = 0; j < n; j++)
         {
            skip = false;
            for (i = 0; i < m; i++)
            {
               if ((int)j == indx[i])
               {
                  skip = true;
                  break;
               }
            }
            if (skip)
            {
               continue; // skip col to be dropped
            }
 
            for (i = 0; i <= j; i++)
            {
               R(i, k) = Rtmp(i, j);
            }
            k++;
         }
 
            // remove the names
         for (k = 0; k < dropNL.size(); k++)
         {
            std::string name(dropNL.getName(k));
            names -= name;
         }
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
      /*------------------------------------------------------------------------------
         Add a priori or 'constraint' information
         Prefer addAPrioriInformation(inverse(Cov), inverse(Cov)*X); */
   void SRI::addAPriori(const Matrix<double>& Cov, const Vector<double>& X)
   {
      if (Cov.rows() != Cov.cols() || Cov.rows() != R.rows() ||
          X.size() != R.rows())
      {
         MatrixException me(
            "Invalid input dimensions:\n  SRI has dimension " +
            asString<int>(R.rows()) + ",\n  while input is Cov(" +
            asString<int>(Cov.rows()) + "x" + asString<int>(Cov.cols()) +
            ") and X(" + asString<int>(X.size()) + ").");
         GNSSTK_THROW(me);
      }
 
      try
      {
         Matrix<double> InvCov = inverseLUD(Cov);
         addAPrioriInformation(InvCov, X);
      }
      catch (MatrixException& me)
      {
         GNSSTK_THROW(me);
      }
   }
 
      // -----------------------------------------------------------------------------
   void SRI::addAPrioriInformation(const Matrix<double>& InvCov,
                                   const Vector<double>& X)
   {
      if (InvCov.rows() != InvCov.cols() || InvCov.rows() != R.rows() ||
          X.size() != R.rows())
      {
         MatrixException me(
            "Invalid input dimensions:\n  SRI has dimension " +
            asString<int>(R.rows()) + ",\n  while input is InvCov(" +
            asString<int>(InvCov.rows()) + "x" + asString<int>(InvCov.cols()) +
            ") and X(" + asString<int>(X.size()) + ").");
         GNSSTK_THROW(me);
      }
 
      try
      {
         Matrix<double> L(lowerCholesky(InvCov));
         Matrix<double> apR(transpose(L)); // R = UT(inv(Cov))
         Vector<double> apZ(apR * X);      // Z = R*X
         SrifMU(R, Z, apR, apZ);
      }
      catch (MatrixException& me)
      {
         GNSSTK_THROW(me);
      }
   }
 
      // -----------------------------------------------------------------------------
   void SRI::getConditionNumber(double& small, double& big) const
   {
      try
      {
         small = big = 0.0;
         const int n = R.rows();
         if (n == 0)
         {
            return;
         }
         SVD<double> svd;
         svd(R);
         svd.sort(true); // now the last s.v. is the smallest
         small = svd.S(n - 1);
         big   = svd.S(0);
      }
      catch (MatrixException& me)
      {
         me.addText("Called by getConditionNumber");
         GNSSTK_RETHROW(me);
      }
   }
 
      /* -----------------------------------------------------------------------------
         Compute state without computing covariance. Use the fact that R is upper
         triangular. Throw if and when a zero diagonal element is found; values at
         larger index are still valid. On output *ptr is the largest singular index */
   void SRI::getState(Vector<double>& X, int *ptr) const
   {
      const int n = Z.size();
      X           = Vector<double>(n, 0.0);
      if (ptr)
      {
         *ptr = -1;
      }
      if (n == 0)
      {
         return;
      }
      int i, j;
      for (i = n - 1; i >= 0; i--)
      { // loop over rows, in reverse order
         if (R(i, i) == 0.0)
         {
            if (ptr)
            {
               *ptr = i;
            }
            MatrixException me(
               "Singular matrix; zero diagonal element at index " +
               asString<int>(i));
            GNSSTK_THROW(me);
         }
         double sum = Z(i);
         for (j = i + 1; j < n; j++) // sum over columns to right of diagonal
         {
            sum -= R(i, j) * X(j);
         }
         X(i) = sum / R(i, i);
      }
   }
 
      /* -----------------------------------------------------------------------------
         get the state X and the covariance matrix C of the state, where
         C = transpose(inverse(R))*inverse(R) and X = inverse(R) * Z.
         Throws MatrixException if R is singular. */
   void SRI::getStateAndCovariance(Vector<double>& X, Matrix<double>& C,
                                   double *ptrSmall, double *ptrBig) const
   {
      try
      {
         double small, big;
         Matrix<double> invR(inverseUT(R, &small, &big));
         if (ptrSmall)
         {
            *ptrSmall = small;
         }
         if (ptrBig)
         {
            *ptrBig = big;
         }
 
            // cout << " small is " << scientific << setprecision(3) << small
            //   << " and big is " << big;
            // cout << " exponent is " << ::log(big) - ::log(small) << endl;
            // how best to test?
            //  ::log(big) - ::log(small) + 1 >=
            //  numeric_limits<double>::max_exponent
         if (small <= 10 * numeric_limits<double>::epsilon())
         {
            MatrixException me("Singular matrix: condition number is " +
                               asString<double>(big) + " / " +
                               asString<double>(small));
            GNSSTK_THROW(me);
         }
 
         C = UTtimesTranspose(invR);
         X = invR * Z;
      }
      catch (MatrixException& me)
      {
         GNSSTK_RETHROW(me);
      }
      catch (VectorException& ve)
      {
         GNSSTK_RETHROW(ve);
      }
   }
 
   //---------------------------------------------------------------------------------
      // output operator
   ostream& operator<<(ostream& os, const SRI& S)
   {
      Namelist NLR(S.names);
      Namelist NLC(S.names);
      NLC += string("State");
      Matrix<double> A;
      A = S.R || S.Z;
      LabeledMatrix LM(NLR, NLC, A);
 
      ios_base::fmtflags flags = os.flags();
      if (flags & ios_base::scientific)
      {
         LM.scientific();
      }
      LM.setw(os.width());
      LM.setprecision(os.precision());
      LM.clean();
         // LM.message("NL");
         // LM.linetag("tag");
 
      os << LM;
 
      return os;
   }
 
} // end namespace gnsstk
 
//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------