Kalman 0.3.0
Kalman Filter
|
A generic Kalman filter. More...
#include <kalman.hpp>
Public Types | |
Public Member Types | |
using | state = implementation::state |
Type of the state estimate column vector X. | |
using | output = implementation::output |
Type of the observation column vector Z. | |
using | estimate_uncertainty = implementation::estimate_uncertainty |
Type of the estimated correlated variance matrix P. | |
using | process_uncertainty = implementation::process_uncertainty |
Type of the process noise correlated variance matrix Q. | |
using | output_uncertainty = implementation::output_uncertainty |
Type of the observation noise correlated variance matrix R. | |
using | state_transition = implementation::state_transition |
Type of the state transition matrix F. | |
using | gain = implementation::gain |
Type of the gain matrix K. | |
using | innovation = implementation::innovation |
Type of the innovation column vector Y. | |
using | innovation_uncertainty = implementation::innovation_uncertainty |
Type of the innovation uncertainty matrix S. | |
Public Member Functions | |
Public Member Functions | |
constexpr | kalman ()=default |
Constructs a Kalman filter without configuration. | |
constexpr | kalman (const kalman &other)=delete |
Copy constructs a filter. | |
constexpr | kalman (kalman &&other) noexcept=default |
Move constructs a filter. | |
constexpr auto | operator= (const kalman &other) -> kalman &=delete |
Copy assignment operator. | |
constexpr auto | operator= (kalman &&other) noexcept -> kalman &=default |
Move assignment operator. | |
constexpr | ~kalman ()=default |
Destructs the Kalman filter. | |
Public Characteristics Member Functions | |
constexpr auto | x () const -> const state & |
Returns the state estimate column vector X. | |
constexpr auto | x () -> state & |
constexpr void | x (const auto &value, const auto &...values) |
Sets the state estimate column vector X. | |
constexpr auto | z () const -> const output & |
Returns the last observation column vector Z. | |
constexpr const auto & | u () const |
Returns the last control column vector U. | |
constexpr auto | p () const -> const estimate_uncertainty & |
Returns the estimated covariance matrix P. | |
constexpr auto | p () -> estimate_uncertainty & |
constexpr void | p (const auto &value, const auto &...values) |
Sets the estimated covariance matrix P. | |
constexpr auto | q () const -> const process_uncertainty & |
Returns the process noise covariance matrix Q. | |
constexpr auto | q () -> process_uncertainty & |
constexpr void | q (const auto &value, const auto &...values) |
Sets the process noise covariance matrix Q. | |
constexpr auto | r () const -> const output_uncertainty & |
Returns the observation noise covariance matrix R. | |
constexpr auto | r () -> output_uncertainty & |
constexpr void | r (const auto &value, const auto &...values) |
Sets the observation noise covariance matrix R. | |
constexpr auto | f () const -> const state_transition & |
Returns the state transition matrix F. | |
constexpr auto | f () -> state_transition & |
constexpr void | f (const auto &value, const auto &...values) |
Sets the state transition matrix F. | |
constexpr const auto & | h () const |
Returns the observation transition matrix H. | |
constexpr auto & | h () |
constexpr void | h (const auto &value, const auto &...values) |
Sets the observation transition matrix H. | |
constexpr const auto & | g () const |
Returns the control transition matrix G. | |
constexpr auto & | g () |
constexpr void | g (const auto &value, const auto &...values) |
Sets the control transition matrix G. | |
constexpr auto | k () const -> const gain & |
Returns the gain matrix K. | |
constexpr auto | y () const -> const innovation & |
Returns the innovation column vector Y. | |
constexpr auto | s () const -> const innovation_uncertainty & |
Returns the innovation uncertainty matrix S. | |
constexpr void | transition (const auto &callable) |
Sets the extended state transition function f(x). | |
constexpr void | observation (const auto &callable) |
Sets the extended state observation function h(x). | |
Public Filtering Member Functions | |
constexpr void | predict (const auto &...arguments) |
Produces estimates of the state variables and uncertainties. | |
template<std::size_t Position> | |
constexpr auto | predict () const |
Returns the Nth prediction argument. | |
constexpr void | update (const auto &...arguments) |
Updates the estimates with the outcome of a measurement. | |
template<std::size_t Position> | |
constexpr auto | update () const |
Returns the Nth update argument. | |
Private Types | |
Private Member Types | |
using | implementation = internal::kalman< State, Output, Input, internal::repack_t< UpdateTypes >, internal::repack_t< PredictionTypes > > |
Implementation details of the filter. | |
Private Attributes | |
Private Member Variables | |
implementation | filter |
Encapsulates the implementation details of the filter. | |
A generic 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 and predicts estimates by adding Gaussians. Designing a filter is as much art as science. Design the state $X$, $P$, the process $F$, $Q$, the measurement $Z$, $R$, the measurement function $H$, and if the system has control inputs $U$, $G$.
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.
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. |
std::function
could too. The polymorphic function wrapper was used in place of function pointers to enable default initialization from this class, captured member variables.Make this class usable in constant expressions.
Is this filter restricted to Newton's equations of motion? That is only a discretized continuous-time kinematic filter? How about non-Newtonian systems?
Symmetrization support might be superfluous. How to confirm it is safe to remove? Optional?
Would we want to support smoothers?
Prepare support for larger dataset recording for graphing, metrics of large test data to facilitate tuning.
Support filter generator from equation? Third party integration?
Compare performance of general filter with its equivalent generated?
Support ranges operator filter?
Support mux pipes https://github.com/joboccara/pipes operator filter?
Reproduce Ardupilot's inertial navigation EKF and comparison benchmarks in SITL (software in the loop simulation).
Should we provide the operator[] for the vector characteristics regardless of implementation? And for the matrix ones too? It could simplify client code.
Should we provide the operator[] for state directly on the filter? Is the state X always what the user would want?
Should a fluent interface be useful in addition to the imperative and declarative paradigms support?
Expand std::format support with standard arguments and Eigen3 types?
Support, test complex number filters?
Use automatic (Eigen::AutoDiffScalar?), symbolic, numerical solvers to define the filter characteristics and simplify solving the dynamic system for non-mathematicians.
Support, use "Taking Static Type-Safety to the Next Level - Physical Units for Matrices" by Daniel Withopf and record the lesson learned: both usage and development is harder without compile time units verification.
Should we add back the call operator? How to resolve the update/predict ordering? And parameter ordering?
Should we support the noise cross covariance N = E[wvᵀ]
for correlated noise sources, with default to null?
Can we implement Temporal Parallelization of Bayesian Smoothers, Simo Sarkka, Senior Member, IEEE, Angel F. Garc ıa-Fernandez, https://arxiv.org/pdf/1905.13002.pdf ? GPU implementation? Parallel implementation?
Definition at line 160 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::estimate_uncertainty = implementation::estimate_uncertainty |
Type of the estimated correlated variance matrix P.
Also known as Σ.
Definition at line 198 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::gain = implementation::gain |
Type of the gain matrix K.
Definition at line 212 of file kalman.hpp.
|
private |
Implementation details of the filter.
The internal implementation, filtering strategies, and presence of members vary based on the configured filter.
Definition at line 171 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::innovation = implementation::innovation |
Type of the innovation column vector Y.
Definition at line 215 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::innovation_uncertainty = implementation::innovation_uncertainty |
Type of the innovation uncertainty matrix S.
Definition at line 218 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::output = implementation::output |
Type of the observation column vector Z.
Also known as Y or O.
Definition at line 193 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::output_uncertainty = implementation::output_uncertainty |
Type of the observation noise correlated variance matrix R.
Definition at line 204 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::process_uncertainty = implementation::process_uncertainty |
Type of the process noise correlated variance matrix Q.
Definition at line 201 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::state = implementation::state |
Type of the state estimate column vector X.
Definition at line 188 of file kalman.hpp.
using fcarouge::kalman< State, Output, Input, UpdateTypes, PredictionTypes >::state_transition = implementation::state_transition |
Type of the state transition matrix F.
Also known as the fundamental matrix, propagation, Φ, or A.
Definition at line 209 of file kalman.hpp.
|
inlineconstexprdefault |
Constructs a Kalman filter without configuration.
|
inlineconstexprdelete |
Copy constructs a filter.
Constructs the filter with the copy of the contents of the other
filter.
other | Another filter to be used as source to initialize the elements of the filter with. |
|
inlineconstexprdefaultnoexcept |
Move constructs a filter.
Move constructor. Constructs the filter with the contents of the other
filter using move semantics (i.e. the data in other
filter is moved from the other into this filter).
other | Another filter to be used as source to initialize the elements of the filter with. |
|
inlineconstexprdefault |
Destructs the Kalman filter.
|
inlineconstexpr |
|
inlineconstexpr |
Returns the state transition matrix F.
|
inlineconstexpr |
Sets the state transition matrix F.
The state transition matrix F is of type state_transition
and the function is of the form state_transition(const state &, const input &, const PredictionTypes &...)
. For non-linear system, or extended filter, F is the Jacobian of the state 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.
value | The first copied initializer used to set the copied state transition matrix F or the copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be bound to the prediction arguments and called by the filter to compute the state transition matrix F function on prediction steps. |
values | The optional second and other copied initializers to set the state transition matrix F. |
|
inlineconstexpr |
|
inlineconstexpr |
Returns the control transition matrix G.
This member function is not present when the filter has no input control.
|
inlineconstexpr |
Sets the control transition matrix G.
The control transition matrix G is of type input_control
and the function is of the form input_control(const PredictionTypes &...)
. This member function is not present when the filter has no input control.
value | The first copied initializer used to set the copied control transition matrix G or the copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be bound to the prediction arguments and called by the filter to compute the control transition matrix G on prediction steps. |
values | The optional second and other copied initializers to set the control transition matrix G. |
|
inlineconstexpr |
|
inlineconstexpr |
Returns the observation transition matrix H.
This member function is not present when the filter has no output model.
|
inlineconstexpr |
Sets the observation transition matrix H.
The observation transition matrix H is of type output_model
and the function is of the form output_model(const state &, const UpdateTypes &...)
. For non-linear system, or extended filter, H is the Jacobian of the state 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 column vector X. This member function is not present when the filter has no output model.
value | The first copied initializer used to set the copied observation transition matrix H or the copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be bound to the prediction arguments and called by the filter to compute the observation, measurement transition matrix H on update steps. |
values | The optional second and other copied initializers to set the observation transition matrix H. |
|
inlineconstexpr |
Returns the gain matrix K.
|
inlineconstexpr |
Sets the extended state observation function h(x).
callable | The copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be called to compute the observation Z on update steps of expression output(const state &, const UpdateTypes &...arguments) . The default function h(x) = H * X is suitable for linear systems. For non-linear system, or extended filter, the client implements a linearization of the observation function hand the state observation H matrix is the Jacobian of the state observation function. |
|
inlineconstexprdelete |
Copy assignment operator.
Destroys or copy-assigns the contents with a copy of the contents of the other filter.
other | Another filter to be used as source to initialize the elements of the filter with. |
*this
.
|
inlineconstexprdefaultnoexcept |
Move assignment operator.
Replaces the contents of the filter with those of the other
filter using move semantics (i.e. the data in other
filter is moved from the other into this filter). The other filter is in a valid but unspecified state afterwards.
other | Another filter to be used as source to initialize the elements of the filter with. |
*this
.
|
inlineconstexpr |
|
inlineconstexpr |
Returns the estimated covariance matrix P.
|
inlineconstexpr |
Sets the estimated covariance matrix P.
value | The first copied initializer used to set the estimated covariance matrix P. |
values | The optional second and other copied initializers to set the estimated covariance matrix P. |
|
inlineconstexpr |
Returns the Nth prediction argument.
Convenience access to the last used prediction arguments.
The | non-type template parameter index position of the prediction argument types. |
PredictionTypes
class template type.
|
inlineconstexpr |
Produces estimates of the state variables and uncertainties.
Also known as the propagation step. Implements the total probability theorem. Estimate the next state by suming the known probabilities.
arguments | The prediction and input parameters of the filter, in that order. The arguments need to be compatible with the filter types. The prediction parameters convertible to the PredictionTypes template pack types are passed through for computations of prediction matrices. The control parameter pack types convertible to the Input template type. The prediction types are explicitly defined with the class definition. |
Consider if returning the state column vector X would be preferable? Or fluent interface? Would be compatible with an ES-EKF implementation?
Can the parameter pack of PredictionTypes
be explicit in the method declaration for user clarity?
Referenced by fcarouge::benchmark::anonymous_namespace{predict_1x1x0.cpp}::bench(), fcarouge::benchmark::anonymous_namespace{predict_1x1x1.cpp}::bench(), and fcarouge::benchmark::anonymous_namespace{predict_linalg_x1x.cpp}::bench().
|
inlineconstexpr |
|
inlineconstexpr |
Returns the process noise covariance matrix Q.
|
inlineconstexpr |
Sets the process noise covariance matrix Q.
value | The first copied initializer used to set the process noise covariance matrix Q is of type process_uncertainty and the function is of the form process_uncertainty(const state &, const PredictionTypes &...) . The copied process noise covariance matrix Q or the copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be bound to the prediction arguments and called by the filter to compute the process noise covariance matrix Q on prediction steps. |
values | The optional second and other copied initializers to set the process noise covariance matrix Q. |
|
inlineconstexpr |
|
inlineconstexpr |
Returns the observation noise covariance matrix R.
The variance there is in each measurement.
|
inlineconstexpr |
Sets the observation noise covariance matrix R.
value | The first copied initializer used to set the observation noise covariance matrix R is of type output_uncertainty and the function is of the form output_uncertainty(const state &, const output &, const UpdateTypes &...) . The copied observation noise covariance matrix R or the copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be called by the filter to compute the observation noise covariance matrix R on prediction steps. |
values | The optional second and other copied initializers to set the observation noise covariance matrix R. |
|
inlineconstexpr |
Returns the innovation uncertainty matrix S.
|
inlineconstexpr |
Sets the extended state transition function f(x).
callable | The copied target Callable object (function object, pointer to function, reference to function, pointer to member function, or pointer to data member) that will be called to compute the next state X on prediction steps of expression state(const state &, const input &, const PredictionTypes &...) . The default function f(x) = F * X is suitable for linear systems. For non-linear system, or extended filter, implement a linearization of the transition function f and the state transition F matrix is the Jacobian of the state transition function. |
|
inlineconstexpr |
Returns the last control column vector U.
This member function is not present when the filter has no input.
|
inlineconstexpr |
Returns the Nth update argument.
Convenience access to the last used update arguments.
The | non-type template parameter index position of the update argument types. |
UpdateTypes
class template type.
|
inlineconstexpr |
Updates the estimates with the outcome of a measurement.
Also known as the observation or correction step. Implements the Bayes' theorem. Combine one measurement and the prior estimate by applying the multiplicative law.
arguments | The update and output parameters of the filter, in that order. The arguments need to be compatible with the filter types. The update parameters convertible to the UpdateTypes template pack types are passed through for computations of update matrices. The observation parameter pack types convertible to the Output template type. The update types are explicitly defined with the class definition. |
Consider if returning the state column vector X would be preferable? Or fluent interface? Would be compatible with an ES-EKF implementation?
Can the parameter pack of UpdateTypes
be explicit in the method declaration for user clarity?
Referenced by fcarouge::benchmark::anonymous_namespace{update_1x1x0.cpp}::bench(), fcarouge::benchmark::anonymous_namespace{update_1x1x1.cpp}::bench(), and fcarouge::benchmark::anonymous_namespace{update_linalg_xx0.cpp}::bench().
|
inlineconstexpr |
|
inlineconstexpr |
Returns the state estimate column vector X.
inline constexpr auto & x(this auto&& self)
.
|
inlineconstexpr |
Sets the state estimate column vector X.
value | The first copied initializer used to set the state estimate column vector X. |
values | The optional second and other copied initializers to set the state estimate column vector X. |
|
inlineconstexpr |
Returns the innovation column vector Y.
|
inlineconstexpr |
Returns the last observation column vector Z.
|
private |
Encapsulates the implementation details of the filter.
Definition at line 180 of file kalman.hpp.