This is a C++ template-based header library for building Kalman-style filters, using Eigen linear algebra data types and algorithms.
n is the dimension of the state.
m is the dimension of (a given) measurement. Note that there can be more than
one measurement type involved, that's fine (and desirable!) - only one goes into
the FlexibleKalmanFilter::correct()
function at a time, where most of these
are used.
Variable | Rows | Cols | Meaning |
---|---|---|---|
x | n | 1 | State |
P | n | n | State error covariance |
Q(dt) | n | n | Process model noise covariance |
z | m | 1 | Measurement |
H | m | n | Measurement Jacobian |
R | m | m | Measurement error covariance |
K | n | m | Kalman gain |
There are separate types for State, Process Model, and one or more Measurement types.
The types involved are loosely related/coupled: the Process Model has to know about the particular State type (in some way), but not vice-versa, and the Measurement needs to know (in some way) about State (more about that below), but about no other Measurement types, or the Process Model.
They are all tied together in the FlexibleKalmanFilter<>
template object,
which holds both an instance of Process Model and State, and has a function that
takes a (deduced, templated) Measurement argument to its correct()
method.
Process Model computations of predicted error covariance often take a similar
form, so a convenience free function
Matrix<n, n> predictErrorCovariance(State const& state, ProcessModel & model, double dt)
is provided. (If you must know: it performs A P Atranspose + Q(dt)
)It's
optional, but its usage is encouraged if your math matches what it does (as it
likely will), so its requirements are also listed below, in addition to the
requirements of the FlexibleKalmanFilter
class methods proper.
Also, a brief note: Most of these types will probably hold what Eigen refers to
as a "fixed-size vectorizable" member, so you'll almost certainly want this line
in the public
section of your types:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
or you'll get weird segfaults if something ends up not being aligned properly.
All the interface stuff below required should be public.
State types should derive publicly from StateBase<MyStateType>
where the
template parameter is their own type (the "Curiously-Recurring Template
Pattern").
Needs public members:
static constexpr size_t Dimension = 12;
- to indicate dimension, aka n - here 12. Used all over.
void setStateVector(? state)
- to replace the n-dimensional state vector, used by
FlexibleKalmanFilter::correct()
- to replace the n-dimensional state vector, used by
Vector<n> stateVector() const
- to retrieve the n-dimensional state vector: should be
const
and will probably return aconst &
(but if you have a good reason, by value is OK too). Used byFlexibleKalmanFilter::correct()
and likely many other places.
- to retrieve the n-dimensional state vector: should be
void setErrorCovariance(? errorCovariance)
- to replace the n x n state error covariance matrix P, used by
FlexibleKalmanFilter::correct()
- to replace the n x n state error covariance matrix P, used by
Matrix<n,n> errorCovariance() const
- to retrieve the n x n state error covariance matrix: should be
const
and will probably return aconst &
(but if you have a good reason, by value is OK too). Used byFlexibleKalmanFilter::correct()
- to retrieve the n x n state error covariance matrix: should be
void postCorrect()
- called at the end ofFlexibleKalmanFilter::correct()
in case your state needs some kind of adjustment at the end. (This is where the "external quaternion" gets updated, for instance.) If you don't need to do anything, just make this do nothing.
Should not contain the filter state - that separate object is kept separately and passed as a parameter as needed. It may contain some state (member variables) of its own if required - typically configuration parameters, etc.
Process model types should derive publicly from
ProcessModelBase<MyProcessModelType>
where the template parameter is their own
type (the "Curiously-Recurring Template Pattern").
using State = YourStateType
- used for convenience and reducing the number of required parameters to
FlexibleKalmanFilter<>
- used for convenience and reducing the number of required parameters to
void predictState(State & state, double dt)
- Called by
FlexibleKalmanFilter::predict(dt)
, should update the state to the predicted state after an elapseddt
seconds. Often uses the convenience free functionpredictErrorCovariance
. Make this const, if you can.
- Called by
Matrix<n, n> getStateTransitionMatrix(State const&, double dt) const
- Gets the state transition matrix A that represents the process model's
effects on the state over the given time interval. You may choose to use
this to update the state within
predictState()
(if manual computation is not more efficient). Optional, but recommended: required ifpredictErrorCovariance
is used inpredictState()
- Gets the state transition matrix A that represents the process model's
effects on the state over the given time interval. You may choose to use
this to update the state within
Matrix<n, n> getSampledProcessNoiseCovariance(double dt)
- Gets the matrix Q that represents the local effect of your process on
state noise. Optional, but recommended: required if
predictErrorCovariance
is used inpredictState()
- Gets the matrix Q that represents the local effect of your process on
state noise. Optional, but recommended: required if
Note that there may (and often are) several types of measurements used in a
particular filter - the FlexibleKalmanFilter::correct()
method is a function
template so it can adjust to any number of measurement types. If you're having
difficulty, check your const
and try a named instance of your measurement as
opposed to passing a temporary.
Only FlexibleKalmanFilter::correct()
and functions related to it (other
correction functions like correctUnscented
, etc.) interacts with Measurement
types.
Measurement types should derive publicly from
MeasurementBase<MyMeasurementType>
where the template parameter is their own
type (the "Curiously-Recurring Template Pattern").
static constexpr size_t Dimension = 4;
- to indicate dimension of your measurement, aka m - here 4.
Vector<m> getResidual(State const& state) const
- Also known as the "innovation" or delta z - this function predicts the measurement expected given the (predicted) state provided, then takes the difference (contextually defined - may be multiplicative) and returns that, typically by value.
Matrix<m,m> getCovariance(State & state) const
- Gets the measurement error covariance matrix R - a square matrix of dimension equal to measurement dimension. Sometimes a function of state, rather than a fixed value for a measurement, so often returning by value. (Sometimes this doesn't need the state or just needs particular state methods that could be implemented by many state classes, so this could be a function template in a base class, as done with the absolute pose and orientation measurements included.)
Matrix<m,n> getJacobian(State & s) const
- Gets the measurement Jacobian H: the change in what you'd expect the measurement to be, with respect to changes in the state, given the (predicted) state. Thus, it has rows equal to the measurement dimension, and cols equal to the state dimension. (If you're using a SCAAT-style filter, much of this will be zero.) This usually required closest coupling to the State type out of all the methods, so if you're splitting things up, this could be in the derived class, as done with the absolute pose and orientation measurements included. This is nearly always a function that ends up returning by value.
- Only required for "normal" EKF-style filtering. If you'd rather not compute a Jacobian (and who wouldn't?), you can use the unscented KF correction instead.
Vector<m> predictMeasurement(State const& state) const
- Returns a predicted measurement given a state.
- This is only technically required for the unscented-style correction, but
you may implemented in all cases since it helps to implement
getResidual()
.
Vector<m> getResidual(Vector<m> const &prediction, State const& state) const
- Unlike the single-argument
getResidual()
, this version does not (usually) use the actual data of the measurement. Instead, it uses a prediction, likely frompredictMeasurement()
. - This is only technically required for the unscented-style correction, but
you may implemented in all cases since it helps to implement the
single-parameter
getResidual()
- it becomesreturn getResidual(predictMeasurement(s), s);
(often)
- Unlike the single-argument
In-code references are often to "Welch 1996" - this is to Greg Welch's PhD dissertation. Specifically, the citation follows, and the link is publicly accessible for the full text.
- Welch, G. F. (1996). SCAAT: Incremental Tracking with Incomplete Information. University of North Carolina at Chapel Hill. Retrieved from http:https://www.cs.unc.edu/techreports/96-051.pdf
Portions of that dissertation were published as "Welch and Bishop 1997" - if you can get a copy of that paper, it is more condensed, but as far as I can tell everything we use is inside the dissertation. (The link is the canonical DOI link, you'll need some library subscription to get the paper from that place.)
- Welch, G. F., & Bishop, G. (1997). SCAAT: incremental tracking with incomplete information. In Proceedings of the 24th annual conference on Computer graphics and interactive techniques - SIGGRAPH ’97 (pp. 333–344). New York, NY, USA: ACM Press/Addison-Wesley Publishing Co. http:https://doi.org/10.1145/258734.258876
Welch gives two citations for the "externalized-quaternion, internal linear incremental orientation" concept, only one of which I could get a copy of, and which is cited in the code. (As above, the link is the canonical one and behind a paywall.)
- Azarbayejani, A., & Pentland, A. P. (1995). Recursive estimation of motion, structure, and focal length. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 17(6), 562–575. http:https://doi.org/10.1109/34.387503
Library structure/design was inspired by
- "Eigen-Kalman" https://github.com/rpavlik/eigen-kalman and/in turn by TAG whose source is now accessible at https://github.com/edrosten/tag/tree/master/tag.
This project: Most files are licensed under the Apache License, Version 2.0. Some are licensed "BSL-1.0 OR Apache-2.0", and this is the preference for new additions.
This code originated as a part of tracking code from OSVR-Core. It was then extracted into a project providing the tracking capabilities standalone, UVBI-and-KalmanFramework-Standalone. Since this is by far the most reusable code from that, though, I then extracted it once more into this repository.
/cmake
- Git subtree from https://github.com/rpavlik/cmake-modules used at compile-time only. Primarily BSL 1.0, some items under the license used by CMake (BSD-style)/vendor/eigen
- Unpacked release from http:https://eigen.tuxfamily.org/ - header-only library under the MPL2 (a file-level copyleft, compatible with proprietary software), defineEIGEN_MPL2_ONLY
to exclude modules with other license from the build (done in the tests)./vendor/catch-single-header-2.13.7
- Single-file version of the Catch2 C++ test framework. BSL-1.0.