Crate status: unstable
Provide implementations for a Kalman Filter, Extended Kalman Filter and an efficient Stochastic Cloning Kalman Filter.
Vectors are represented by lowercase letters like a, matrices are written as uppercase letters like A. Additional information may be added after an underscore and optionally grouped with {}
, for example v_{bullet}
.
In source code, matrices are prefixed with mat_
, column vectors with vec_
and row vectors with rvec_
. All symbols are converted to lowercase. (Only to keep the compiler from complaining). A describing index may be
added as well, for example vec_v_bullet
for the vector v that describes the bullet speed.
A discrete time-invariant linear system uses the following matrix and vector letters:
system measurement
noise x_init noise
│ │ │
│ ▼ │
┌─────┐ ▼ x_{k+1} ┌──────────┐ x_k ┌─────┐ ▼
u_k ────▶│ H │────▶⊕─────────▶│ delay │─────┬────▶│ C │───⊕────▶ y_k
└─────┘ ▲ └──────────┘ │ └─────┘
│ │
│ ┌─────┐ │
└─────────────│ F │◀──────┘
└─────┘
Since the kalman filter estimates the system state, the measurement equation represents the available measurements (y_k), not compulsorily some other system output that has to be controlled by the controller in a later step.
For a linear model given in continuous form, the following symbols will be used:
system measurement
noise x_init noise
│ │ │
│ │ │
┌─────┐ ▼ ┌──────────┐ ▼ x ┌─────┐ ▼
u ────▶│ B │────▶⊕─────────▶│Integrator│─▶⊕──┬────▶│ C │───⊕────▶ y
└─────┘ ▲ └──────────┘ │ └─────┘
│ │
│ ┌─────┐ │
└─────────────│ A │◀──────┘
└─────┘
Note that A and B from the continuous form can be transformed into F and H from the discrete form. The current kalman filter implementations need the discrete model, but the model in the tests can also be fed with the continuous matrices. These forms will be integrated for calculation:
The conversion from (A, B) to (F, H) for a certain contant timestep dt can be done as follows. See kalmanfilter::systems::continuous_to_discrete
.
The model is given through two functions called f()
and c()
.
system measurement
noise x_init noise
│ │ │
│ ▼ │
▼ x_{k+1} ┌──────────┐ x_k ┌────────┐ ▼
⊕─────────▶│ delay │─────┬────▶│ c(x_k) │───⊕────▶ y_k
▲ └──────────┘ │ └────────┘
│ │
│ ┌───────────────┐ │
└─────│ f(x_k, u_k) │◀────┘
└───────────────┘
▲
u_k ─────┘
The model is given through two functions called f()
and c()
.
system measurement
noise x_init noise
│ │ │
│ ▼ │
▼ ┌────────────┐ x ┌────────┐ ▼
⊕─────────▶│ INTEGRATOR │─────┬────▶│ c(x_t) │───⊕────▶ y_k
▲ └────────────┘ │ └────────┘
│ │
│ ┌───────────────┐ │
└───────│ f(x_t, u_t) │◀────┘
└───────────────┘
▲
u_t ─────┘