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'
}
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();
}