SRIFilter Class Reference

#include <SRIFilter.hpp>

Inheritance diagram for SRIFilter:

[legend]Collaboration diagram for SRIFilter:

[legend]List of all members.

---

Detailed Description

class SRIFilter inherits SRI and implements a square root information filter, which is the square root formulation of the Kalman filter algorithm.

SRIFilter may be used for Kalman filtering, smoothing, or for simple least squares, including weighted, linear or linearized, robust and/or sequential algorithms.

At any point the state X and covariance P are related to the SRI by X = inverse(R) * z , P = inverse(R) * inverse(transpose(R)), or R = upper triangular square root (Cholesky decomposition) of the inverse of P, and z = R * X.

The SRIFilter implements Kalman filter algorithm, which includes sequential least squares (measurement update), dynamic propagation (time update), and smoothing (technically the term 'Kalman filter algorithm' is reserved for the classical algorithm just as Kalman presented it, in terms of a state vector and its covariance matrix).

The SRIFilter measurment update (which is actually just linear least squares) is half of the SRIFilter (Kalman filter) - there is a 'time update' that propagates the SRI (and thus the state and covariance) forward in time using the dynamical model of the filter. These are algebraically equivalent to the classical Kalman algorithm, but are more efficient and numerically stable (actually the Kalman algorithm has been shown to be numerically unstable!). The SRIFilter smoothing algorithms consists of a 'backwards' filter, implemented by applying a 'smoother update' to the SRIFilter at each point in reverse order.

Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential Estimation," Academic Press, 1977.

Definition at line 93 of file SRIFilter.hpp.

Public Member Functions
	SRIFilter (void) throw ()
	empty constructor
	SRIFilter (const unsigned int N) throw ()
	constructor given the dimension N.
	SRIFilter (const Namelist &NL) throw ()
	constructor given a Namelist; its dimension determines the SRI dimension.
	SRIFilter (const Matrix< double > &R, const Vector< double > &Z, const Namelist &NL) throw (MatrixException)
	explicit constructor - throw if the dimensions are inconsistent.
	SRIFilter (const SRIFilter &right) throw ()
	copy constructor
SRIFilter &	operator= (const SRIFilter &right) throw ()
	operator=
void	measurementUpdate (const Matrix< double > &H, Vector< double > &D, const Matrix< double > &CM=SRINullMatrix) throw (MatrixException,VectorException)
	SRIF (Kalman) simple linear measurement update with optional weight matrix.
void	timeUpdate (Matrix< double > &PhiInv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &zw, Matrix< double > &Rwx) throw (MatrixException)
	SRIF (Kalman) time update This routine uses the Householder transformation to propagate the SRIFilter state and covariance through a time step.
void	smootherUpdate (Matrix< double > &Phi, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &zw, Matrix< double > &Rwx) throw (MatrixException)
	SRIF (Kalman) smoother update This routine uses the Householder transformation to propagate the SRIF state and covariance through a smoother (backward filter) step.
void	zeroAll (void)
	remove all stored information by setting the SRI to zero (does not re-dimension).
void	Reset (const int N=0)
	reset the computation, i.e.
Static Public Member Functions
void	DMsmootherUpdate (Matrix< double > &P, Vector< double > &X, Matrix< double > &Phinv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx) throw (MatrixException)
	Covariance/State version of the Kalman smoother update (Dyer-McReynolds).
void	DMsmootherUpdateWithControl (Matrix< double > &P, Vector< double > &X, Matrix< double > &Phinv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx, Vector< double > &U) throw (MatrixException)
	Modification for case with control vector: Xj+1 = Phi*Xj + Gwj + u.

---

Constructor & Destructor Documentation

SRIFilter (  void   )  throw ()

empty constructor Definition at line 65 of file SRIFilter.cpp.

SRIFilter (  const unsigned int  N  )  throw ()

constructor given the dimension N. Parameters: N  dimension of the SRIFilter. Definition at line 73 of file SRIFilter.cpp.

SRIFilter (  const Namelist &  NL  )  throw ()

constructor given a Namelist; its dimension determines the SRI dimension. Parameters: NL  Namelist for the SRIFilter. Definition at line 84 of file SRIFilter.cpp.

SRIFilter (  const Matrix< double > &  R, const Vector< double > &  Z, const Namelist &  NL )  throw (MatrixException)

explicit constructor - throw if the dimensions are inconsistent. Parameters: R  Initial information matrix, an upper triangular matrix of dim N. Z  Initial information data vector, of length N. NL  Namelist for the SRIFilter, also of length N. Exceptions: MatrixException  if dimensions are not consistent. Definition at line 96 of file SRIFilter.cpp. References GPSTK_THROW.

