#include <SRiteratedextendedkalmanfilter.h>
Public Member Functions | |
virtual void | CalculateCovariance (MatrixWrapper::Matrix &R_vf, MatrixWrapper::Matrix &H_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &SR) |
Calculate the covariance of the new state estimate (P_k) | |
virtual void | CalculateMatrix (MatrixWrapper::Matrix &H_i, MatrixWrapper::SymmetricMatrix &R_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &Sr, MatrixWrapper::Matrix &K_i) |
Calculate K_i , invS and Sr. | |
virtual void | CalculateMean (MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i) |
Calculate the new state estimate. | |
virtual void | CalculateMeasUpdate (MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R) |
virtual void | MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s) |
Perform a measurement update use a measurement model, measurements z and virutal measurements s. | |
virtual void | MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z) |
Perform a measurement update use a measurement model, measurements z. | |
void | PriorSet (MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P) |
Set mean and covariance of the state estimation to a specific value. | |
MatrixWrapper::Matrix | SRCovarianceGet () const |
Returns a square root of the covariance of the measurement input u. | |
void | SRCovarianceSet (MatrixWrapper::Matrix JP_new) |
Set the square root covariance to a specific value. | |
SRIteratedExtendedKalmanFilter (Gaussian *prior, unsigned int nr_it=1) | |
virtual void | SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u) |
Perform a system update with the current measurement model ans system model using an input u. | |
virtual void | SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel) |
Perform a system update with the current measurement model and system model. | |
virtual | ~SRIteratedExtendedKalmanFilter () |
Destructor. | |
Private Attributes | |
MatrixWrapper::Matrix | JP |
the upper triangular matrix cholesky decompostion of the state covariance ( ) | |
unsigned int | nr_iterations |
Variable indicating the number of iterations of the filter. |
This is a class implementing the Kalman Filter (KF) class for Square Root Iterated Extended Kalman Filters. this is a possible implementation of a Kalman filter, which in will yield better numerical stable results, since the covariance matrix is defined as the Square root of the Covariance matrix of the state estimation. See
@Book{ Anderson_auxiliary, author = {Anderson, B.D.O. and Moore, J.B.}, title = {Optimal filtering}, publisher = {Prentice-Hall, Englewood Cliffs, NJ }, year = {1979} }
for more details. Note that this particular implementation:
and internally defined as a linear measurement model with virtual measurement z_k^{virtual}
Definition at line 53 of file SRiteratedextendedkalmanfilter.h.
BFL::SRIteratedExtendedKalmanFilter::SRIteratedExtendedKalmanFilter | ( | Gaussian * | prior, |
unsigned int | nr_it = 1 |
||
) |
Constructor
prior | pointer to the Monte Carlo Pdf prior density |
nr_it | the number of iterations in one update |
Definition at line 29 of file SRiteratedextendedkalmanfilter.cpp.
Destructor.
Definition at line 36 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::CalculateCovariance | ( | MatrixWrapper::Matrix & | R_vf, |
MatrixWrapper::Matrix & | H_i, | ||
MatrixWrapper::Matrix & | invS, | ||
MatrixWrapper::Matrix & | SR | ||
) | [virtual] |
Calculate the covariance of the new state estimate (P_k)
Definition at line 221 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::CalculateMatrix | ( | MatrixWrapper::Matrix & | H_i, |
MatrixWrapper::SymmetricMatrix & | R_i, | ||
MatrixWrapper::Matrix & | invS, | ||
MatrixWrapper::Matrix & | Sr, | ||
MatrixWrapper::Matrix & | K_i | ||
) | [virtual] |
void BFL::SRIteratedExtendedKalmanFilter::CalculateMean | ( | MatrixWrapper::ColumnVector & | x_k, |
const MatrixWrapper::ColumnVector & | z, | ||
MatrixWrapper::ColumnVector & | Z_i, | ||
MatrixWrapper::Matrix & | K_i | ||
) | [virtual] |
Calculate the new state estimate.
Definition at line 213 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::CalculateMeasUpdate | ( | MatrixWrapper::ColumnVector | z, |
MatrixWrapper::ColumnVector | Z, | ||
MatrixWrapper::Matrix | H, | ||
MatrixWrapper::SymmetricMatrix | R | ||
) | [virtual] |
Calculate Kalman filter Measurement Update
with
Definition at line 169 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::MeasUpdate | ( | MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const | measmodel, |
const MatrixWrapper::ColumnVector & | z, | ||
const MatrixWrapper::ColumnVector & | s | ||
) | [virtual] |
Perform a measurement update use a measurement model, measurements z and virutal measurements s.
Implements BFL::KalmanFilter.
Definition at line 66 of file SRiteratedextendedkalmanfilter.cpp.
virtual void BFL::SRIteratedExtendedKalmanFilter::MeasUpdate | ( | MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const | measmodel, |
const MatrixWrapper::ColumnVector & | z | ||
) | [virtual] |
Perform a measurement update use a measurement model, measurements z.
void BFL::SRIteratedExtendedKalmanFilter::PriorSet | ( | MatrixWrapper::ColumnVector & | X, |
MatrixWrapper::SymmetricMatrix & | P | ||
) |
Set mean and covariance of the state estimation to a specific value.
Definition at line 163 of file SRiteratedextendedkalmanfilter.cpp.
Matrix BFL::SRIteratedExtendedKalmanFilter::SRCovarianceGet | ( | ) | const |
Returns a square root of the covariance of the measurement input u.
Definition at line 153 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::SRCovarianceSet | ( | MatrixWrapper::Matrix | JP_new | ) |
Set the square root covariance to a specific value.
Definition at line 158 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::SysUpdate | ( | SystemModel< MatrixWrapper::ColumnVector > *const | sysmodel, |
const MatrixWrapper::ColumnVector & | u | ||
) | [virtual] |
Perform a system update with the current measurement model ans system model using an input u.
Implements BFL::KalmanFilter.
Definition at line 39 of file SRiteratedextendedkalmanfilter.cpp.
void BFL::SRIteratedExtendedKalmanFilter::SysUpdate | ( | SystemModel< MatrixWrapper::ColumnVector > *const | sysmodel | ) | [virtual] |
Perform a system update with the current measurement model and system model.
Definition at line 59 of file SRiteratedextendedkalmanfilter.cpp.
MatrixWrapper::Matrix BFL::SRIteratedExtendedKalmanFilter::JP [private] |
the upper triangular matrix cholesky decompostion of the state covariance ( )
Definition at line 118 of file SRiteratedextendedkalmanfilter.h.
unsigned int BFL::SRIteratedExtendedKalmanFilter::nr_iterations [private] |
Variable indicating the number of iterations of the filter.
Definition at line 116 of file SRiteratedextendedkalmanfilter.h.