Moving Horizon Estimator¶
A moving horizon estimation (MHE) approach is an optimization-based estimation scheme that considers the past state estimate, as well as input and measurement data within a window (M). The optimization problem can be stated as
where \(\hat w\), \(\hat v\) refer to process and measurement noise respectively. \(\hat x^*_{t-M}\) can be chosen as either the estimate of an internal estimator, e.g. EKF (\(\hat x^{EKF}\)), or the previous MHE solution \(\hat x_{t-M|t-M}\). The discount factor \(\eta\), determines how large the impact of older measurements is.
The dynamic and measurement models are given as
Available Solvers¶
For more general information on Acados solvers see here Acados.
Creating the Solver¶
Since the MHE is an optimization based estimator, a solver needs to be generated. This is done by running the following line bash src/crs/estimators/mhe_solvers/acados/acados_pacejka_mhe_solver/script/create_solver.sh.
The following parameters need to be set in the config of the solver before generation:
- Horizon length
- Timesteps: 1/freqency at which MHE runs
- Cost Type: Depending on which sensors you are using you can either use a LINEAR_LS or a NONLINEAR_LS
- Sensors: Note that you can compile the solver for sensors and then end up not using them, you can NOT however attempt to use sensors that were not specified in the solver. We therefore recommend just compiling the solver with all available sensors
- Bounds: State and Input bounds
Available sensors for the solver and required solver type:
- mocap: LINEAR_LS
- imu: NONLINEAR_LS
- imu_yaw_rate: LINEAR_LS
- wheel_encoders: NONLINEAR_LS
- lighthouse: NONLINEAR_LS
If you want to change any of these parameters, you need to regenerate the solver!
MHE in CRS¶
The CRS MHE supports the Pacejka model. The MHE will automatically use the same car model as the simulator node <experiments>/model.yaml.
Measurements supported in the MHE are: Motion capture, IMU, IMU Yaw Rate, Wheel Encoders and Lighthouse. If a new measurement is publish, the MHE will add it to the corresponding data buffer, which is then used as the "window of past measurements".
By setting the parameter use_internal_estimator to True the internal estimator estimate is used in the cost, if set to False the previous MHE estimate is used. The type of internal estimator can be set under internal_estimator by setting type.
The MHE has the option of filtering data. Measurements are added to the respective data buffers and then interpolated. The measurements in the data buffers can be filtered if the parameter use_internal_filteris set to True. In addition the MHE can preform outlier rejection for individual sensors, by evaluating if two measurements are too far apart (i.e. over a given threshold) when interpolating. The respective setting can be set for each sensor under MHE_outlier_rejection with use_outlier_rejection and outlier_threshold.
In addition the window size can be set with max_buffer_size and the discount factor \(\eta\) is set with eta.
Tuning Parameters¶
| name | description |
|---|---|
| Q | process noise covariance matrix |
| P | state covariance matrix |
| R_mocap | mocap measurement noise covariance matrix |
| R_imu | imu measurement noise covariance matrix |
| R_imu_yaw_rate | imu_yaw_rate measurement noise covariance matrix |
| R_wheel_encoder | wheel_encoders measurement noise covariance matrix |
| R_lighthouse | lighthouse measurement noise covariance matrix |
| eta | discount factor |
Note that these values appear as the inverse in the cost of the MHE
Example Configuration
type: "mhe"
pub_rate: 100.0 # Hz
use_internal_estimator: false # If true, the internal estimator estimate is used in the cost formulation of the MHE
use_internal_filter: false # If true, the measurements are filtered in the MHE buffer.
internal_filter_type: "median"
# ============ Internal Estimator ============
internal_estimator:
type: "discrete_ekf"
# initial state
initial_state:
type: "pacejka_car"
value: [0.15, -1.05, 0, 0.5, 0, 0]
initial_input:
type: "pacejka_car"
value: [0.0, 0.2]
# initial P (state covariance matrix)
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],
]
# ============ MHE ============
initial_state:
type: "pacejka_car"
value: [0, 0, 0, 0.5, 0, 0]
initial_input:
type: "pacejka_car"
value: [0.0, 0.4]
# Cost for MHE: Note these values appear as the inverse in the cost!
P:
value: [[0.1], [0.1], [0.1], [0.1], [0.1], [0.1]]
is_diag: true
Q:
value: [[0.01], [0.01], [0.01], [0.01], [0.01], [0.01]]
is_diag: true
R_ mocap:
value: [[0.0001], [0.0001], [0.0001]]
is_diag: true
R_imu:
value: [[0.01], [0.01], [0.01]]
is_diag: true
R_imu_yaw_rate:
value: [[0.01]]
is_diag: true
R_wheel_encoders:
value: [[2], [2], [2], [2]]
is_diag: true
R_lighthouse:
value: [[0.0001], [0.0001], [0.0001], [0.0001]]
is_diag: true
solver_type: "ACADOS"
start_delay: 5 # The internal estimator will run for 5s to ensure the data buffer is filled and if needed, an internal estimator estimate is available
max_buffer_size: 600
lag_compensation_time: 0.01
warmstart_iterations: 10
eta: 0.9 # Discount Factor
# ============ SENSORS ============
sensors:
sensor_names: ["mocap", "imu", "wheel_encoders"] # Other options: imu_yaw_rate, lighthouse
mocap:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
key: mocap # optional, if not set use name of sensor
outlier_rejection: # Outlier Rejection for internal estimator
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 3
max_consecutive_outliers: 5
MHE_outlier_rejection:
use_outlier_rejection: true
outlier_threshold: 0.1
imu:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
key: imu # optional, if not set use name of sensor
outlier_rejection: # Outlier Rejection for internal estimator
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 3
max_consecutive_outliers: 5
MHE_outlier_rejection:
use_outlier_rejection: true
outlier_threshold: 0.1
imu_yaw_rate:
R:
value: [[0.01]] # std of 0.1 m/s -> cov of 0.1 * 0.1 = 0.01
is_diag: true
key: imu_yaw_rate # optional, if not set use name of sensor
outlier_rejection: # Outlier Rejection for internal estimator
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 10
max_consecutive_outliers: 8
MHE_outlier_rejection:
use_outlier_rejection: true
outlier_threshold: 0.1
wheel_encoders:
R:
value: [[2], [2], [2], [2]] # std of 1 rad/s -> cov of 1 * 1 = 1
is_diag: true
key: wheel_encoders # optional, if not set use name of sensor
outlier_rejection: # Outlier Rejection for internal estimator
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 5
max_consecutive_outliers: 8
MHE_outlier_rejection:
use_outlier_rejection: true
outlier_threshold: 0.9
lighthouse:
sensor_pos:
value: [[-0.045, -0.06, -0.045, -0.06], [0.015, 0.015, -0.015, -0.015]]
key: lighthouse
base_stations: ["bs0"]
bs0:
bs_ID: 0
R:
value: [[0.000101, 0.0001, 0.0001, 0.0001], [0.0001, 0.000101, 0.0001, 0.0001], [0.0001, 0.0001, 0.000101, 0.0001], [0.0001, 0.0001, 0.0001, 0.000101]]
P_bs:
value: [[0.0809189], [0.620857], [1.8015]]
R_bs:
value: [[0.0433872, 0.998188, 0.0416836], [-0.247378, 0.0511575, -0.967567], [-0.967947, 0.0316685, 0.24915]]
dt1: -0.0508728
dt2: 0.0464478
outlier_rejection: # Outlier Rejection for internal estimator
use_outlier_rejection: true
outlier_rejection_type: "cov_threshold"
outlier_threshold: 3 # threshold is set to 3*covariance
max_consecutive_outliers: 8
MHE_outlier_rejection:
use_outlier_rejection: true
outlier_threshold: 0.1