SRIFilter (  const SRIFilter &  right  )  throw () [inline]

copy constructor Parameters: right  SRIFilter to be copied Definition at line 120 of file SRIFilter.hpp.

---

Member Function Documentation

void DMsmootherUpdate (  Matrix< double > &  P, Vector< double > &  X, Matrix< double > &  Phinv, Matrix< double > &  Rw, Matrix< double > &  G, Vector< double > &  Zw, Matrix< double > &  Rwx )  throw (MatrixException) [static]

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; it is equivalent to the SRI form implemented in SRIFilter::smootherUpdate(). NB. This routine does NOT use the SRIFilter object; it is implemented as a member function to be consistent with other updates. Parameters: X  Vector<double> X(N) A priori state, derived from SRI (R*X=Z) P  Matrix<double> P(N,N) A priori covariance, derived from SRI (P=R^-1*R^-T) Rw  Matrix<double> Rw(Ns,Ns) Process noise covariance (UT), output of SRIF TU Rwx  Matrix<double> Rwx(Ns,N) PN 'cross term', output of SRIF TU Zw  Vector<double> Zw(Ns) Process noise state, output of SRIF TU Phinv  Matrix<double> Phinv(N,N) Inverse of state transition, saved at SRIF TU G  Matrix<double> G(N,Ns) Noise coupling matrix, saved at SRIF TU Exceptions: MatrixException  if the input is inconsistent Returns: void On return, X and P are the updated state and covariance, and the other inputs are trashed. 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.) Besides being more efficient, it requires the inverse of the state transition matrix, which is what was used in the time update (the SRIS requires the non-inverse state transition matrix). 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. Definition at line 206 of file SRIFilter.cpp. References GPSTK_RETHROW.

void DMsmootherUpdateWithControl (  Matrix< double > &  P, Vector< double > &  X, Matrix< double > &  Phinv, Matrix< double > &  Rw, Matrix< double > &  G, Vector< double > &  Zw, Matrix< double > &  Rwx, Vector< double > &  U )  throw (MatrixException) [static]

Modification for case with control vector: Xj+1 = Phi*Xj + Gwj + u.

void measurementUpdate (  const Matrix< double > &  H, Vector< double > &  D, const Matrix< double > &  CM = SRINullMatrix )  throw (MatrixException,VectorException)

SRIF (Kalman) simple linear measurement update with optional weight matrix. Parameters: H  Partials matrix, dimension MxN. D  Data vector, length M; on output D is post-fit residuals. CM  Measurement covariance matrix, dimension MxM. Exceptions: if  dimension N does not match dimension of SRI, or if other dimensions are inconsistent, or if CM is singular. Definition at line 133 of file SRIFilter.cpp. References GPSTK_RETHROW, GPSTK_THROW, gpstk::inverse(), Cholesky::L, gpstk::SrifMU(), and gpstk::SRINullMatrix.

SRIFilter & operator= (  const SRIFilter &  right  )  throw ()

operator= Parameters: right  SRIFilter to be copied Definition at line 120 of file SRIFilter.cpp.

void Reset (  const int  N = 0  )

reset the computation, i.e. remove all stored information, and optionally change the dimension. If N is not input, the dimension is not changed. Parameters: N  new SRIFilter dimension (optional). Definition at line 231 of file SRIFilter.cpp. References Vector::resize(), Matrix::resize(), and Matrix::rows().

void smootherUpdate (  Matrix< double > &  Phi, Matrix< double > &  Rw, Matrix< double > &  G, Vector< double > &  zw, Matrix< double > &  Rwx )  throw (MatrixException)

SRIF (Kalman) smoother update This routine uses the Householder transformation to propagate the SRIF state and covariance through a smoother (backward filter) step. If the existing SRI state is of dimension N, and the number of noise parameter is Ns, then the inputs must be dimensioned as indicated. Parameters: Phi  Matrix<double> State transition matrix, an N by N matrix. Phi is destroyed on output. Rw  Matrix<double> A priori square root information matrix for the process noise, an Ns by Ns upper triangular matrix (which has Ns(Ns+1)/2 elements), output of the time update. G  Matrix<double> The n by ns matrix associated with process noise, which relates the state to the noise variables. The process noise covariance is G*Q*transpose(G) where inverse(Q) is transpose(Rw)*Rw, also input to the time update. G is destroyed on output. zw  Vector<double> A priori 'state' associated with the process noise, a vector with Ns elements, output of the time update. Rwx  Matrix<double> An Ns by N matrix, output of the time update. 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. All the inputs are trashed on output. Exceptions: MatrixException  if the input is inconsistent Returns: void 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. 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 = inverse(R) * z , P = inverse(R) * inverse(transpose(R)).] 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. Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential Estimation," Academic Press, 1977, pg 216. Definition at line 194 of file SRIFilter.cpp. References GPSTK_RETHROW.

