void SRI::reshape(const Namelist& NL)      throw(MatrixException,VectorException)   {      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) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_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)      throw(MatrixException,VectorException)   {      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,k,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..)            k = all.index(names.labels[j]);            if(k == -1) {               MatrixException me("Algorithm error 1");               GPSTK_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++) {            k = all.index(S.names.labels[j]);            if(k == -1) {               MatrixException me("Algorithm error 2");               GPSTK_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         Householder<double> HA;         HA(A);         // submatrix args are matrix,toprow,topcol,numrows,numcols         R = Matrix<double>(HA.A,0,0,n,n);         Vector<double> T = Vector<double>(HA.A.colCopy(n));         Z = Vector<double>(T,0,n);         names = all;         return *this;      }      catch(MatrixException& me) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_RETHROW(ve);      }   }   // --------------------------------------------------------------------------------   // merge two SRIs to produce a third. ? should this be operator&() ?   SRI operator+(const SRI& Sleft,                 const SRI& Sright)      throw(MatrixException,VectorException)   {      try {         SRI S(Sleft);         S += Sright;         return S;      }      catch(MatrixException& me) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_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)      throw()   {      if(n >= R.rows())         return;      //TD this is not right -- you should permute the element      //to the first row, then zero      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 int n)      throw()   {      if(n <= 0) {         R = 0.0;         Z = 0.0;         return;      }      if(n >= 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)      throw(MatrixException)   {      if(X0.size() != R.cols()) {         using namespace StringUtils;                  MatrixException me("Invalid input dimension: SRI has dimension "               + asString<int>(R.rows()) + " while input has length "               + asString<int>(X0.size())               );         GPSTK_THROW(me);      }      Z = Z - R * X0;   }   // --------------------------------------------------------------------------------   // Transform this SRI with the transformation matrix T;   // i.e. R -> T * R * inverse(T) and Z -> T * Z. The matrix inverse(T)   // may optionally be supplied as input, otherwise it is computed from   // T. NB names in this SRI are most likely changed; but this routine does   // not change the Namelist. Throw MatrixException if the input has   // the wrong dimension or cannot be inverted.   void SRI::transform(const Matrix<double>& T,                       const Matrix<double>& invT)      throw(MatrixException,VectorException)   {      if(T.rows() != R.rows() ||         T.cols() != R.cols() ||         (&invT != &SRINullMatrix && (invT.rows() != R.rows() ||         invT.cols() != R.cols()))) {            using namespace StringUtils;                     MatrixException me("Invalid input dimension:\n  SRI has dimension "               + asString<int>(R.rows()) + " while T has dimension "               + asString<int>(T.rows()) + "x"               + asString<int>(T.cols()));            if(&invT != &SRINullMatrix) me.addText("\n  and invT has dimension "                           + asString<int>(invT.rows()) + "x"                           + asString<int>(invT.cols()));            GPSTK_THROW(me);      }      try {            // get the inverse matrix         Matrix<double> Ti(T);         if(&invT == &SRINullMatrix)            Ti = inverseSVD(T);         else            Ti = invT;            // transform         Matrix<double> B = T * R * Ti;         Vector<double> Q = T * Z;            // re-triangularize         R = 0.0;         Z = 0.0;         SrifMU(R,Z,B,Q);      }      catch(MatrixException& me) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_RETHROW(ve);      }   }   // --------------------------------------------------------------------------------   // Transform the state by the transformation matrix T; i.e. X -> T*X,   // without transforming the SRI; this is done by right multiplying R by   // inverse(T), which is the input. Thus R -> R*inverse(T),   // so R*inverse(T)*T*X = Z.  Input is the _inverse_ of the transformation.   // throw MatrixException if input dimensions are wrong.   void SRI::transformState(const Matrix<double>& invT)      throw(MatrixException)   {      if(invT.rows() != R.rows() || invT.cols() != R.rows()) {         using namespace StringUtils;         MatrixException me("Invalid input dimension: SRI has dimension "            + asString<int>(R.rows()) + " while invT has dimension "            + asString<int>(invT.rows()) + "x"            + asString<int>(invT.cols()));         GPSTK_THROW(me);      }         // transform      Matrix<double> A = R * invT;         // re-triangularize      Householder<double> HA;      HA(A);      R = HA.A;   }   // --------------------------------------------------------------------------------   // 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,                   const double& q)      throw(MatrixException,VectorException)   {      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         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) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_RETHROW(ve);      }   }   // --------------------------------------------------------------------------------   // 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::biasFix(const unsigned int& in,                     const double& bias)      throw(MatrixException,VectorException)   {      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;         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);