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.

#include <SRIFilter.hpp>

Inheritance diagram for gnsstk::SRIFilter:

[legend]

Public Member Functions
void	measurementUpdate (const Matrix< double > &H, Vector< double > &D, const Matrix< double > &CM=SRINullMatrix)

void	measurementUpdate (const SparseMatrix< double > &H, Vector< double > &D, const SparseMatrix< double > &CM=SRINullSparseMatrix)

SRIFilter &	operator= (const SRIFilter &right)

void	reset (const int N=0)

void	smootherUpdate (Matrix< double > &Phi, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx)

	SRIFilter ()
	empty constructor More...

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

	SRIFilter (const Namelist &NL)

	SRIFilter (const SRIFilter &right)

	SRIFilter (const unsigned int N)

void	timeUpdate (Matrix< double > &PhiInv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx)

void	zeroAll ()

Public Member Functions inherited from gnsstk::SRI
void	addAPriori (const Matrix< double > &Cov, const Vector< double > &X)

void	addAPrioriInformation (const Matrix< double > &ICov, const Vector< double > &X)

SRI &	append (const SRI &S)

void	getConditionNumber (double &small, double &big) const

std::string	getName (const unsigned int in) const

Namelist	getNames () const

Matrix< double >	getR () const

void	getState (Vector< double > &X, int *ptrSingularIndex=NULL) const

void	getStateAndCovariance (Vector< double > &X, Matrix< double > &C, double ptrSmall=NULL, double ptrBig=NULL) const

Vector< double >	getZ () const

unsigned int	index (std::string &name)

void	measurementUpdate (Matrix< double > &Partials, Vector< double > &Data)

void	measurementUpdate (SparseMatrix< double > &Partials, Vector< double > &Data)

void	merge (const SRI &S)

SRI &	operator+= (const Namelist &NL)

SRI &	operator+= (const SRI &S)

SRI &	operator= (const SRI &right)
	operator= More...

void	permute (const Namelist &NL)

void	Qbump (const unsigned int in, double q=0.0)

void	reshape (const Namelist &NL)

void	retriangularize (const Matrix< double > &A)

void	retriangularize (Matrix< double > RR, Vector< double > ZZ)

void	setFromCovState (const Matrix< double > &Cov, const Vector< double > &State, const Namelist &NL)

bool	setName (const unsigned int in, const std::string &label)

void	shift (const Vector< double > &X0)

void	shiftZ (const Vector< double > &Z0)

unsigned int	size () const

void	split (const Namelist &NL, SRI &S)

	SRI ()
	empty constructor More...

	SRI (const Matrix< double > &R, const Vector< double > &Z, const Namelist &NL)

	SRI (const Namelist &NL)

	SRI (const SRI &s)
	copy constructor More...

	SRI (const unsigned int N)

void	stateFix (const std::string &name, double value, double sigma, bool restore)

void	stateFix (const unsigned int index, double value, double sigma, bool restore)

void	stateFixAndRemove (const Namelist &drops, const Vector< double > &values)

void	stateFixAndRemove (const unsigned int index, double value)

void	transform (const Matrix< double > &invT, const Namelist &NL)

void	zeroAll (const unsigned int n=0)

void	zeroOne (const unsigned int n)

void	zeroState ()
	Zero out (set all elements to zero) the state (Vector Z) only. More...

Static Public Member Functions
static void	DMsmootherUpdate (Matrix< double > &P, Vector< double > &X, Matrix< double > &Phinv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx)

static 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)

Private Member Functions
void	defaults ()
	initialization used by constructors More...

Static Private Member Functions
template<class T >
static void	SrifSU (Matrix< T > &R, Vector< T > &Z, Matrix< T > &Phi, Matrix< T > &Rw, Matrix< T > &G, Vector< T > &Zw, Matrix< T > &Rwx)

template<class T >
static void	SrifSU_DM (Matrix< T > &P, Vector< T > &X, Matrix< T > &Phinv, Matrix< T > &Rw, Matrix< T > &G, Vector< T > &Zw, Matrix< T > &Rwx)

template<class T >
static void	SrifTU (Matrix< T > &R, Vector< T > &Z, Matrix< T > &Phi, Matrix< T > &Rw, Matrix< T > &G, Vector< T > &Zw, Matrix< T > &Rwx)

Additional Inherited Members
Protected Attributes inherited from gnsstk::SRI
Namelist	names
	Namelist parallel to R and Z, labelling the elements of the state vector. More...

Matrix< double >	R
	Information matrix, an upper triangular (square) matrix. More...

Vector< double >	Z
	SRI state vector, of length equal to the dimension (row and col) of R. More...

Constructor & Destructor Documentation

◆ SRIFilter() [1/5]

gnsstk::SRIFilter::SRIFilter ( )

empty constructor

Definition at line 62 of file SRIFilter.cpp.

◆ SRIFilter() [2/5]

gnsstk::SRIFilter::SRIFilter ( const unsigned int N )

constructor given the dimension N.

Parameters

N	dimension of the SRIFilter.

Definition at line 66 of file SRIFilter.cpp.

◆ SRIFilter() [3/5]

gnsstk::SRIFilter::SRIFilter ( const Namelist & NL )

constructor given a Namelist; its dimension determines the SRI dimension.

Parameters

NL	Namelist for the SRIFilter.

Definition at line 76 of file SRIFilter.cpp.

◆ SRIFilter() [4/5]

gnsstk::SRIFilter::SRIFilter	(	const Matrix< double > &	R,
		const Vector< double > &	Z,
		const Namelist &	NL
	)

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 90 of file SRIFilter.cpp.

