Skip to content

Acados MHE Solver

The acados implementation of the pacejka MHE solver uses the Pacejka Model and the Acados Solver to solve the nonlinear optimization problem.

For each stage of the mhe formulation, the following parameters need to be provided.

mhe_costs

  • P State Covariance Matrix : Punishes the error in the state estimate
  • Q Process Noise Covariance Matrix : Punishes the error in the dynamic model
  • R_vicon Measurement Noise Covariance Matrix : Punishes the error in the measurement model
  • R_imu Measurement Noise Covariance Matrix : Punishes the error in the measurement model
  • R_imu_yaw_rate Measurement Noise Covariance Matrix : Punishes the error in the measurement model
  • R_wheel_encoders Measurement Noise Covariance Matrix: Punishes the error in the measurement model
  • R_lighthouse Measurement Noise Covariance Matrix: Punishes the error in the measurement model
  • eta -Discount Factor_: Large values lead to lower impact of older measurements

references

  • state Reference state: Either last MHE estimate or internal estimator estimate
  • input Applied input
  • vicon_measurement Measurement of vicon sensor
  • imu_measurement Measurement of imu sensor
  • imu_yaw_rate_measurement Measurement of yaw rate of imu sensor
  • wheel_encoder_measurement Measurement of wheel encoder sensor
  • lighthosue_measurement Measurement of lighthouse sensor
  • valid_vicon Bool determining if vicon measurement can be used
  • valid_imu Bool determining if imu measurement can be used
  • valid_imu_yaw_rate Bool determining if yaw rate of imu measurement can be used
  • valid_wheel_encoders Bool determining if wheel encoder measurement can be used
  • valid_lighthouse Bool determining if lighthouse measurement can be used

Example Code

The following code illustrates the usage of the solver interface

// Last solution is a struct containing arrays states_, inputs_ and trajectory_estimate_
// Run solver
mhe_solvers::pacejka_solvers::cost_values mhe_costs; // Define cost
for (int current_stage = 0; current_stage < solver_->getHorizonLength(); current_stage++)
{
   // next stage points to current_stage + 1. If current_stage is at end of horizon, next stage directy points to
   // current stage i.e. current_stage = 2 -> next_stage = 3, current_stage = 29 -> next_stage = 29, assuming
   // horizon of 30
   int next_stage = current_stage + (current_stage != solver_->getHorizonLength() - 1);
   // Define reference at this stage
   mhe_solvers::pacejka_solvers::references references; 
   // ...
   solver_->updateParams(current_stage,
                         discrete_model->getParams(),  // Model Dynamics
                         mhe_costs,                    // Costs
                         references,                   // Tracking point
                         lighthouse_params_1,          // Lighthouse sweep 1 params
                         lighthouse_params_2);         // Lighthouse sweep 2 params
   );

   // Use previous solution as initial guess this time
   solver_->setStateInitialGuess(next_stage, &last_solution.states_[current_stage * solver_->getStateDimension()]);
}
solver_->solve(&last_solution.states_[0], &last_solution.inputs_[0]);

Building the C-Code

The C code needs to be manually generated by running:

bash src/crs/estimators/mhe_solvers/acados/acados_pacejka_mhe_solver/script/create_
solver.sh