void timeUpdate (  Matrix< double > &  PhiInv, Matrix< double > &  Rw, Matrix< double > &  G, Vector< double > &  zw, Matrix< double > &  Rwx )  throw (MatrixException)

SRIF (Kalman) time update This routine uses the Householder transformation to propagate the SRIFilter state and covariance through a time step. If the existing SRI state is of dimension n, and the number of noise parameter is ns, then the inputs must be dimensioned as indicated. Parameters: PhiInv  Matrix<double> Inverse of state transition matrix, an n by n matrix. PhiInv is destroyed on output. Rw  Matrix<double> a priori square root information matrix for the process noise, an ns by ns upper triangular matrix. Rw is modified on output. G  Matrix<double> The n by ns matrix associated with process noise, which relates the state to the noise variables. The process noise covariance is G*Q*transpose(G) where inverse(Q) is transpose(Rw)*Rw. G is destroyed on output. Zw  Vector<double> a priori 'state' associated with the process noise, a vector with ns elements. Usually set to zero by the calling routine (for unbiased process noise). Used for output. Rwx  Matrix<double> An ns by n matrix which is set to zero by this routine and used for output. Output: The updated square root information matrix and SRIF state (R,Z) and the matrices which are used in smoothing: Rw, Zw, Rwx. Exceptions: MatrixException  if the input is inconsistent Returns: void Method: This SRIF time update method treats the process noise and mapping information as a separate data equation, and applies a Householder transformation to the (appended) equations to solve for an updated state. Thus there is another 'state' variable associated with whatever state variables have process noise. The matrix G relates the process noise variables to the regular state variables, and appears in the term GQtranspose(G) of the covariance. If all n state variables have process noise, then ns=n and G is an n by n matrix. Since some (or all) of the state variables may not have process noise, ns may be zero. [Ref. Bierman ftnt pg 122 seems to indicate that variables with zero process noise can be handled by ns=n & setting a column of G=0. But note that the case of the matrix G=0 is the same as ns=0, because the first ns columns would be zero below the diagonal in that case anyway, so the HH transformation would be null.] For startup, all of the a priori information and state arrays may be zero. That is, "no information" would imply that R and Z are zero, as well as Zw. A priori information (covariance) and state are handled by setting P = inverse(R)*transpose(inverse((R)), Z = R*X. Normally Zw is reset to zero before each call. There are three ways to handle non-zero process noise covariance. (1) If Q is the (known) a priori process noise covariance Q, then set Q=Rw(-1)*Rw(-T), and G=1. (2) Transform process noise covariance matrix to UDU form, Q=UDU, then set G=U and Rw = (D)**-1/2. (3) Take the sqrt of process noise covariance matrix Q, then set G=this sqrt and Rw = 1. [2 and 3 have been tested.] The routine applies a Householder transformation to a large matrix formed by concatenation of the input matricies. Two preliminary steps are to form Rd = R*PhiInv (stored in PhiInv) and -Rd*G (stored in G) by matrix multiplication, and to set Rwx to the zero matrix. Then the Householder transformation is applied to the following matrix, dimensions are shown in (): _ (ns) (n) (1) _ _ _ (ns) | Rw 0 Zw | ==> | Rw Rwx Zw | (n) | -Rd*G Rd Z | ==> | 0 R Z | . - - - The SRI matricies R and Rw remain upper triangular. The matrix Rwx is related to the sensitivity of the state estimate to the unmodeled parameters in Zw. The sensitivity matrix is Sen = -inverse(Rw)*Rwx, where perturbation in model X = Sen * diagonal(a priori sigmas of parameter uncertainties). The quantities Rw, Rwx and Zw on output are to be saved and used in the sqrt information fixed interval smoother (SRIS), during the backward filter process. ------------------------------------------------------------------- Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential Estimation," Academic Press, 1977, pg 121. Definition at line 181 of file SRIFilter.cpp. References GPSTK_RETHROW.

void zeroAll (  void   )

remove all stored information by setting the SRI to zero (does not re-dimension). Definition at line 221 of file SRIFilter.cpp.

---

The documentation for this class was generated from the following files:

• SRIFilter.hpp
• SRIFilter.cpp

---