Extended Kalman Filter¶
Kalman filtering is a way of combining our measurements with knowledge on the car's behavior. In a prediction step, we predict where we expect the car to be in the next time step based on the previous car state, the applied control input, and the physical model of the car. In an update step, we compare the new measurement with our prediction and compute the most likely actual car state. The difference between normal and extended Kalman filtering is that extended Kalman filtering allows nonlinear dynamics, which is crucial in our application.
The CRS EKF supports two different state transition models, the kinematic and the Pacejka model. The EKF will automatically use the same car model as the simulator node <experiments>/model.yaml. However, model discreptancies can be configured using the <experiments>/estimator.yaml. For most applications, it is recommended to use the Pacejka model, since it is a more accurate depiction of reality. For the kinematic modelas well as the Pacejka model an explicit Runge Kutta 4 discretization is used.
Two types of measurements are supported in the EKF. Motion capture and IMU measurements. If a new measurement message is published to ROS, the EKF will do the prior update (using a ZOH of the last received input) and measurement update for either type of measurement in order to estimate a cartesian car state.
The EKF estimator is not only more precise, but it also has a very small delay. Therefore, it should always be preferred over the low pass estimator.
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: "discrete_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