sri.cpp

linux的gps应用

               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

//------------------------------------------------------------------------------------