srifilter.cpp

1 gps基本算法

      //---------------------------------------------------------------
      for(j=0; j<n; j++) {                // loop over columns of Rwx and Phi
         sum = T(0);
         for(i=j+1; i<n; i++)             // rows of Phi
            sum += Phi(i,j)*Phi(i,j);
         dum = Phi(j,j);
         sum += dum*dum;
         sum = (dum > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
         delta = dum - sum;
         Phi(j,j) = sum;
         beta = sum*delta;
         if(beta > EPS) continue;
         beta = T(1)/beta;

            // apply jth Householder transformation to columns of Phi on row j
         for(k=j+1; k<n; k++) {           // columns of Phi
            sum = delta * Phi(j,k);
            for(i=j+1; i<n; i++)
               sum += Phi(i,j)*Phi(i,k);
            if(sum == T(0)) continue;
            sum *= beta;
            Phi(j,k) += sum*delta;
            for(i=j+1; i<n; i++)
               Phi(i,k) += sum * Phi(i,j);
         }

            // apply jth Householder transformation to Z
         sum = delta *Z(j);
         for(i=j+1; i<n; i++)
            sum += Z(i) * Phi(i,j);
         if(sum == T(0)) continue;
         sum *= beta;
         Z(j) += sum*delta;
         for(i=j+1; i<n; i++)
            Z(i) += sum * Phi(i,j);
      }                                   // end loop over cols of Rwx and Phi

         // copy transformed R out of Phi
       for(j=0; j<n; j++)
          for(i=0; i<=j; i++)
             R(i,j) = Phi(i,j);

   }
   catch(MatrixException& me) { GPSTK_RETHROW(me); }
}  // end SrifTU

