#include <extendedKalmanFilter.h>
Public Member Functions | |
| ExtendedKalmanFilter (double startTime=0.0) | |
| Default constructor. | |
| virtual | ~ExtendedKalmanFilter () |
| Destructor. | |
| virtual void | addMeasurement (unsigned int measurementID, double timestamp, numeric::Array1D< double > const &measurement, numeric::Array1D< double > const &controlInput) |
| Use this member function tell the filter about a new measurement, and to request that the state estimate be updated to reflect this new measurement. | |
| virtual void | checkMeasurementJacobians (unsigned int measurementID, numeric::Array1D< double > const &epsilonArray, numeric::Array2D< double > &residualArray0) |
| This member function provides a sanity check on the user-provided Jacobian computation, which is a common source of errors. | |
| virtual void | checkProcessJacobians (numeric::Array1D< double > const &epsilonArray, numeric::Array1D< double > const &controlInput, numeric::Array2D< double > &residualArray) |
| This member function provides a sanity check on the user-provided Jacobian computation, which is a common source of errors. | |
| void | doPredictionStep (double currentTime, Array1D< double > const &controlInput) |
| Use this member function only if you are not using member function addMeasurement(). | |
| void | doMeasurementUpdate (unsigned int measurementID, numeric::Array1D< double > const &measurement) |
| Use this member function only if you are not using member function addMeasurement(). | |
| virtual void | freezeKalmanGain () |
| This member function may optionally be called to disable updates of Kalman gain and estimation error covariance. | |
| virtual void | getStateEstimate (double ×tamp, numeric::Array1D< double > &state, numeric::Array2D< double > &covariance) |
| This member function returns the current state estimate for the filter, as well as the estimated covariance of the state estimate. | |
| virtual void | setStateEstimate (double timestamp, numeric::Array1D< double > const &state, numeric::Array2D< double > const &covariance) |
| This member function sets the initial state estimate for the filter, as well as the covariance of any Gaussian noise reflected in the initial state estimate. | |
| virtual void | unfreezeKalmanGain () |
| This member function may optionally be called to reverse the effect of freezeKalmanGain(). | |
Protected Member Functions | |
| virtual numeric::Array1D< double > | applyMeasurementModel (unsigned int measurementID, double currentTime, double previousTime, numeric::Array1D< double > const ¤tState)=0 |
| Subclasses should override this function to implement the measurement model(s) relevant to the filter. | |
| virtual numeric::Array1D< double > | applyProcessModel (double currentTime, double previousTime, numeric::Array1D< double > const &previousState, numeric::Array1D< double > const &controlInput)=0 |
| Subclasses should override this function to implement the process model to be tracked by the filter. | |
| virtual void | getMeasurementJacobians (unsigned int measurementID, double currentTime, double previousTime, numeric::Array1D< double > const &state, numeric::Array2D< double > &stateJacobian, numeric::Array2D< double > &noiseJacobian)=0 |
| This member function should be overridden by subclasses to return the first derivatives of measurement model with respect to the input state, and with respect to the measurement noise. | |
| virtual void | getProcessJacobians (double currentTime, double previousTime, numeric::Array1D< double > const &state, numeric::Array2D< double > &stateJacobian, numeric::Array2D< double > &noiseJacobian)=0 |
| This member function should be overridden by subclasses to return the first derivative of the process model with respect to the input state, and with respect to the control input. | |
| virtual numeric::Array2D< double > | getMeasurementNoiseCovariance (unsigned int measurementID, double currentTime, double previousTime)=0 |
| This member function returns the (modeled) covariance of the measurement noise. | |
| virtual numeric::Array2D< double > | getProcessNoiseCovariance (double currentTime, double previousTime)=0 |
| This member function returns the (modeled) covariance of the process noise. | |
Protected Attributes | |
| numeric::Array2D< double > | m_covariance |
| numeric::Array1D< double > | m_state |
| double | m_previousTimestamp |
| double | m_timestamp |
This interface is not stable, and may not even stay as part of dlrComputerVision. Caveat emptor.
Definition at line 33 of file extendedKalmanFilter.h.
| dlr::computerVision::ExtendedKalmanFilter::ExtendedKalmanFilter | ( | double | startTime = 0.0 |
) | [explicit] |
| virtual dlr::computerVision::ExtendedKalmanFilter::~ExtendedKalmanFilter | ( | ) | [inline, virtual] |
| void dlr::computerVision::ExtendedKalmanFilter::addMeasurement | ( | unsigned int | measurementID, | |
| double | timestamp, | |||
| numeric::Array1D< double > const & | measurement, | |||
| numeric::Array1D< double > const & | controlInput | |||
| ) | [virtual] |
Use this member function tell the filter about a new measurement, and to request that the state estimate be updated to reflect this new measurement.
Under the hood, it just calls doPredictionStep() and doMeasurementUpdate() in sequence.
| measurementID | This argument identifies to which measurement model the measurment corresponds. | |
| timestamp | This argument indicates the time at which the measurement was acquired. | |
| measurement | This argument specifies the value of the measurement. | |
| controlInput | This argument specifies the control input |
Definition at line 40 of file extendedKalmanFilter.cpp.
References doMeasurementUpdate(), and doPredictionStep().
| virtual numeric::Array1D<double> dlr::computerVision::ExtendedKalmanFilter::applyMeasurementModel | ( | unsigned int | measurementID, | |
| double | currentTime, | |||
| double | previousTime, | |||
| numeric::Array1D< double > const & | currentState | |||
| ) | [protected, pure virtual] |
Subclasses should override this function to implement the measurement model(s) relevant to the filter.
For example, a (not extended) Kalman filter would make this function calculate quantity(ies) having the form (H * currentState).
| measurementID | This argument indicates which of the (possibly many) measurement models to simulate. | |
| currentTime | This argument indicates the time to which state should be extrapolated. In standard discrete KF and EKF formulations, this argument is ignored, because the time step is always 1. We include it to accomodate time-dependent process models. | |
| previousTime | This argument indicates the time at which the previous update occurred. In standard discrete KF and EKF formulations, this argument is ignored, because the time step is always 1. We include it to accomodate time-dependent process models. | |
| currentState | This argument specifies the state estimate at the current time. |
Referenced by checkMeasurementJacobians(), and doMeasurementUpdate().
| virtual numeric::Array1D<double> dlr::computerVision::ExtendedKalmanFilter::applyProcessModel | ( | double | currentTime, | |
| double | previousTime, | |||
| numeric::Array1D< double > const & | previousState, | |||
| numeric::Array1D< double > const & | controlInput | |||
| ) | [protected, pure virtual] |
Subclasses should override this function to implement the process model to be tracked by the filter.
For example, a (not extended) Kalman filter would make this function calculate the quantity (A * previousState + B * controlInput).
| currentTime | This argument indicates the time to which state should be extrapolated. In standard discrete KF and EKF formulations, this argument is ignored, because the time step is always 1. We include it to accomodate time-dependent process models. | |
| previousTime | This argument indicates the time at which the previous update occurred. In standard discrete KF and EKF formulations, this argument is ignored, because the time step is always 1. We include it to accomodate time-dependent process models. | |
| previousState | This argument specifies the state estimate at the time of the most recent update. | |
| controlInput | This argument specifies control input in effect between the time of the last update and currentTime. |
Referenced by checkProcessJacobians(), and doPredictionStep().
| void dlr::computerVision::ExtendedKalmanFilter::checkMeasurementJacobians | ( | unsigned int | measurementID, | |
| numeric::Array1D< double > const & | epsilonArray, | |||
| numeric::Array2D< double > & | residualArray0 | |||
| ) | [virtual] |
This member function provides a sanity check on the user-provided Jacobian computation, which is a common source of errors.
In its current version it only checks the Jacobian with respect to the state vector.
It works by computing a finite-difference approximation of the Jacobian, and then finding the difference between this approximation and the Jacobian provided by this->getMeasurementJacobians(). The difference is returned through a reference argument so that it can be evaluated by the calling context.
| measurementID | This argument identifies for which measurement model the Jacobian should be checked. | |
| epsilonArray | This argument specifies the step size for the finite-difference approximation. Each element of epsilonArray corresponds to one dimension of the state vector. Finite differences will be calculated in dimension "n" by evaluating the measurement model around the current state at offsets of +/- epsilon[n]. | |
| residualArray0 | This argument returns the difference between the appoximated Jacobian with respect to state, and the corresponding Jacobian returned by getMeasurementJacobians(). |
Definition at line 52 of file extendedKalmanFilter.cpp.
References applyMeasurementModel(), getMeasurementJacobians(), and getMeasurementNoiseCovariance().
| void dlr::computerVision::ExtendedKalmanFilter::checkProcessJacobians | ( | numeric::Array1D< double > const & | epsilonArray, | |
| numeric::Array1D< double > const & | controlInput, | |||
| numeric::Array2D< double > & | residualArray | |||
| ) | [virtual] |
This member function provides a sanity check on the user-provided Jacobian computation, which is a common source of errors.
In its current version it only checks the Jacobian with respect to the state vector.
It works by computing a finite-difference approximation of the Jacobian, and then finding the difference between this approximation and the Jacobian provided by this->getProcessJacobians(). The difference is returned through a reference argument so that it can be evaluated by the calling context.
| epsilonArray | This argument specifies the step size for the finite-difference approximation. Each element of epsilonArray corresponds to one dimension of the state vector. Finite differences will be calculated in dimension "n" by evaluating the measurement model around the current state at offsets of +/- epsilon[n]. | |
| controlInput | This argument specifies what control input should be used when calling applyProcessModel() to generate the finite differences. | |
| residualArray0 | This argument returns the difference between the appoximated Jacobian with respect to state, and the corresponding Jacobian returned by getProcessJacobians(). |
Definition at line 127 of file extendedKalmanFilter.cpp.
References applyProcessModel(), getProcessJacobians(), and getProcessNoiseCovariance().
| void dlr::computerVision::ExtendedKalmanFilter::doMeasurementUpdate | ( | unsigned int | measurementID, | |
| numeric::Array1D< double > const & | measurement | |||
| ) |
Use this member function only if you are not using member function addMeasurement().
It causes the internal state to be updated based on a new measurement. Normally, this member function is called directly by addMeasurement(), but it is exposed here so that the user has more control. For example, it may be useful to update the filter state after calling doPredictionStep(), but before calling doMeasurementUpdate().
| measurementID | This argument identifies to which measurement model the measurment corresponds. | |
| measurement | This argument specifies the value of the measurement. |
Definition at line 301 of file extendedKalmanFilter.cpp.
References applyMeasurementModel(), getMeasurementJacobians(), and getMeasurementNoiseCovariance().
Referenced by addMeasurement().
| void dlr::computerVision::ExtendedKalmanFilter::doPredictionStep | ( | double | currentTime, | |
| Array1D< double > const & | controlInput | |||
| ) |
Use this member function only if you are not using member function addMeasurement().
It uses the current process model to estimate the process state at the specified time. Normally, this member function is called directly by addMeasurement(), but it is exposed here so that the user has more control. For example, it may be useful to update the filter state after calling doPredictionStep(), but before calling doMeasurementUpdate().
| currentTime | This argument indicates the time at which the prediction should apply. Traditionally (for a discrete Kalman filter), the timestamp is incremented by 1 at each step. | |
| controlInput | This argument specifies the control input in effect since the previous timestamp. |
Definition at line 248 of file extendedKalmanFilter.cpp.
References applyProcessModel(), getProcessJacobians(), and getProcessNoiseCovariance().
Referenced by addMeasurement().
| void dlr::computerVision::ExtendedKalmanFilter::freezeKalmanGain | ( | ) | [virtual] |
This member function may optionally be called to disable updates of Kalman gain and estimation error covariance.
If the process model matches the actual system well, the Kalman gain and estimation error covariance will often converge to nearly constant values. In this case, disabling updates will prevent unnecessary calculation.
Definition at line 196 of file extendedKalmanFilter.cpp.
| virtual void dlr::computerVision::ExtendedKalmanFilter::getMeasurementJacobians | ( | unsigned int | measurementID, | |
| double | currentTime, | |||
| double | previousTime, | |||
| numeric::Array1D< double > const & | state, | |||
| numeric::Array2D< double > & | stateJacobian, | |||
| numeric::Array2D< double > & | noiseJacobian | |||
| ) | [protected, pure virtual] |
This member function should be overridden by subclasses to return the first derivatives of measurement model with respect to the input state, and with respect to the measurement noise.
| measurementID | This argument indicates for which of the (possibly many) measurement models to compute Jacobians. | |
| currentTime | The current time is provided to accomodate time-dependent measurement models. | |
| previousTime | The current time is provided to accomodate time-dependent measurement models. | |
| state | This argument specifies the current state estimate, in case the measurement model is dependent on state (watch for bad interactions if it is, since this moves us even further from the original Discrete Kalman Filter). | |
| stateJacobian | This argument is used to return a matrix in which each row reflects the first derivatives of the corresponding element of the measurement with respect to the elements of the input state. | |
| noiseJacobian | This argument is used to return a matrix in which each row reflects the first derivatives of the corresponding element of the output with respect to the elements of the measurement noise. This will often just be an identity matrix. |
Referenced by checkMeasurementJacobians(), and doMeasurementUpdate().
| virtual numeric::Array2D<double> dlr::computerVision::ExtendedKalmanFilter::getMeasurementNoiseCovariance | ( | unsigned int | measurementID, | |
| double | currentTime, | |||
| double | previousTime | |||
| ) | [protected, pure virtual] |
This member function returns the (modeled) covariance of the measurement noise.
| measurementID | This argument indicates for which of the (possibly many) measurement models to return the noise covariance. | |
| currentTime | The current time is provided to accomodate time-dependent process models. | |
| previousTime | The current time is provided to accomodate time-dependent measurement models. |
Referenced by checkMeasurementJacobians(), and doMeasurementUpdate().
| virtual void dlr::computerVision::ExtendedKalmanFilter::getProcessJacobians | ( | double | currentTime, | |
| double | previousTime, | |||
| numeric::Array1D< double > const & | state, | |||
| numeric::Array2D< double > & | stateJacobian, | |||
| numeric::Array2D< double > & | noiseJacobian | |||
| ) | [protected, pure virtual] |
This member function should be overridden by subclasses to return the first derivative of the process model with respect to the input state, and with respect to the control input.
| currentTime | The current time is provided to accomodate time-varying measurement models. | |
| previousTime | The current time is provided to accomodate time-dependent measurement models. | |
| state | This argument specifies the current state estimate, in case the process model is dependent on state (watch for bad interactions if it is, since this moves us even further from the original Discrete Kalman Filter). | |
| stateJacobian | This argument is used to return a square matrix in which each row reflects the first derivatives of the corresponding element of the output with respect to the elements of the input state. | |
| noiseJacobian | This argument is used to return a not-necessarily-square matrix in which each row reflects the first derivatives of the corresponding element of the output with respect to the elements of the process noise. |
Referenced by checkProcessJacobians(), and doPredictionStep().
| virtual numeric::Array2D<double> dlr::computerVision::ExtendedKalmanFilter::getProcessNoiseCovariance | ( | double | currentTime, | |
| double | previousTime | |||
| ) | [protected, pure virtual] |
This member function returns the (modeled) covariance of the process noise.
| currentTime | The current time is provided to accomodate time-dependent process models. | |
| previousTime | The current time is provided to accomodate time-dependent measurement models. |
Referenced by checkProcessJacobians(), and doPredictionStep().
| void dlr::computerVision::ExtendedKalmanFilter::getStateEstimate | ( | double & | timestamp, | |
| numeric::Array1D< double > & | state, | |||
| numeric::Array2D< double > & | covariance | |||
| ) | [virtual] |
This member function returns the current state estimate for the filter, as well as the estimated covariance of the state estimate.
| timestamp | This argument returns the time of the most recent state estimate. | |
| state | This argument returns the state estimate. | |
| covariance | This argument returns the estimated covariance. |
Definition at line 209 of file extendedKalmanFilter.cpp.
| void dlr::computerVision::ExtendedKalmanFilter::setStateEstimate | ( | double | timestamp, | |
| numeric::Array1D< double > const & | state, | |||
| numeric::Array2D< double > const & | covariance | |||
| ) | [virtual] |
This member function sets the initial state estimate for the filter, as well as the covariance of any Gaussian noise reflected in the initial state estimate.
| timestamp | This argument specifies the time of the initial state estimate. | |
| state | This argument specifies the initial state estimate. | |
| covariance | This argument specifies the covariance associated with the initial state estimate. |
Definition at line 224 of file extendedKalmanFilter.cpp.
| void dlr::computerVision::ExtendedKalmanFilter::unfreezeKalmanGain | ( | ) | [virtual] |
This member function may optionally be called to reverse the effect of freezeKalmanGain().
That is, it re-enables updates to the Kalman gain and estimation error covariance. It does not retroactively update the estimates, rather it simply reenables the updates for subsequent calculations.
Definition at line 238 of file extendedKalmanFilter.cpp.
1.5.8