UnscentedKalmanFilter::UnscentedKalmanFilter(SensorModel &sensor_model) :
sensor(sensor_model) {
#if defined(UKF_INTEGRATOR_RK4)
integrator = IntegratorRK4();
#elif defined(UKF_INTEGRATOR_HEUN)
integrator = IntegratorHeun();
#elif defined(UKF_INTEGRATOR_EULER)
integrator = IntegratorEuler();
#endif
state = StateVector::Zero();
state.attitude() << 0, 0, 0, 1;
process_noise_covariance <<
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6,
(real_t)1e-6, (real_t)1e-6, (real_t)1e-6;
state_covariance = StateCovariance::Zero();
state_covariance.diagonal() <<
(real_t)M_PI * (real_t)M_PI * (real_t)0.0625, (real_t)M_PI * (real_t)M_PI * (real_t)0.0625, 1000,
50, 50, 50,
10, 10, 10,
(real_t)M_PI * (real_t)0.25, (real_t)M_PI * (real_t)0.25, (real_t)M_PI * (real_t)0.25,
2, 2, 2,
5, 5, 5,
20, 20, 20,
0, 0, 0;
dynamics = NULL;
}
OptimalControlProblem::OptimalControlProblem(DynamicsModel *d) {
#if defined(NMPC_INTEGRATOR_RK4)
integrator = IntegratorRK4();
#elif defined(NMPC_INTEGRATOR_HEUN)
integrator = IntegratorHeun();
#elif defined(NMPC_INTEGRATOR_EULER)
integrator = IntegratorEuler();
#endif
dynamics = d;
/* Initialise inequality constraints to +/-infinity. */
lower_state_bound = StateConstraintVector::Ones() * -NMPC_INFTY;
upper_state_bound = StateConstraintVector::Ones() * NMPC_INFTY;
lower_control_bound = ControlConstraintVector::Ones() * -NMPC_INFTY;
upper_control_bound = ControlConstraintVector::Ones() * NMPC_INFTY;
/* Initialise weight matrices. */
state_weights = StateWeightMatrix::Identity();
control_weights = ControlWeightMatrix::Identity();
terminal_weights = StateWeightMatrix::Identity();
qp_options = qpDUNES_setupDefaultOptions();
qp_options.maxIter = 5;
qp_options.printLevel = 0;
qp_options.stationarityTolerance = 1e-3;
}