#include <nonminimalkalmanfilter.h>
Public Member Functions | |
virtual void | MeasUpdate (MeasurementModel< ColumnVector, ColumnVector > *const measmodel, const ColumnVector &z, const ColumnVector &s) |
NonminimalKalmanFilter (Gaussian *prior, unsigned int NrIterations, vector< NLSysModel *> minimalsysmodels, vector< NLMeasModel *> minimalmeasmodels, vector< GiNaC::symbol > nonlinearstate= *(new vector< GiNaC::symbol >)) | |
virtual void | SysUpdate (SystemModel< ColumnVector > *const sysmodel, const ColumnVector &u) |
virtual | ~NonminimalKalmanFilter () |
Destructor. More... | |
![]() | |
void | AllocateMeasModel (const vector< unsigned int > &meas_dimensions) |
Function to allocate memory needed during the measurement update,. More... | |
void | AllocateMeasModel (const unsigned int &meas_dimensions) |
Function to allocate memory needed during the measurement update. More... | |
KalmanFilter (Gaussian *prior) | |
Constructor. More... | |
virtual Gaussian * | PostGet () |
Get Posterior density. More... | |
virtual | ~KalmanFilter () |
Destructor. More... | |
![]() | |
Filter (Pdf< MatrixWrapper::ColumnVector > *prior) | |
Constructor. More... | |
Filter (const Filter< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > &filt) | |
copy constructor More... | |
virtual void | Reset (Pdf< MatrixWrapper::ColumnVector > *prior) |
Reset Filter. More... | |
int | TimeStepGet () const |
Get current time. More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s) |
Full Update (system with inputs/sensing params) More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s) |
Full Update (system without inputs, with sensing params) More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z) |
Full Update (system without inputs/sensing params) More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z) |
Full Update (system with inputs, without sensing params) More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u) |
System Update (system with inputs) More... | |
virtual bool | Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel) |
System Update (system without inputs) More... | |
virtual bool | Update (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s) |
Measurement Update (system with "sensing params") More... | |
virtual bool | Update (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z) |
Measurement Update (system without "sensing params") More... | |
virtual | ~Filter () |
destructor More... | |
Private Attributes | |
Linearise * | Linear |
IteratedExtendedKalmanFilter * | MinimalFilter |
NLMeasModel * | MinimalMeasModel |
Gaussian * | MinimalPrior |
vector< GiNaC::symbol > | MinimalState |
IteratedExtendedKalmanFilter * | NonminimalFilter |
Gaussian * | NonminimalPrior |
vector< GiNaC::symbol > | NonminimalState |
Additional Inherited Members | |
![]() | |
void | CalculateMeasUpdate (const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R) |
void | CalculateSysUpdate (const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q) |
virtual void | MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)=0 |
Measurement Update (overloaded) More... | |
void | PostMuSet (const MatrixWrapper::ColumnVector &c) |
Set expected value of posterior estimate. More... | |
void | PostSigmaSet (const MatrixWrapper::SymmetricMatrix &s) |
Set covariance of posterior estimate. More... | |
virtual void | SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)=0 |
System Update. More... | |
virtual bool | UpdateInternal (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s) |
Actual implementation of Update, varies along filters. More... | |
![]() | |
Matrix | _K |
std::map< unsigned int, MeasUpdateVariables > | _mapMeasUpdateVariables |
std::map< unsigned int, MeasUpdateVariables >::iterator | _mapMeasUpdateVariables_it |
ColumnVector | _Mu_new |
SymmetricMatrix | _Sigma_new |
Matrix | _Sigma_temp |
Matrix | _Sigma_temp_par |
Matrix | _SMatrix |
![]() | |
Pdf< MatrixWrapper::ColumnVector > * | _post |
Pointer to the Posterior Pdf. More... | |
Pdf< MatrixWrapper::ColumnVector > * | _prior |
prior Pdf More... | |
int | _timestep |
Represents the current timestep of the filter. More... | |
This is a class implementing the Kalman Filter (KF) class for Non Minimal State Kalman Filters.
The System- and MeasurementUpdate equasions are not linear. Substituting the state by a non-minimal state will make the System- and MeasurementUpdate linear in the non-minimal state
Definition at line 47 of file nonminimalkalmanfilter.h.
BFL::NonminimalKalmanFilter::NonminimalKalmanFilter | ( | Gaussian * | prior, |
unsigned int | NrIterations, | ||
vector< NLSysModel *> | minimalsysmodels, | ||
vector< NLMeasModel *> | minimalmeasmodels, | ||
vector< GiNaC::symbol > | nonlinearstate = *(new vector<GiNaC::symbol>) |
||
) |
Constructor
prior | pointer to the Gaussian prior density |
NrIterations | Number of iterations that will be used (this class uses 2 IEKF: one for the minimal state and one for the nonminimal state) |
minimalsysmodels | vector of measurement models that can be used for measurement updates |
minimalmeasmodels | vector of system models that can be used for system updates |
nonlinearstate | symbols of state in wich the filter should be linearised |
Definition at line 30 of file nonminimalkalmanfilter.cpp.
|
virtual |
Destructor.
Definition at line 71 of file nonminimalkalmanfilter.cpp.
|
virtual |
Definition at line 88 of file nonminimalkalmanfilter.cpp.
|
virtual |
Definition at line 81 of file nonminimalkalmanfilter.cpp.
|
private |
Definition at line 77 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 78 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 80 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 79 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 76 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 78 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 79 of file nonminimalkalmanfilter.h.
|
private |
Definition at line 76 of file nonminimalkalmanfilter.h.