|
Kalman 0.5.4
Kalman Filter
|
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.
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.
See INSTALL.md for installation instructions.
The library is the state estimation layer between the application and an established linear algebra backend.
Link against the CMake target fcarouge-kalman::kalman.
Include the library header in your sources:
For forward declarations only, include instead:
The public API lives in the fcarouge namespace. Import it with a using-declaration or namespace alias as your project's style prefers.
Filters with state x output x input dimensions as 1x1x1 and 1x1x0 (no input) are supported through vanilla C++. Add a backend for higher dimension filters:
| Integration | Example Backend |
|---|---|
| Built-in Types | No backend needed. |
| Eigen | Example backend: support/eigen. |
| Eigen + TypedLinearAlgebra | Example backend: support/eigexed. |
| Eigen + TypedLinearAlgebra + mp-units | Example backend: support/mp_units_eigen. |
| Freestanding | Example backend: support/freestanding. |
| mp-units | Example backend: support/mp_units. |
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.
| Filter | Sample |
|---|---|
| 1x1x0 constant system dynamic model filter of the building height. | Usage example: kf_1x1x0_building_height.cpp, with units: kf_1x1x0_building_height_unit.cpp. |
| 1x1x0 constant system dynamic model filter of the temperature of a liquid in a tank. | Usage example: kf_1x1x0_liquid_temperature.cpp, with units: kf_1x1x0_liquid_temperature_unit.cpp. |
| 1x1x1 constant velocity dynamic model filter of the 1-dimension position of a dog. | Usage example: kf_1x1x1_1d_dog_position.cpp, with units: kf_1x1x1_1d_dog_position_unit.cpp. |
| 1x2x0 nonlinear output model extended filter of the terrain altitude below a vehicle, from a range finder and an optical flow sensor, with consistency tests and a constraint applied outside of the filter, as in the PX4 autopilot. | Usage example with units: ekf_1x2x0_terrain_estimation_unit.cpp. |
| 2x1x0 dynamic linear regression filter of the pairs trading hedge ratio and intercept of two stocks, with a daily output model, for mean reversion trading. | Usage example with units: kf_2x1x0_pairs_hedge_ratio_unit.cpp. |
| 2x1x0 local linear trend structural time series model filter of the short-term forecast of a daily stock closing price. | Usage example with units: kf_2x1x0_stock_price_unit.cpp. |
| 2x1x1 constant acceleration dynamic model filter of the 1-dimension position and velocity of a rocket altitude. | Usage example: kf_2x1x1_1d_rocket_altitude.cpp. |
| 2x1x1 constant velocity dynamic model filter of the 1-dimension position and velocity of a target relative to a vehicle, driven by the vehicle's acceleration, from camera measurements delivered after a delay, rewound to the instant they describe and replayed. | Usage example with units: kf_2x1x1_delayed_target_tracking_unit.cpp. |
| 3x1x0 nonlinear output model extended filter of the 3-dimension position of a receiver from its ranges to beacons at known positions, with consistency tests after an alignment, as in the ArduPilot autopilot. | Usage example with units: ekf_3x1x0_beacon_position_unit.cpp. |
| 3x2x0 nonlinear output model extended filter of the 2-dimension wind and the airspeed sensor scale factor of a circling fixed-wing aircraft, from its airspeed and sideslip, with consistency tests and a bounded scale factor, as in the PX4 autopilot. | Usage example with units: ekf_3x2x0_wind_airspeed_scale_unit.cpp. |
| 4x1x0 nonlinear dynamic model extended filter of the thermal, current of warm air, strength, radius, and location. | Usage example: ekf_4x1x0_thermal_soaring.cpp. |
| 4x2x0 nonlinear output model extended filter of the 2-dimension position and velocity of a target from its bearing and range to a sensor, with the measured bearing unwrapped around the predicted one. | Usage example with units: ekf_4x2x0_range_bearing_tracking_unit.cpp. |
| 6x2x0 constant acceleration dynamic model filter of the 2-dimension vehicle location, velocity, and acceleration. | Usage example: kf_6x2x0_2d_vehicle_position.cpp, with units: kf_6x2x0_2d_vehicle_position_unit.cpp. |
| 6x4x0 extended filter of the 3-dimension position and velocity of the NASA Apollo lunar module abort guidance system for spacecraft rendezvous approaching the command/service module. | Usage example: ekf_6x4x0_3d_apollo_position.cpp. |
| 8x4x0 constant velocity dynamic model filter of the 2-dimension position and velocity of the center, aspect ratio, and height of a bounding box. | Usage example: kf_8x4x0_2d_bounding_box.cpp. |
| 9x4x0 error-state extended filter of the attitude, velocity, and gyroscope bias of a camera gimbal, from its inertial sensors, a zero velocity, and a compass heading, as in the ArduPilot autopilot. | Usage example with units: ekf_9x4x0_gimbal_attitude_unit.cpp. |
Example from the 2-dimension vehicle location, velocity, and acceleration vehicle estimation sample. Six estimated states and two observed outputs filter.
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.
Also documented in the fcarouge/kalman.hpp header.
| Template Parameter | Definition |
|---|---|
Filter | Exposition only. The deduced internal filter template parameter. Class template argument deduction (CTAD) figures out the filter type based on the declared configuration. See deduction guide. |
| 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 |
The member types are optionally present according to the filter configuration.
| Member Function | Definition |
|---|---|
(constructor) | Constructs a Kalman filter from its declared configuration. |
(move constructor) | Constructs the filter, default. |
(move assignment operator) | Assigns values to the filter, default. |
(destructor) | Destructs the filter. |
| Characteristic | Definition |
|---|---|
f | Manages the state transition matrix F. Gets or sets the value. Configures the callable object of expression state_transition(const input &, const PredictionTypes &...), state_transition(const PredictionTypes &...), or state_transition(const state &, const PredictionTypes &...) evaluated at the state estimate before the prediction, of the type configured at construction, to compute the value. The default value is the matrix with all its diagonal elements equal to ones, and zeroes everywhere else. |
g | Manages the control transition matrix G. Gets or sets the value. Configures the callable object of expression input_control(const PredictionTypes &...), of the type configured at construction, to compute the value. The default value is the matrix with all its diagonal elements equal to ones, and zeroes everywhere else. This member function is defined only if the filter supports input control. |
h | Manages the observation transition matrix H. Gets or sets the value. Configures the callable object of expression output_model(const state &, const UpdateTypes &...), of the type configured at construction, to compute the value. The default value is the matrix with all its diagonal elements equal to ones, and zeroes everywhere else. 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 matrix with all its diagonal elements equal to ones, and zeroes everywhere else. |
p | Manages the estimated covariance matrix P. Gets or sets the value. The default value is the matrix with all its diagonal elements equal to ones, and zeroes everywhere else. |
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 or sets the value. Configures the callable object of expression process_uncertainty(const state &, const PredictionTypes &...), of the type configured at construction, 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 or sets the value. Configures the callable object of expression output_uncertainty(const state &, const output &, const UpdateTypes &...), of the type configured at construction, 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 matrix with all its diagonal elements equal to ones, and zeroes everywhere else. |
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 or 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. |
The characteristics are optionally present according to the filter configuration. A characteristic is set from values that direct-list-initialize it, without narrowing of the characteristic nor of its elements, through a modifiable filter, or replaced by a callable of its configured type. A characteristic is read as a constant reference from an lvalue filter, valid for the lifetime of the filter and reflecting its later changes, and as a moved value from an rvalue filter.
| Modifier | Definition |
|---|---|
predict | Produces estimates of the state variables and uncertainties. |
update | Updates the estimates with the outcome of a measurement. |
The prediction and the update take the arguments of their declared types followed by values that direct-list-initialize the input and the output, respectively.
Declare zero, one, or more policies among the characteristics of a filter, in any order, to select its update form or to condition its estimate uncertainty. A policy is part of the filter type, selected at compile-time, at no runtime cost. An omitted policy selects the default filter.
| Policy | Definition |
|---|---|
joseph_form | Updates the estimate uncertainty as P = (I - KH)P(I - KH)ᵀ + KRKᵀ, the default, declared explicitly. Valid for any gain, the most robust and the most expensive form, evaluated at a quadratic rather than a cubic cost in the state dimension for a scalar output. |
information_form | Updates the estimate uncertainty as P = (P⁻¹ + HᵀR⁻¹H)⁻¹. Inverts state-sized rather than output-sized matrices: cheaper only for outputs much larger than the state. Requires invertible estimate and output uncertainties. |
simple_form | Updates the estimate uncertainty as P = (I - KH)P, the cheapest form, evaluated at a quadratic rather than a cubic cost in the state dimension for a scalar output. Rounding loses the symmetry of the estimate uncertainty and may lose its positive semi-definiteness. |
symmetric_form | Updates the estimate uncertainty as P = P - KSKᵀ, symmetric in exact arithmetic. Subtracting nearly equal matrices of a precise output may lose the positive semi-definiteness of the estimate uncertainty. |
symmetrize | Averages the estimate uncertainty with its transpose after each update and prediction, P = (P + Pᵀ) / 2, restoring its symmetry lost to rounding. |
floors{σ...} | Keeps each state variance at least σ² after each update and prediction, one standard deviation σ per state in its units: a runtime value, or a compile-time constant cw<σ> without storage. Useful when a variance would otherwise collapse, an overconfident filter ignoring new outputs, for example under model errors or for weakly observable states. |
Without an update form policy, the filter updates the estimate uncertainty in the Joseph form: declaring joseph_form is the default filter, of the same type. The update forms agree in exact arithmetic since the filter computes the optimal gain: they trade numerical robustness for cost. A filter declares at most one update form, one symmetrization, and one floors. The symmetrization keeps the diagonal of the estimate uncertainty, which the floors change alone: their order does not matter.
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. Hosted implementation only.
Pipe one or more decorators to the filter declaration to attach additional responsibilities. A decorator takes ownership of the filter: decorate an existing filter by moving it, std::move(filter) | print.
| Decorator | Definition |
|---|---|
print | Print filter activities to the standard output. Hosted implementation only. |
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:
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:
double with about 16 significant digits to reduce loss of information compared to float.float measurement of a double state, before passing them to the filter.<format> and <print> headers, are omitted from a freestanding implementation rather than reimplemented. The same header serves both implementations without configuration.std::function restores the replacement by any compatible callable, at the cost of an indirection, and of allocations for std::function.The filter's state, output, and input column vectors should be strongly typed parameters to allow the filter to participate in full compile-time safeties verification.
There exist Kalman filters with hundreds of state variables.
The float data type has about seven significant digits. Floating point error is a loss of information to account for in design.
The units of useful matrices are factorizable, i.e. the unit of an element is expressed as the product of the row and column indexed units. The deduced units result type of a matrix product collapses and folds the inner indexed units merely returning the outer units.
Safe physical linear algebra not only includes types and units safety, but also coordinate axes and frames reference.
A specialized filter is an optimization of the general equations, not a new model. A filter declared without a characteristic is the general filter as-if that characteristic were at its default value, with the unused algebra removed. The meaning of a prediction or an update is a property of the model alone, never of how the filter is declared. Each specialized filter should be tested to agree with the general filter.
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.
The embedded footprint reports the flash, RAM, and worst-case stack sizes of filter configurations on a bare-metal Arm Cortex-M4 target.
| Term | Definition |
|---|---|
| CKF | The Cubature Kalman Filter is a sampled nonlinear version of the Kalman filter. Useful for high-dimension nonlinear dynamics systems. This filter propagates 2n equally weighted cubature points, selected by a third-degree spherical-radial cubature rule, without tuning parameters. |
| 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. |
| EKF-IMM | The Interacting Multiple Model Extended Kalman Filter runs a bank of EKFs, one per dynamics model, mixed by Markov chain model transition probabilities. Useful for maneuvering systems switching between dynamics, such as target tracking. This filter estimates the state and the probability of each model. |
| EnKF | The Ensemble Kalman Filter is the Monte Carlo version of the Kalman filter. Useful for very high-dimension nonlinear systems, such as weather forecasting. This filter propagates an ensemble of state samples and estimates the covariance from the ensemble sample 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. |
| Euler-KF | The Euler angles Kalman Filter estimates an attitude parameterized by roll, pitch, and yaw angles. Useful for attitude estimation away from the gimbal lock. This filter has a minimal three-parameter attitude state, singular at a pitch of ±90°. |
| Fading-Memory | The Fading-Memory Kalman Filter scales the predicted estimate uncertainty by a fading factor greater than one. Useful for systems with model mismatch or unmodeled dynamics. This filter exponentially discounts older measurements in favor of recent ones. |
| Finite/Fixed-Memory | The Finite or Fixed-Memory Kalman Filter estimates the state from a sliding window of the most recent measurements only. Useful for systems with model mismatch or temporary disturbances. This filter has a finite impulse response, bounding the influence of any measurement in time. |
| FKF | The Fast Kalman Filter exploits the block structure of a large linear system whose states partition into local states and global parameters. Useful for very large systems, such as calibrating many sensors against common parameters. This filter solves the partitioned equations block by block rather than inverting the full matrices, at a cost growing linearly rather than cubically in the number of blocks. |
| FKF | The Federated Kalman Filter is the decentralized version of the Kalman filter. Useful for fault-tolerant multi-sensor fusion. This filter fuses the estimates of local filters, one per sensor, in a master filter sharing information back to the local filters. |
| Forward-Backward | The Forward-Backward Kalman Filter, or two-filter smoother, combines a forward filter with a backward-in-time filter. Useful for offline post-processing. This filter estimates each state from all past and future measurements. |
| IEKF | The Iterated Extended Kalman Filter iterates the update of the Extended Kalman Filter. Useful for strongly nonlinear output models. This filter relinearizes the output model about each new estimate, a Gauss-Newton iteration, until convergence. |
| Joseph | The Joseph form updates the estimate uncertainty as P = (I - KH)P(I - KH)ᵀ + KRKᵀ. Useful for numerical robustness. This form is valid for any gain and preserves the symmetry and positive semi-definiteness of the estimate uncertainty. |
| KF | The Kalman Filter is the optimal linear estimator, in the minimum mean square error sense, of a linear system with Gaussian noises. Useful for linear dynamics systems. This filter recursively predicts the state and its uncertainty, then updates them with a measurement. |
| Linearized | The Linearized Kalman Filter linearizes the model about a nominal trajectory known in advance rather than about the current estimate. Useful for systems following a planned trajectory. This filter may precompute its gains, but diverges when the state departs from the nominal trajectory. |
| MEKF | The Multiplicative Extended Kalman Filter estimates a unit quaternion attitude through a three-component attitude error multiplicatively composed with a reference quaternion. Useful for attitude estimation. This filter preserves the quaternion unit norm with a minimal error state. |
| MRP-EKF | The Modified Rodrigues Parameters Extended Kalman Filter estimates an attitude parameterized by modified Rodrigues parameters. Useful for attitude estimation. This filter has a minimal three-parameter attitude state, switching to the shadow set to avoid its singularity at a rotation of 360°. |
| MRP-UKF | The Modified Rodrigues Parameters Unscented Kalman Filter is the unscented version of the MRP-EKF. Useful for highly nonlinear attitude estimation. This filter samples sigma points of the modified Rodrigues parameters attitude. |
| MSCKF | The Multi-State Constraint Kalman Filter is an EKF-based approach that leverages constraints between multiple and varied measurements to improve pose estimation accuracy and robustness. |
| QSR-UKF | The Quaternion Square-Root Unscented Kalman Filter is the square-root unscented version of the MEKF. Useful for highly nonlinear attitude estimation. This filter propagates the Cholesky factor of the covariance of quaternion attitude sigma points. |
| SKF | The Schmidt Kalman Filter, or consider filter, accounts for the uncertainty of nuisance parameters without estimating them. Useful for systems with uncertain biases or parameters not worth estimating. This filter keeps the consider parameters and their covariances but does not update them. |
| SKF | The Switching Kalman Filter estimates a system switching between linear dynamics models, a discrete Markov chain regime. Useful for systems with abrupt changes of behavior, such as fault detection or maneuvering targets. This filter runs a bank of Kalman filters, one per regime, and merges their estimates and the regime probabilities, through generalized pseudo-Bayesian or interacting multiple model approximations. |
| Smoother | The Kalman smoother estimates past states from measurements up to a later time. Useful for offline post-processing and delayed decisions. This smoother runs a fixed-interval (Rauch-Tung-Striebel), fixed-lag, or fixed-point backward pass over the filter estimates. |
| SR-UKF | The Square-Root Unscented Kalman Filter handles non-linearity with covariance factorization of the unscented transformations (sigma points) to maintain numerical stability. |
| UDU | The UDU, or Bierman-Thornton, Kalman Filter factorizes the estimate uncertainty as P = UDUᵀ, with a unit upper triangular U and a diagonal D. Useful for numerical stability on limited precision hardware. This filter guarantees a positive semi-definite estimate uncertainty without square roots. |
| UKF | The Unscented Kalman Filter is the sampled version of the Extended Kalman Filter. Useful for highly nonlinear dynamics systems. This filter samples unscented transformations (sigma points) about an estimate working point of the current mean using an Unscented Transformation technique. |
| UKF-GSF | The Gaussian Sum Filter of Unscented Kalman Filters approximates a non-Gaussian distribution by a weighted sum of Gaussians, one UKF each. Useful for multimodal distributions, such as an unknown initial heading. This filter weights each UKF by the likelihood of its measurement residuals. |
| UKF-IMM | The Interacting Multiple Model Unscented Kalman Filter is the unscented version of the EKF-IMM. Useful for maneuvering highly nonlinear systems. This filter mixes a bank of UKFs, one per dynamics model. |
| USQUE | The UnScented QUaternion Estimator is an unscented attitude filter. Useful for attitude estimation with large initial errors. This filter samples sigma points of a generalized Rodrigues parameters attitude error, composed with a reference unit quaternion. |
| UT | The Unscented Transformation propagates a mean and covariance through a nonlinear function from a deterministic set of weighted sigma points. Useful for the sampled nonlinear filters, such as the UKF. This transformation captures the mean and covariance to the second order without derivatives. |
The library is used in projects:
Your project link here!
The library is designed, developed, and tested with the help of third-party tools and services acknowledged and thanked here:
Thank you to everyone who has contributed code, tests, or documentation!
See CONTRIBUTORS.md for the full, credited list, CHANGELOG.md for notable changes, and CONTRIBUTING.md to join.
Become a sponsor today! Support this project with coffee and infrastructure!
Your group logo and link here!
Your name and link here!
Thanks everyone!
Need help integrating, tuning, or extending Kalman filters in your product? Commercial support, consulting, training, and custom development are available. Get in touch.

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