Square Root EKF
Square Root Kalman Filter¶
The following square root ekf implementation is based on the following paper: Tracy, K. (2022). "A Square-Root Kalman Filter Using Only QR Decompositions". arXiv preprint arXiv:2208.06452.
The square root EKF works with only triangular square roots of the covariance matrices, resulting in a more numerically robust version of the traditional EKF. By utilizing QR-decompositions the resulting computations can drastically be simplified. For further insights we refer the reader to the paper mentioned above.
Outlier Rejection¶
The outlier rejection allows the EKF to reject measurements that are deemed outliers. This can be enabled for each sensor individually by setting use_outlier_rejection: true for the respective sensor. There are two options of outlier rejection approaches. The user can also set a max_consecutive_outliers value. This will result in a maximum of max_consecutive_outliers measurements being rejected before a measurement is accepted, regardless of whether it is considered an outlier or not. The type of outlier rejection to use can be set using the outlier_rejection_type string to either cov_threshold or chi_squared. If you want to look at the outlier rejection stats, e.g. the threshold, set log_diagnostic_data: true.
Covariance Threshold:¶
A measurement is rejected if the norm of the innovation is greater than some multiple of the innovation covariance matrix. i.e a data point (measurement) \(y_{meas}\) is rejected if
where \(y(x)\) refers to the measurement model prediction and \(th\) is the threshold value that can be set by the user (outlier_threshold) and \(s_{ii}\) are the diagonal entries of the innovation covariance matrix \(S\) computed in the measurement update step of the EKF.
Chi-Squared:¶
The user can define a threshold which corresponds to the percentage of the Gaussian distribution that is accepted. I.e. if the threshold is chosen as 0.9, the outer most 10% of the Gaussian distribution of the measurements are rejected. (We assume the measurements are sampled form a Gaussian distribution).
The Mahalanobis distance is a measure of the distance between a point (here measurement) and a distribution. If the Mahalanobis distance is zero, the measurement corresponds to the mean of the distribution. We compute it as
The threshold is then computed as
\(t = 2\Gamma^{-1}(\frac{m}{2}, p \cdot \gamma(\frac{m}{2}))\)
where \(\Gamma()\) refers to the gamma function and \(\gamma()\) the lower incomplete gamma function. The user chosen threshold (percentage) is given as \(p\) (outlier_threshold).
A measurement is rejected if \(d \geq t\).
Tuning Parameters¶
| name | description |
|---|---|
| Q | process noise covariance matrix |
| P_init | initial state covariance matrix |
Example Configuration
type: "sqrt_ekf"
pub_rate: 50.0 # Hz
log_diagnostic_data: true
initial_state:
type: "pacejka_car"
value: [0, 0, 0, 0.5, 0, 0]
initial_input:
type: "pacejka_car"
value: [0.0, 0.4]
P_init:
[
[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
]
# ============ SENSORS ============
sensors:
sensor_names: ["mocap", "imu", "wheel_encoders"] # Other Options: "lighthouse", "imu_yaw_rate"
mocap:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
key: mocap # optional, if not set use name of sensor
outlier_rejection:
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 5
max_consecutive_outliers: 8
imu:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
key: imu # optional, if not set use name of sensor
outlier_rejection:
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 5
max_consecutive_outliers: 8
wheel_encoders:
R:
value: [[1], [1], [1], [1]]
is_diag: true
key: wheel_encoders # optional, if not set use name of sensor
outlier_rejection:
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 5
max_consecutive_outliers: 8
# ============ MODEL ============
model:
# type describes what type of model we want to use / should be loaded
type: "pacejka_discrete"
Q:
value:
[
[0.01, 0, 0, 0, 0, 0],
[0, 0.01, 0, 0, 0, 0],
[0, 0, 0.01, 0, 0, 0],
[0, 0, 0, 0.0001, 0, 0],
[0, 0, 0, 0, 0.0001, 0],
[0, 0, 0, 0, 0, 0.0001],
]
is_diag: false
# ============ THIS IS ENTIRELY OPTIONAL ============
# If this is NOT defiend, then the default model which is loaded, is loaded from the ros parameters located at /model
# This means that e.g. the EKF uses the same Q as the simulation -> EKF has perfect model and Q
model:
Q:
value:
[
[0.01, 0, 0, 0, 0, 0],
[0, 0.01, 0, 0, 0, 0],
[0, 0, 0.01, 0, 0, 0],
[0, 0, 0, 0.0001, 0, 0],
[0, 0, 0, 0, 0.0001, 0],
[0, 0, 0, 0, 0, 0.0001],
]
is_diag: false
# only change size and inertia of model
model_params:
m: 0.200
I: 0.000605