Core Classes
Error
Invariant Extended Kalman Filter
-
template<class pM>
class InEKF::InEKF The Invariant Extended Kalman Filter.
- Template Parameters
pM – Process Model. Pulls group and control info from it, can be left out if class template argument deduction (C++17) is used.
Public Functions
-
inline InEKF(pM *pModel, Group state, ERROR error = ERROR::RIGHT)
Construct a new InEKF object.
- Parameters
pModel – Pointer to the process model.
state – Initial state, must be of same group that process model uses, and must be uncertain.
error – Right or left invariant error.
-
Group predict(const U &u, double dt = 1)
Prediction Step.
- Parameters
u – Control, must be same as what process model uses.
dt – Delta t. Used sometimes depending on process model. Defaults to 1.
- Returns
State estimate
-
Group update(std::string name, const Eigen::VectorXd &z)
Update Step.
- Parameters
name – Name of measurement model.
z – Measurement. May vary in size depending on how measurement model processes it.
- Returns
State estimate.
-
void addMeasureModel(std::string name, MeasureModel<Group> *m)
Add measurement model to the filter.
- Parameters
name – Name of measurement model.
m – A measure model pointer, templated by the used group.
-
void addMeasureModels(std::map<std::string, MeasureModel<Group>*> m)
Add multiple measurement models to the filter.
- Parameters
m – Map from model names to model. Can be used passed in as {“name”: model, “another”: diff_model}
-
inline const Group &getState() const
Get the current state estimate.
- Returns
const Group&
-
inline void setState(const Group &state)
Set the current state estimate.
- Parameters
state – Current state estimate
Measure Model
-
template<class Group>
class InEKF::MeasureModel Base class measure model. Written to be inherited from, but in most cases this class will be sufficient.
- Template Parameters
Group – State’s group that is being tracked.
Public Types
-
typedef Eigen::Matrix<double, Group::rotSize, Group::rotSize> MatrixS
Size of matrix needed for the measurement model covariance.
Public Functions
-
inline MeasureModel()
Construct a new Measure Model object.
-
inline MeasureModel(VectorB b, const MatrixS &M, ERROR error)
Construct a new Measure Model object, automatically creating H. Should be used most of the time.
- Parameters
b – b vector from measurement model. Will be used to create H.
M – Measurement covariance.
error – Type of invariant measurement (right or left).
-
inline virtual VectorB processZ(const Eigen::VectorXd &z, const Group &state)
Process measurement before putting into InEKF. Can be used to change frames, convert r/b->x/y, or append 0s. By default is used to append zeros/ones onto it according to b vector set. Called first in update step.
- Parameters
z – Measurement
state – Current state estimate.
- Returns
Processed measurement.
-
inline virtual MatrixH makeHError(const Group &state, ERROR iekfERROR)
Sets and returns H_error_ for settings where filter error type != measurement error type. Done by multiplying H by adjoint of current state estimate. Called second in update step.
- Parameters
state – Current state estimate.
iekfERROR – Type of filter error.
- Returns
H_error_
-
inline virtual VectorV calcV(const VectorB &z, const Group &state)
Computes innovation based on measurement model. Called third in the update step.
- Parameters
z – Measurement.
state – Current state estimate.
- Returns
Truncated innovation.
Process Model
-
template<class Group, class U>
class InEKF::ProcessModel Base class process model.
- Template Parameters
Group – State’s group that is being tracked.
U – Form of control. Can be either a group, or an Eigen::Matrix<double,n,1>
Public Types
Public Functions
-
inline ProcessModel()
Construct a new Process Model object.
-
inline virtual Group f(U u, double dt, Group state)
Propagates state forward one timestep. Must be overriden, has no implementation.
- Parameters
u – Control
dt – Delta time
state – Current state
- Returns
Updated state estimate
-
inline virtual MatrixCov makePhi(const U &u, double dt, const Group &state, ERROR error)
Make a discrete time linearized process model matrix, with \(\Phi = \exp(A\Delta t) \). Must be overriden, has no implementation.
- Parameters
u – Control
dt – Delta time
state – Current state estimate (shouldn’t be needed unless doing an “Imperfect InEKF”)
error – Right or left error. Function should be implemented to handle both.
- Returns
Phi