//------------------------------------------------------------------------------------
// Kalman smoother update.
// This routine uses the Householder transformation to propagate the SRIF
// state and covariance through a smoother (backward filter) step.
// Input:
// R     A priori square root information (SRI) matrix (an N by N 
//          upper triangular matrix)
// z     a priori SRIF state vector, an N vector (state is x, z = R*x).
// Phi   State transition matrix, an N by N matrix. Phi is destroyed on output.
// Rw    A priori square root information matrix for the process
//          noise, an Ns by Ns upper triangular matrix (which has 
//          Ns(Ns+1)/2 elements).
// G     The N by Ns matrix associated with process noise.  The 
//          process noise covariance is GQGtrans where Qinverse 
//          is Rw(trans)*Rw.
// Zw    A priori 'state' associated with the process noise,
//          a vector with Ns elements.
// Rwx   An Ns by N matrix.
//
// The inputs Rw,Zw,Rwx are the output of the SRIF time update, and these and
// Phi and G are associated with the same timestep.
// 
// Output:
//    The updated square root information matrix and SRIF smoothed state (R,z).
// All other inputs are trashed.
// 
// Return values:
//    SrifSU returns void, but throws exceptions if the input matrices
// or vectors have incompatible dimensions or incorrect types.
// 
// Method:
//    The fixed interval square root information smoother (SRIS) is 
// composed of two Kalman filters, one identical with the square root 
// information filter (SRIF), the other similar but operating on the
// data in reverse order and combining the current (smoothed) state
// with elements output by the SRIF in its forward run and saved.
// Thus a smoother is composed of a forward filter which saves all of
// its output, followed by a backward filter which makes use of that
// saved information.
//    This form of the SRIF backward filter algorithm is equivalent to the
// Dyer-McReynolds SRIS algorithm, which uses less computer resources, but
// propagates the state and covariance rather than the SRI (R,z). (As always,
// at any point the state X and covariance P are related to the SRI by
// X = R^-1 * z , P = R^-1 * R^-T.)
//    For startup of the backward filter, the state after the final 
// measurement update of the SRIF is given another time update, the
// output of which is identified with the a priori values for the 
// backward filter.  Backward filtering proceeds from there, the N+1st
// point, toward the first point.
//
//    In this implementation of the backward filter, the Householder
// transformation is applied to the following matrix
// (dimensions are shown in ()):
// 
//       _  (Ns)     (N)      (1) _          _                  _
// (Ns) |  Rw+Rwx*G  Rwx*Phi  Zw   |   ==>  |   Rw   Rwx   Zw    |
// (N)  |  R*G       R*Phi    z    |   ==>  |   0     R    z     | .
//       -                        -          -                  -
// The SRI matricies R and Rw remain upper triangular.
//
//    For the programmer: First create an NsXNs matrix A, then
// Rw+Rwx*G -> A, Rwx*Phi -> Rwx, R*Phi -> Phi, and R*G -> G, and
// the transformation is applied to the matrix:
//
//       _ (Ns)   (N)  (1) _
// (Ns) |   A    Rwx   Zw   |
// (N)  |   G    Phi   z    |
//       -                 -
//
// then the (upper triangular) matrix R is copied out of Phi into R.
// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
//      Estimation," Academic Press, 1977.
template <class T>
void SRIFilter::SrifSU(Matrix<T>& R,
                       Vector<T>& Z,
                       Matrix<T>& Phi,
                       Matrix<T>& Rw,
                       Matrix<T>& G,
                       Vector<T>& Zw,
                       Matrix<T>& Rwx)
   throw(MatrixException)
{
   unsigned int N=R.rows(),Ns=Rw.rows();

   if(Phi.rows() < N || Phi.cols() < N ||
      G.rows() < N || G.cols() < Ns ||
      R.cols() != N ||
      Rwx.rows() < Ns || Rwx.cols() < N ||
      Z.size() < N || Zw.size() < Ns) {
      MatrixException me("Invalid input dimensions:\n  R is "
         + asString<int>(R.rows()) + "x"
         + asString<int>(R.cols()) + ", Z has length "
         + asString<int>(Z.size()) + "\n  Phi is "
         + asString<int>(Phi.rows()) + "x"
         + asString<int>(Phi.cols()) + "\n  Rw is "
         + asString<int>(Rw.rows()) + "x"
         + asString<int>(Rw.cols()) + "\n  G is "
         + asString<int>(G.rows()) + "x"
         + asString<int>(G.cols()) + "\n  Zw has length "
         + asString<int>(Zw.size()) + "\n  Rwx is "
         + asString<int>(Rwx.rows()) + "x"
         + asString<int>(Rwx.cols())
         );
      GPSTK_THROW(me);
   }

   const T EPS=-T(1.e-200);
   int i, j, k;
   T sum, beta, delta, diag;

try {
      // Rw+Rwx*G -> A
   Matrix<T> A;
   A = Rw + Rwx*G;
   Rwx = Rwx * Phi;
   Phi = R * Phi;
   G = R * G;

         //-----------------------------------------
         // HouseHolder Transformation

         // Loop over first Ns columns
   for(j=0; j<Ns; j++) {                  // columns of A
      sum = T(0);
      for(i=j+1; i<Ns; i++) {             // rows i below diagonal in A
         sum += A(i,j) * A(i,j);
      }
      for(i=0; i<N; i++) {                // all rows i in G
         sum += G(i,j) * G(i,j);
      }

      diag = A(j,j);
      sum += diag*diag;
      sum = (diag > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
      delta = diag - sum;
      A(j,j) = sum;
      beta = sum*delta;
      if(beta > EPS) continue;
      beta = T(1)/beta;

            // apply jth HH trans to submatrix below and right of j,j
      for(k=j+1; k<Ns; k++) {                // cols to right of diag
         sum = delta * A(j,k);
         for(i=j+1; i<Ns; i++) {             // rows of A below diagonal
            sum += A(i,j)*A(i,k);
         }
         for(i=0; i<N; i++) {                // all rows of G
            sum += G(i,j)*G(i,k);
         }
         if(sum == T(0)) continue;
         sum *= beta;
            //------------------------------------------
         A(j,k) += sum*delta;

         for(i=j+1; i<Ns; i++) {             // rows of A > j (same loops again)
            A(i,k) += sum * A(i,j);
         }
         for(i=0; i<N; i++) {                // all rows of G (again)
            G(i,k) += sum * G(i,j);
         }
      }

            // apply jth HH trans to Rwx and Phi sub-matrices
      for(k=0; k<N; k++) {                // all columns of Rwx / Phi
         sum = delta * Rwx(j,k);
         for(i=j+1; i<Ns; i++) {          // rows of Rwx below j
            sum += A(i,j) * Rwx(i,k);
         }
         for(i=0; i<N; i++) {             // all rows of Phi
            sum += G(i,j) * Phi(i,k);
         }
         if(sum == T(0)) continue;
         sum *= beta;
         Rwx(j,k) += sum*delta;
         for(i=j+1; i<Ns; i++) {          // rows of Rwx below j (again)
            Rwx(i,k) += sum * A(i,j);
         }
         for(i=0; i<N; i++) {             // all rows of Phi (again)
            Phi(i,k) += sum * G(i,j);
         }
      }

            // apply jth HH trans to Zw and Z
      sum = delta * Zw(j);
      for(i=j+1; i<Ns; i++) {             // rows (elements) of Zw below j
         sum += A(i,j) * Zw(i);
      }
      for(i=0; i<N; i++) {                // all rows (elements) of Z
         sum += Z(i) * G(i,j);
      }
      if(sum == T(0)) continue;
      sum *= beta;
      Zw(j) += sum*delta;
      for(i=j+1; i<Ns; i++) {             // rows of Zw below j (again)
         Zw(i) += sum * A(i,j);
      }
      for(i=0; i<N; i++) {                // all rows of Z (again)
         Z(i) += sum * G(i,j);
      }
   }

         // Loop over columns past the Ns block: all of Rwx and Phi
   for(j=0; j<N; j++) {
      sum = T(0);
      for(i=j+1; i<N; i++) {              // rows of Phi at and below j
         sum += Phi(i,j) * Phi(i,j);
      }
      diag = Phi(j,j);
      sum += diag*diag;
      sum = (diag > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
      delta = diag - sum;
      Phi(j,j) = sum;
      beta = sum*delta;
      if(beta > EPS) continue;
      beta = T(1)/beta;

               // apply HH trans to Phi sub-block below and right of j,j
      for(k=j+1; k<N; k++) {                 // columns k > j
         sum = delta * Phi(j,k);
         cout << "";//gcc 3.4.4 bug
         for(i=j+1; i<N; i++) {              // rows below j
            sum += Phi(i,j) * Phi(i,k);
         }
         if(sum == T(0)) continue;
         sum *= beta;
         Phi(j,k) += sum*delta;
         for(i=j+1; i<N; i++) {              // rows below j (again)
            Phi(i,k) += sum * Phi(i,j);
         }
      }
               // Now apply to the Z column
      sum = delta * Z(j);
      for(i=j+1; i<N; i++) {                 // rows of Z below j
         sum += Z(i) * Phi(i,j);
      }
      if(sum == T(0)) continue;
      sum *= beta;
      Z(j) += sum*delta;
      for(i=j+1; i<N; i++) {                 // rows of Z below j (again)
         Z(i) += sum * Phi(i,j);
      }
   }
      //------------------------------
      // Transformation finished

      //-------------------------------------
      // copy transformed R out of Phi into R
   R = T(0);
   for(j=0; j<N; j++) {
      for(i=0; i<=j; i++) {
         R(i,j) = Phi(i,j);
      }
   }
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}  // end SrifSU

//------------------------------------------------------------------------------
// Covariance/State version of the Kalman smoother update (Dyer-McReynolds).
// This routine implements the Dyer-McReynolds form of the state and covariance
// recursions which constitute the backward filter of the Square Root
// Information Smoother.
// 
// Input: (assume N and Ns are greater than zero)
//		Vector X(N)				A priori state, derived from SRI (R*X=Z)
// 	Matrix P(N,N)			A priori covariance, derived from SRI (P=R^-1*R^-T)
// 	Matrix Rw(Ns,Ns)		Process noise covariance (UT), output of SRIF TU
// 	Matrix Rwx(Ns,N)		PN 'cross term', output of SRIF TU
// 	Vector Zw(Ns)			Process noise state, output of SRIF TU
// 	Matrix Phinv(N,N)		Inverse of state transition, saved at SRIF TU
// 	Matrix G(N,Ns)			Noise coupling matrix, saved at SRIF TU
// Output:
// 	Updated X and P. The other inputs are trashed.
// Return values:
// 	GPSTK::SINGULAR if the Process Noise Matrix (Rw) is singular
// 	GPSTK::OK if ok
// 
// Method:
// 	The fixed interval square root information smoother (SRIS) is 
// composed of two Kalman filters, one identical with the square root 
// information filter (SRIF), the other similar but operating on the
// data in reverse order and combining the current (smoothed) state
// with elements output by the SRIF in its forward run and saved.
// Thus a smoother is composed of a forward filter which saves all of
// its output, followed by a backward filter which makes use of that
// saved information.
// 	This form of the SRIS algorithm is equivalent to the SRIS backward
// filter Householder transformation algorithm, but uses less computer
// resources. It is not necessary to update both the state and the
// covariance, although doing both at once is less expensive than
// doing them separately. (This routine does both.)
// 	For startup of the backward filter, the state after the final 
// measurement update of the SRIF is given another time update, the
// output of which is identified with the a priori values for the 
// backward filter.  Backward filtering proceeds from there, the N+1st
// point, toward the first point.
//
// Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
//      Estimation," Academic Press, 1977.
template <class T>
void SRIFilter::SrifSU_DM(Matrix<T>& P,
                          Vector<T>& X,
                          Matrix<T>& Phinv,
                          Matrix<T>& Rw,
                          Matrix<T>& G,
                          Vector<T>& Zw,
                          Matrix<T>& Rwx)
   throw(MatrixException)
{
   unsigned int N=P.rows(),Ns=Rw.rows();

   if(P.cols() != P.rows() ||
      X.size() != N ||
      Rwx.cols() != N ||
      Zw.size() != Ns ||
      Rwx.rows() != Ns || Rwx.cols() != N ||
      Phinv.rows() != N || Phinv.cols() != N ||
      G.rows() != N || G.cols() != Ns ) {
      MatrixException me("Invalid input dimensions:\n  P is "
         + asString<int>(P.rows()) + "x"
         + asString<int>(P.cols()) + ", X has length "
         + asString<int>(X.size()) + "\n  Phinv is "
         + asString<int>(Phinv.rows()) + "x"
         + asString<int>(Phinv.cols()) + "\n  Rw is "
         + asString<int>(Rw.rows()) + "x"
         + asString<int>(Rw.cols()) + "\n  G is "
         + asString<int>(G.rows()) + "x"
         + asString<int>(G.cols()) + "\n  Zw has length "
         + asString<int>(Zw.size()) + "\n  Rwx is "
         + asString<int>(Rwx.rows()) + "x"
         + asString<int>(Rwx.cols())
         );
      GPSTK_THROW(me);
   }

try {
   G = G * inverse(Rw);
   Matrix<T> F;
   F = ident<T>(N) + G*Rwx;
   // update X
   Vector<T> C;
   C = F*X - G*Zw;
   X = Phinv * C;
   // update P
   P = F*P*transpose(F) + G*transpose(G);
   P = Phinv*P*transpose(Phinv);
}
catch(Exception& e) { GPSTK_RETHROW(e); }
} // end SrifSU_DM

//------------------------------------------------------------------------------------
} // end namespace gpstk