mad-location-manager-lib 1.0
Library for fusing GPS and ENU accelerometer data
|
Template-based Kalman filter implementation. More...
#include <kalman.h>
Protected Member Functions | |
kalman_filter () | |
void | estimate () |
void | correct () |
Protected Attributes | |
mtx_t< double, state_d, state_d > | F |
mtx_t< double, measure_d, state_d > | H |
state transition model | |
mtx_t< double, state_d, control_d > | B |
observation model | |
mtx_t< double, state_d, state_d > | Q |
control matrix | |
mtx_t< double, measure_d, measure_d > | R |
process noise covariance | |
mtx_t< double, control_d, 1 > | Uk |
observation noise covariance | |
mtx_t< double, measure_d, 1 > | Zk |
control vector | |
mtx_t< double, state_d, 1 > | Xk_km1 |
actual measured values vector | |
mtx_t< double, state_d, state_d > | Pk_km1 |
predicted state estimate | |
mtx_t< double, measure_d, 1 > | Yk |
predicted estimate covariance | |
mtx_t< double, measure_d, measure_d > | Sk |
measurement innovation | |
mtx_t< double, state_d, measure_d > | K |
innovation covariance | |
mtx_t< double, state_d, 1 > | Xk_k |
Kalman gain. | |
mtx_t< double, measure_d, 1 > | Yk_k |
updated (current) state | |
mtx_t< double, state_d, state_d > | Pk_k |
post fit residual | |
mtx_t< double, state_d, state_d > | I |
updated estimate covariance | |
Template-based Kalman filter implementation.
state_d | Dimension of state vector (number of state variables) |
measure_d | Dimension of measurement vector (number of measurements) |
control_d | Dimension of control vector (number of control inputs) |
Generic Kalman filter for linear state estimation using Eigen matrices. Implements standard predict-correct cycle with configurable dimensions.
|
inlineprotected |
|
inlineprotected |
|
inlineprotected |
observation model
state transition model
updated estimate covariance
innovation covariance
post fit residual
predicted state estimate
control matrix
process noise covariance
measurement innovation
observation noise covariance
Kalman gain.
actual measured values vector
predicted estimate covariance
updated (current) state
control vector