예제 #1
0
TestPosition::TestPosition(std::string position_sink_name) :
  position_sink(position_sink_name)
, accel_distribution(0.0, 5.0)
, state(6,1,CV_32F)
, accel_vec(3,1,CV_32F)
, state_transition_mat(6, 6, CV_32F)
, input_mat(6, 3, CV_32F) {

    position_sink.createSharedObject();
    createStaticMatracies();
    
    // Initial condition
    state.at<float>(0) = 0.0; // x
    state.at<float>(1) = 0.0; // x'
    state.at<float>(2) = 0.0; // y
    state.at<float>(3) = 0.0; // y'
    state.at<float>(4) = 0.0; // z
    state.at<float>(5) = 0.0; // z'

}
예제 #2
0
void RandomAccel2D::configure(const po::variables_map &vm)
{
    // Check for config file and entry correctness
    auto config_table = oat::config::getConfigTable(vm);
    oat::config::checkKeys(config_keys_, config_table);
    
    // Rate
    double fs = 1e8; // Very fast s.t. process cannot keep up
    if (oat::config::getNumericValue<double>(
                vm, config_table, "rate", fs, 0)) {
        enforce_sample_clock_ = true;
    } else {
        tick_ = clock_.now();
    }
    generateSamplePeriod(fs);

    // Rate
    oat::config::getNumericValue<uint64_t>(
        vm, config_table, "num-samples", num_samples_, 0);

    // Room
    std::vector<double> r;
    if (oat::config::getArray<double, 4>(vm, config_table, "room", r)) {
        room_.x = r[0];
        room_.y = r[1];
        room_.width = r[2];
        room_.height = r[3];
    }

    // Acceleration
    double a;
    if (oat::config::getNumericValue<double>(vm, config_table, "sigma-accel", a)) {
        accel_distribution_.param(
            std::normal_distribution<double>::param_type(0, a));
    }

    // Configure STM
    createStaticMatracies();
}