◆ SRIFilter() [5/5]

gnsstk::SRIFilter::SRIFilter ( const SRIFilter & right )

inline

copy constructor

Parameters

right SRIFilter to be copied

Definition at line 126 of file SRIFilter.hpp.

Member Function Documentation

◆ defaults()

void gnsstk::SRIFilter::defaults ( )

inlineprivate

initialization used by constructors

Definition at line 432 of file SRIFilter.hpp.

◆ DMsmootherUpdate()

void gnsstk::SRIFilter::DMsmootherUpdate	(	Matrix< double > &	P,
		Vector< double > &	X,
		Matrix< double > &	Phinv,
		Matrix< double > &	Rw,
		Matrix< double > &	G,
		Vector< double > &	Zw,
		Matrix< double > &	Rwx
	)

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 266 of file SRIFilter.cpp.

◆ DMsmootherUpdateWithControl()

static void gnsstk::SRIFilter::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
	)

static

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

Exceptions

MatrixException

◆ measurementUpdate() [1/2]

void gnsstk::SRIFilter::measurementUpdate	(	const Matrix< double > &	H,
		Vector< double > &	D,
		const Matrix< double > &	CM = `SRINullMatrix`
	)

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

SomeUnknownException	if dimension N does not match dimension of SRI, or if other dimensions are inconsistent, or if CM is singular.
MatrixException
VectorException

Definition at line 123 of file SRIFilter.cpp.

◆ measurementUpdate() [2/2]

void gnsstk::SRIFilter::measurementUpdate	(	const SparseMatrix< double > &	H,
		Vector< double > &	D,
		const SparseMatrix< double > &	CM = `SRINullSparseMatrix`
	)

SRIF (Kalman) simple linear measurement update with optional weight matrix SparseMatrix version

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

SomeUnknownException	if dimension N does not match dimension of SRI, or if other dimensions are inconsistent, or if CM is singular.
MatrixException
VectorException

Definition at line 179 of file SRIFilter.cpp.

◆ operator=()

SRIFilter & gnsstk::SRIFilter::operator= ( const SRIFilter & right )

operator=

Parameters

right SRIFilter to be copied

Definition at line 111 of file SRIFilter.cpp.

◆ reset()

void gnsstk::SRIFilter::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 290 of file SRIFilter.cpp.

◆ smootherUpdate()

void gnsstk::SRIFilter::smootherUpdate	(	Matrix< double > &	Phi,
		Matrix< double > &	Rw,
		Matrix< double > &	G,
		Vector< double > &	Zw,
		Matrix< double > &	Rwx
	)

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 GQtranspose(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 251 of file SRIFilter.cpp.

◆ SrifSU()

template<class T >

void gnsstk::SRIFilter::SrifSU	(	Matrix< T > &	R,
		Vector< T > &	Z,
		Matrix< T > &	Phi,
		Matrix< T > &	Rw,
		Matrix< T > &	G,
		Vector< T > &	Zw,
		Matrix< T > &	Rwx
	)

staticprivate

SRIF smoother update (non-SRI version); SRIFilter::smootherUpdate for doc.

Exceptions

MatrixException

Definition at line 649 of file SRIFilter.cpp.

◆ SrifSU_DM()

template<class T >

void gnsstk::SRIFilter::SrifSU_DM	(	Matrix< T > &	P,
		Vector< T > &	X,
		Matrix< T > &	Phinv,
		Matrix< T > &	Rw,
		Matrix< T > &	G,
		Vector< T > &	Zw,
		Matrix< T > &	Rwx
	)

staticprivate

SRIF smoother update in covariance / state form; see SRIFilter::DMsmootherUpdate() for doc.

Exceptions

MatrixException

Definition at line 914 of file SRIFilter.cpp.

◆ SrifTU()

template<class T >

void gnsstk::SRIFilter::SrifTU	(	Matrix< T > &	R,
		Vector< T > &	Z,
		Matrix< T > &	Phi,
		Matrix< T > &	Rw,
		Matrix< T > &	G,
		Vector< T > &	Zw,
		Matrix< T > &	Rwx
	)

staticprivate

SRIF time update (non-SRI version); SRIFilter::timeUpdate for doc.

Exceptions

MatrixException

Definition at line 398 of file SRIFilter.cpp.

◆ timeUpdate()

void gnsstk::SRIFilter::timeUpdate	(	Matrix< double > &	PhiInv,
		Matrix< double > &	Rw,
		Matrix< double > &	G,
		Vector< double > &	Zw,
		Matrix< double > &	Rwx
	)

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 GQtranspose(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 235 of file SRIFilter.cpp.

◆ zeroAll()

void gnsstk::SRIFilter::zeroAll ( )

remove all stored information by setting the SRI to zero (does not re-dimension).

Definition at line 283 of file SRIFilter.cpp.

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

Detailed Description

Public Member Functions

Static Public Member Functions

Private Member Functions

Static Private Member Functions

Additional Inherited Members

Constructor & Destructor Documentation

◆ SRIFilter() [1/5]

◆ SRIFilter() [2/5]

◆ SRIFilter() [3/5]

◆ SRIFilter() [4/5]

◆ SRIFilter() [5/5]

Member Function Documentation

◆ defaults()

◆ DMsmootherUpdate()

◆ DMsmootherUpdateWithControl()

◆ measurementUpdate() [1/2]

◆ measurementUpdate() [2/2]

◆ operator=()

◆ reset()

◆ smootherUpdate()

◆ SrifSU()

◆ SrifSU_DM()

◆ SrifTU()

◆ timeUpdate()

backward filter process.

◆ zeroAll()