Пример #1
0
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;
}
Пример #2
0
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;
}