The Kalman filter is a Bayesian filter that uses multivariate Gaussians, a recursive state estimator, a linear quadratic estimator (LQE), and an Infinite Impulse Response (IIR) filter. It is a control theory tool applicable to signal estimation, sensor fusion, or data assimilation problems. The filter is applicable for unimodal and uncorrelated uncertainties. The filter assumes white noise, propagation and measurement functions are differentiable, and that the uncertainty stays centered on the state estimate. The filter is the optimal linear filter under assumptions. The filter updates estimates by multiplying Gaussians rather than integrating differential equations. The filter predicts estimates by adding Gaussians. The filter maintains an estimate of the state and its uncertainty over the sequential estimation process. The filter is named after Rudolf E. Kálmán, who was one of the primary developers of its theory in 1960.
Designing a filter is as much art as science, with the following recipe. Model the real world in state-space notation. Then, compute and select the fundamental matrices, select the states X, P, the processes F, Q, the measurements Z, R, the measurement function H, and if the system has control inputs U, G. Evaluate the performance and iterate.
This library supports various simple and extended filters. The implementation is independent from linear algebra backends. Arbitrary parameters can be added to the prediction and update stages to participate in gain-scheduling or linear parameter varying (LPV) systems. The default filter type is a generalized, customizable, and extended filter. The default type parameters implement a one-state, one-output, and double-precision floating-point type filter. The default update equation uses the Joseph form. Examples illustrate various usages and implementation tradeoffs. A standard formatter specialization is included for representation of the filter states. Filters with state x output x input dimensions as 1x1x1 and 1x1x0 (no input) are supported through vanilla C++. Higher dimension filters require a linear algebra backend. Customization points and type injections allow for implementation tradeoffs.
- Examples
- Installation
- Reference
- Considerations
- Resources
- Continuous Integration & Deployment Actions
- License
Example from the building height estimation sample. One estimated state and one observed output filter.
kalman filter;
filter.x(60.);
filter.p(225.);
filter.r(25.);
filter.update(48.54);Example from the 2-dimension vehicle location, velocity, and acceleration vehicle estimation sample. Six estimated states and two observed outputs filter.
using kalman = kalman<vector<double, 6>, vector<double, 2>>;
kalman filter;
filter.x(0., 0., 0., 0., 0., 0.);
filter.p(kalman::estimate_uncertainty{ { 500, 0, 0, 0, 0, 0 },
{ 0, 500, 0, 0, 0, 0 },
{ 0, 0, 500, 0, 0, 0 },
{ 0, 0, 0, 500, 0, 0 },
{ 0, 0, 0, 0, 500, 0 },
{ 0, 0, 0, 0, 0, 500 } });
filter.q(0.2 * 0.2 * kalman::process_uncertainty{ { 0.25, 0.5, 0.5, 0, 0, 0 },
{ 0.5, 1, 1, 0, 0, 0 },
{ 0.5, 1, 1, 0, 0, 0 },
{ 0, 0, 0, 0.25, 0.5, 0.5 },
{ 0, 0, 0, 0.5, 1, 1 },
{ 0, 0, 0, 0.5, 1, 1 } });
filter.f(kalman::state_transition{ { 1, 1, 0.5, 0, 0, 0 },
{ 0, 1, 1, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0.5 },
{ 0, 0, 0, 0, 1, 1 },
{ 0, 0, 0, 0, 0, 1 } });
filter.h(kalman::output_model{ { 1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0 } });
filter.r(kalman::output_uncertainty{ { 9, 0 }, { 0, 9 } });
filter.predict();
filter.update(-375.93, 301.78);Example from the thermal, current of warm air, strength, radius, and location estimation sample. Four estimated states and one observed output extended filter with two additional prediction arguments and two additional update arguments.
using kalman = kalman<vector<float, 4>, float, void, std::tuple<float, float>,
std::tuple<float, float>>;
kalman filter;
filter.x(1 / 4.06, 80, 0, 0);
filter.p(kalman::estimate_uncertainty{ { 0.0049, 0, 0, 0 },
{ 0, 400, 0, 0 },
{ 0, 0, 400, 0 },
{ 0, 0, 0, 400 } });
filter.transition([](const kalman::state &x, const float &drift_x,
const float &drift_y) -> kalman::state {
return x + kalman::state{ 0, 0, -drift_x, -drift_y };
});
filter.q(kalman::process_uncertainty{ { 0.000001, 0, 0, 0 },
{ 0, 0.0009, 0, 0 },
{ 0, 0, 0.0009, 0 },
{ 0, 0, 0, 0.0009 } });
filter.r(0.2025);
filter.observation([](const kalman::state &x, const float &position_x,
const float &position_y) -> kalman::output {
return kalman::output{ x(0) *
std::exp(-((x(2) - position_x)*(x(2) - position_x) +
(x(3) - position_y) * (x(3) - position_y)) / x(1) * x(1)) };
filter.h([](const kalman::state &x, const float &position_x,
const float &position_y) -> kalman::output_model {
const auto exp{ std::exp(-((x(2) - position_x) * (x(2) - position_x) +
(x(3) - position_y) * (x(3) - position_y)) / (x(1) * x(1))) };
const kalman::output_model h{
exp,
2 * x(0) * (((x(2) - position_x) * (x(2) - position_x) +
(x(3) - position_y) * (x(3) - position_y)) / (x(1) * x(1))) * exp,
-2 * (x(0) * (x(2) - position_x) / (x(1) * x(1))) * exp,
-2 * (x(0) * (x(3) - position_y) / (x(1) * x(1))) * exp
};
return h;
});
filter.predict(drift_x, drift_y);
filter.update(position_x, position_y, variometer);- 1x1 constant system dynamic model filter of the temperature of a liquid in a tank.
- 1x1x1 constant velocity dynamic model filter of the 1-dimension position of a dog.
- 2x1x1 constant acceleration dynamic model filter of the 1-dimension position and velocity of a rocket altitude.
- 8x4 constant velocity dynamic model filter of the 2-dimension position and velocity of the center, aspect ratio, and height of a bounding box.
git clone --depth 1 "https://github.com/FrancoisCarouge/kalman"
cmake -S "kalman" -B "build"
cmake --build "build" --parallel
sudo cmake --install "build"find_package(kalman)
target_link_libraries(your_target PRIVATE kalman::kalman)For more, see installation instructions.
Also documented in the fcarouge/kalman.hpp header.
template <
typename State,
typename Output,
typename Input,
typename UpdateTypes,
typename PredictionTypes>
class kalman| Template Parameter | Definition |
|---|---|
State |
The type template parameter of the state column vector X. State variables can be observed (measured), or hidden variables (inferred). This is the the mean of the multivariate Gaussian. Defaults to double. |
Output |
The type template parameter of the measurement column vector Z. Defaults to double. |
Input |
The type template parameter of the control U. A void input type can be used for systems with no input control to disable all of the input control features, the control transition matrix G support, and the other related computations from the filter. Defaults to void. |
UpdateTypes |
The additional update function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the state observation H and the observation noise R matrices. The parameters are also propagated to the state observation function object H. Defaults to no parameter types, the empty pack. |
PredictionTypes |
The additional prediction function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the process noise Q, the state transition F, and the control transition G matrices. The parameters are also propagated to the state transition function object F. Defaults to no parameter types, the empty pack. |
| Member Type | Dimensions | Definition | Also Known As |
|---|---|---|---|
estimate_uncertainty |
x by x | Type of the estimated, hidden covariance matrix p. |
P, Σ |
gain |
x by z | Type of the gain matrix k. |
K, L |
innovation_uncertainty |
z by z | Type of the innovation uncertainty matrix s. |
S |
innovation |
z by 1 | Type of the innovation column vector y. |
Y |
input_control |
x by u | Type of the control transition matrix g. This member type is defined only if the filter supports input control. |
G, B |
input |
u by 1 | Type of the control column vector u. This member type is defined only if the filter supports input. |
U |
output_model |
z by x | Type of the observation transition matrix h. This member type is defined only if the filter supports output model. |
H, C |
output_uncertainty |
z by z | Type of the observation, measurement noise covariance matrix r. |
R |
output |
z by 1 | Type of the observation column vector z. |
Z, Y, O |
process_uncertainty |
x by x | Type of the process noise covariance matrix q. |
Q |
state_transition |
x by x | Type of the state transition matrix f. |
F, Φ, A |
state |
x by 1 | Type of the state estimate, hidden column vector x. |
X |
| Member Function | Definition |
|---|---|
(constructor) |
Constructs the filter. |
(destructor) |
Destructs the filter. |
operator= |
Assigns values to the filter. |
| Characteristic | Definition |
|---|---|
f |
Manages the state transition matrix F. Gets the value. Initializes and sets the value. Configures the callable object of expression state_transition(const state &, const input &, const PredictionTypes &...) to compute the value. The default value is the identity matrix. |
g |
Manages the control transition matrix G. Gets the value. Initializes and sets the value. Configures the callable object of expression input_control(const PredictionTypes &...) to compute the value. The default value is the identity matrix. This member function is defined only if the filter supports input control. |
h |
Manages the observation transition matrix H. Gets the value. Initializes and sets the value. Configures the callable object of expression output_model(const state &, const UpdateTypes &...) to compute the value. The default value is the identity matrix. This member function is defined only if the filter supports output model. |
k |
Manages the gain matrix K. Gets the value last computed during the update. The default value is the identity matrix. |
p |
Manages the estimated covariance matrix P. Gets the value. Initializes and sets the value. The default value is the identity matrix. |
q |
Manages the process noise covariance matrix Q from the process noise w expected value E[wwᵀ] and its variance σ² found by measuring, tuning, educated guesses of the noise. Gets the value. Initializes and sets the value. Configures the callable object of expression process_uncertainty(const state &, const PredictionTypes &...) to compute the value. The default value is the null matrix. |
r |
Manages the observation, measurement noise covariance matrix R from the measurement noise v expected value E[vvᵀ] and its variance σ² found by measuring, tuning, educated guesses of the noise. Gets the value. Initializes and sets the value. Configures the callable object of expression output_uncertainty(const state &, const output &, const UpdateTypes &...) to compute the value. The default value is the null matrix. |
s |
Manages the innovation uncertainty matrix S. Gets the value last computed during the update. The default value is the identity matrix. |
u |
Manages the control column vector U. Gets the value last used in prediction. This member function is defined only if the filter supports input. |
x |
Manages the state estimate column vector X. Gets the value. Initializes and sets the value. The default value is the null column vector. |
y |
Manages the innovation column vector Y. Gets the value last computed during the update. The default value is the null column vector. |
z |
Manages the observation column vector Z. Gets the value last used during the update. The default value is the null column vector. |
transition |
Manages the state transition function object f. Configures the callable object of expression state(const state &, const input &, const PredictionTypes &...) to compute the transition state value. The default value is the equivalent to f(x) = F * X. The default function is suitable for linear systems. For extended filters transition is a linearization of the state transition while F is the Jacobian of the transition function: F = ∂f/∂X = ∂fj/∂xi that is each row i contains the derivatives of the state transition function for every element j in the state column vector X. |
observation |
Manages the state observation function object h. Configures the callable object of expression output(const state &, const UpdateTypes &...arguments) to compute the observation state value. The default value is the equivalent to h(x) = H * X. The default function is suitable for linear systems. For extended filters observation is a linearization of the state observation while H is the Jacobian of the observation function: H = ∂h/∂X = ∂hj/∂xi that is each row i contains the derivatives of the state observation function for every element j in the state vector X. |
| Modifier | Definition |
|---|---|
predict |
Produces estimates of the state variables and uncertainties. |
update |
Updates the estimates with the outcome of a measurement. |
A specialization of the standard formatter is provided for the filter. Use std::format to store a formatted representation of all of the characteristics of the filter in a new string. Standard format parameters to be supported.
kalman filter;
std::print("{}", filter);
// {"f": 1, "h": 1, "k": 1, "p": 1, "q": 0, "r": 0, "s": 1, "x": 0, "y": 0, "z": 0}Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud.
This package explores what could be a Kalman filter implementation a la standard library. The following concerns are considered:
- Separation of the application domain and integration needs.
- Separation of the mathematical concepts and linear algebra implementation.
- Generalization and specialization of modern language and library support.
In theory there is no difference between theory and practice, while in practice there is. The following engineering tradeoffs have been selected for this library implementation:
- Update and prediction additional arguments are stored in the filter at the costs of memory and performance for the benefits of consistent data access and records.
- The default floating point data type for the filter is
doublewith about 16 significant digits to reduce loss of information compared tofloat. - The ergonomics and precision of the default filter takes precedence over performance.
Design, development, and testing uncovered unexpected facets of the projects:
- The filter's state, output, and input column vectors should be type template parameters to allow the filter to participate in full compile-time verification of unit- and index-type safeties for input parameters and characteristics.
- There exist Kalman filters with hundreds of state variables.
- The
floatdata type has about seven significant digits. Floating point error is a loss of information to account for in design.
The benchmarks share some performance information. Custom specializations and implementations can outperform this library. Custom optimizations may include: using a different covariance estimation update formula; removing symmetry support; using a different matrix inversion formula; removing unused or identity model dynamics supports; implementing a generated, unrolled filter algebra expressions; or running on accelerator hardware.
| Term | Definition |
|---|---|
| EKF | The Extended Kalman Filter is the nonlinear version of the Kalman filter. Useful for nonlinear dynamics systems. This filter linearizes the model about an estimate working point of the current mean and covariance. |
| ESKF | The Error State Kalman Filter is the error estimation version of the Kalman filter. Useful for linear error state dynamics systems. This filter estimates the errors rather than the states. |
| UKF | The Unscented Kalman Filter is the sampled version of the Extended Kalman Filter. Useful for highly nonlinear dynamics systems. This filter samples sigma points about an estimate working point of the current mean using an Unscented Transformation technique. |
Further terms should be defined and demonstrated for completeness: CKF, EKF-IMM, EnKF, Euler-KF, Fading-Memory, Finite/Fixed-Memory, Forward-Backward, FKF, IEKF, Joseph, KF, Linearized, MEKF, MRP-EKF, MRP-UKF, MSCKF, SKF, Smoother, UKF-GSF, UKF-IMM, USQUE, UDU, and UT.
Resources to learn about Kalman filters:
- A New Approach to Linear Filtering and Prediction Problems by Kalman, Rudolph Emil in Transactions of the ASME - Journal of Basic Engineering, Volume 82, Series D, pp 35-45, 1960 - Transcription by John Lukesh.
- KalmanFilter.NET by Alex Becker.
- Kalman and Bayesian Filters in Python by Roger Labbe.
- How Kalman Filters Work by Tucker McClure of An Uncommon Lab.
- Wikipedia Kalman filter by Wikipedia, the free encyclopedia.
- Applications of Kalman Filtering in Aerospace 1960 to the Present by Mohinder S. Grewal and Angus P. Andrews. IEEE 2010.
The library is used in projects:
- GstKalman: A GStreamer Kalman filter video plugin.
Your project link here!
The library is designed, developed, and tested with the help of third-party tools and services acknowledged and thanked here:
- actions-gh-pages to upload the documentation to GitHub pages.
- Clang for compilation and code sanitizers.
- CMake for build automation.
- cmakelang for pretty CMake list files.
- Coveralls to measure code coverage.
- cppcheck for static analysis.
- Doxygen for documentation generation.
- Doxygen Awesome for pretty documentation.
- Eigen for linear algebra.
- GCC for compilation and code sanitizers.
- Google Benchmark to implement the benchmarks.
- lcov to process coverage information.
- MSVC for compilation and code sanitizers.
- Valgrind to check for correct memory management.
Become a sponsor today! Support this project with coffee and infrastructure!
Your group logo and link here!
Your name and link here!
Thanks everyone!
Kalman Filter is public domain:
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or distribute this software, either in source code form or as a compiled binary, for any purpose, commercial or non-commercial, and by any means.
In jurisdictions that recognize copyright laws, the author or authors of this software dedicate any and all copyright interest in the software to the public domain. We make this dedication for the benefit of the public at large and to the detriment of our heirs and successors. We intend this dedication to be an overt act of relinquishment in perpetuity of all present and future rights to this software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
For more information, please refer to https://unlicense.org