jj++;            }            ii++;         }         R = Rnew;         Z = Znew;         names -= names.labels[in];      }      catch(MatrixException& me) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_RETHROW(ve);      }   }   //---------------------------------------------------------------------------------   // Add a priori or 'constraint' information   void SRI::addAPriori(const Matrix<double>& Cov, const Vector<double>& X)      throw(MatrixException)   {      if(Cov.rows() != Cov.cols() || Cov.rows() != R.rows() || X.size() != R.rows()) {         using namespace StringUtils;                  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()) + ")."            );         GPSTK_THROW(me);      }      try {         Matrix<double> invcovar;         Vector<double> zstate;            invcovar = inverse(Cov);         Cholesky<double> Ch;         Ch(invcovar);         zstate = Ch.U * X;            // R = UT(inv(Cov)) and z = R*X         SrifMU(R, Z, Ch.U, zstate);      }      catch(MatrixException& me) {         GPSTK_THROW(me);      }   }   // --------------------------------------------------------------------------------   // 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.   // NB this is the most efficient way to invert the SRI problem.   void SRI::getStateAndCovariance(Vector<double>& X,                                   Matrix<double>& C,                                   double *ptrSmall,                                   double *ptrBig)      throw(MatrixException,VectorException)   {      try {         Matrix<double> invR;         invR = inverseUT(R,ptrSmall,ptrBig);         C = UTtimesTranspose(invR);         X = invR * Z;      }      catch(MatrixException& me) {         GPSTK_RETHROW(me);      }      catch(VectorException& ve) {         GPSTK_RETHROW(ve);      }   }   //---------------------------------------------------------------------------------   // output operator   ostream& operator<<(ostream& os, const SRI& S)   {      Namelist NL(S.names);      NL += string("State");      Matrix<double> A;      A = S.R || S.Z;      LabelledMatrix LM(NL,A);      LM.setw(os.width());      LM.setprecision(os.precision());      os << LM;      return os;   }   //---------------------------------------------------------------------------------   // This routine uses the Householder algorithm to update the SRIFilter   // state and covariance.   // Input:   //    R  a priori SRI matrix (upper triangular, dimension N)   //    Z  a priori SRI data vector (length N)   //    A  concatentation of H and D : A = H || D, where   //    H  Measurement partials, an M by N matrix.   //    D  Data vector, of length M   //       H and D may have row dimension > M; then pass M:   //    M  (optional) Row dimension of H and D   // Output:   //    Updated R and Z.  H is trashed, but the data vector D   //    contains the residuals of fit (D - A*state).   // Return values:   //    SrifMU returns void, but throws exceptions if the input matrices   // or vectors have incompatible dimensions.   //    // Measurment noise associated with H and D must be white   // with unit covariance.  If necessary, the data can be 'whitened'   // before calling this routine in order to satisfy this requirement.   // This is done as follows.  Compute the lower triangular square root    // of the covariance matrix, L, and replace H with inverse(L)*H and   // D with inverse(L)*D.   //    //    The Householder transformation is simply an orthogonal   // transformation designed to make the elements below the diagonal   // zero.  It works by explicitly performing the transformation, one   // column at a time, without actually constructing the transformation   // matrix.  Let y be column k of the input matrix.  y can be zeroed   // below the diagonal as follows:  let sum=sign(y(k))*sqrt(y*y), and   // define vector u(k)=y(k)+sum, u(j)=y(j) (j.gt.k).  This defines the   // transformation matrix as (1-bu*u), with b=2/u*u=1/sum*u(k).   // Redefine y(k)=u(k) and apply the transformation to elements of the   // input matrix below and to the right of the (k,k) element.  This    // algorithm for each column k=0,n-1 in turn is equivalent to a single   // orthogonal transformation which triangularizes the matrix.   //   // Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential   //      Estimation," Academic Press, 1977.   template <class T>   void SrifMU(Matrix<T>& R,               Vector<T>& Z,               Matrix<T>& A,               unsigned int M)      throw(MatrixException)   {      if(A.cols() <= 1 || A.cols() != R.cols()+1 || Z.size() < R.rows()) {         if(A.cols() > 1 && R.rows() == 0 && Z.size() == 0) {            // create R and Z            R = Matrix<double>(A.cols()-1,A.cols()-1,0.0);            Z = Vector<double>(A.cols()-1,0.0);         }         else {            using namespace StringUtils;                        MatrixException me("Invalid input dimensions:\n  R has dimension "               + asString<int>(R.rows()) + "x"               + asString<int>(R.cols()) + ",\n  Z has length "               + asString<int>(Z.size()) + ",\n  and A has dimension "               + asString<int>(A.rows()) + "x"               + asString<int>(A.cols()));            GPSTK_THROW(me);         }      }      const T EPS=-T(1.e-200);      unsigned int m=M, n=R.rows();      if(m==0 || m > A.rows()) m=A.rows();      unsigned int np1=n+1;         // if np1 = n, state vector Z is not updated      unsigned int i,j,k;      T dum, delta, beta;      for(j=0; j<n; j++) {          // loop over columns         T sum = T(0);         for(i=0; i<m; i++)            sum += A(i,j)*A(i,j);   // sum squares of elements in this column         if(sum <= T(0)) continue;         dum = R(j,j);         sum += dum * dum;         sum = (dum > T(0) ? -T(1) : T(1)) * ::sqrt(sum);         delta = dum - sum;         R(j,j) = sum;         if(j+1 > np1) break;         beta = sum*delta;         if(beta > EPS) continue;         beta = T(1)/beta;         for(k=j+1; k<np1; k++) {   // columns to right of diagonal            sum = delta * (k==n ? Z(j) : R(j,k));            for(i=0; i<m; i++)               sum += A(i,j) * A(i,k);            if(sum == T(0)) continue;            sum *= beta;            if(k==n) Z(j) += sum*delta;            else   R(j,k) += sum*delta;            for(i=0; i<m; i++)               A(i,k) += sum * A(i,j);         }      }   }  // end SrifMU       //---------------------------------------------------------------------------------   // This is simply SrifMU(R,Z,A) with H and D passed in rather   // than concatenated into a single Matrix A = H || D.   template <class T>   void SrifMU(Matrix<T>& R,               Vector<T>& Z,               Matrix<T>& H,               Vector<T>& D,               unsigned int M)      throw(MatrixException)   {      Matrix<double> A;      try { A = H || D; }      catch(MatrixException& me) { GPSTK_RETHROW(me); }      SrifMU(R,Z,A,M);         // copy residuals out of A into D      D = Vector<double>(A.colCopy(A.cols()-1));   }   //---------------------------------------------------------------------------------   // Invert the upper triangular matrix stored in the square matrix UT, using a very   // efficient algorithm. Throw MatrixException if the matrix is singular.   // If the pointers are defined, on exit (but not if an exception is thrown),   // they return the smallest and largest eigenvalues of the matrix.   template <class T>   Matrix<T> inverseUT(const Matrix<T>& UT,                       T *ptrSmall,                       T *ptrBig)      throw(MatrixException)   {      if(UT.rows() != UT.cols() || UT.rows() == 0) {         using namespace StringUtils;                  MatrixException me("Invalid input dimensions: "               + asString<int>(UT.rows()) + "x"               + asString<int>(UT.cols()));         GPSTK_THROW(me);      }      unsigned int i,j,k,n=UT.rows();      T big,small,sum,dum;      Matrix<T> Inv(UT);         // start at the last row,col      dum = UT(n-1,n-1);      if(dum == T(0))         throw SingularMatrixException("Singular matrix");      big = small = dum;      Inv(n-1,n-1) = T(1)/dum;      if(n == 1) return Inv;                 // 1x1 matrix      for(i=0; i<n-1; i++) Inv(n-1,i)=0;         // now move to rows i = n-2 to 0      for(i=n-2; i>=0; i--) {         if(UT(i,i) == T(0))            throw SingularMatrixException("Singular matrix");         if(fabs(UT(i,i)) > big) big = fabs(UT(i,i));         if(fabs(UT(i,i)) < small) small = fabs(UT(i,i));         dum = T(1)/UT(i,i);         Inv(i,i) = dum;                        // diagonal element first            // now do off-diagonal elements (i,i+1) to (i,n-1)         for(j=i+1; j<n; j++) {            sum = T(0);            for(k=i+1; k<=j; k++)               sum += Inv(k,j) * UT(i,k);            Inv(i,j) = - sum * dum;         }         for(j=0; j<i; j++) Inv(i,j)=0;         if(i==0) break;         // NB i is unsigned, hence 0-1 = 4294967295!      }      if(ptrSmall) *ptrSmall=small;      if(ptrBig) *ptrBig=big;      return Inv;   }   //---------------------------------------------------------------------------------   // Given an upper triangular matrix UT, compute the symmetric matrix   // UT * transpose(UT) using a very efficient algorithm.   template <class T>   Matrix<T> UTtimesTranspose(const Matrix<T>& UT)      throw(MatrixException)   {      unsigned int n=UT.rows();      if(n == 0 || UT.cols() != n) {         using namespace StringUtils;         MatrixException me("Invalid input dimensions: "               + asString<int>(UT.rows()) + "x"               + asString<int>(UT.cols()));         GPSTK_THROW(me);      }      unsigned int i,j,k;      T sum;      Matrix<T> S(n,n);      for(i=0; i<n-1; i++) {        // loop over rows of UT, except the last         sum = T(0);                // diagonal element (i,i)         for(j=i; j<n; j++)            sum += UT(i,j)*UT(i,j);         S(i,i) = sum;         for(j=i+1; j<n; j++) {     // loop over columns to right of (i,i)            sum = T(0);            for(k=j; k<n; k++)               sum += UT(i,k) * UT(j,k);            S(i,j) = S(j,i) = sum;         }      }      S(n-1,n-1) = UT(n-1,n-1)*UT(n-1,n-1);   // the last diagonal element      return S;   }} // end namespace gpstk//------------------------------------------------------------------------------------