void Pass_wd::matrix_draw() {
bool esc_checker=false;
do {
for(int i=0; i<MATRIX_SIZE; i++) {
srand((unsigned)GetTickCount());
set_color_matrix(MATRIX_COLOR,BLACK); // setting color for matrix "test_clr.cpp"
if(_kbhit() && _getch()==ESC) {
esc_checker=true;
break;
}
if((_kbhit() && _getch()!=ESC) || (m[i].return_pos_y() == MATRIX_DEAD_LINE)) {
system("cls");
matrix_pos_clear();
matrix_pos();
}
m[i].draw_vert();
Sleep(MATRIX_SPEED);
}
} while(esc_checker==false);
delete_color(); // delete color
}
void PredictionModel::reset_model_()
{
nh_.param<double>("refresh_frequency", refresh_frequency_, 100.0);
MatrixWrapper::Matrix matrix_pos(3,3);
matrix_pos(1,1) = 1.0;
matrix_pos(1,2) = 0.0;
matrix_pos(1,3) = 0.0;
matrix_pos(2,1) = 0.0;
matrix_pos(2,2) = 1.0;
matrix_pos(2,3) = 0.0;
matrix_pos(3,1) = 0.0;
matrix_pos(3,2) = 0.0;
matrix_pos(3,3) = 1.0;
MatrixWrapper::Matrix matrix_vel(3,3);
matrix_vel(1,1) = 1.0;
matrix_vel(1,2) = 0.0;
matrix_vel(1,3) = 0.0;
matrix_vel(2,1) = 0.0;
matrix_vel(2,2) = 1.0;
matrix_vel(2,3) = 0.0;
matrix_vel(3,1) = 0.0;
matrix_vel(3,2) = 0.0;
matrix_vel(3,3) = 1.0;
std::vector<MatrixWrapper::Matrix> system_matrices(2);
system_matrices[0] = matrix_pos;
system_matrices[1] = matrix_vel;
MatrixWrapper::ColumnVector sys_noise_mu(3);
sys_noise_mu(1) = 0.0;
sys_noise_mu(2) = 0.0;
sys_noise_mu(3) = 0.0;
// the std dev is dependent on the system
// refresh rate (otherwise, the faster we
// update the system, the more dispersed the
// data would be)
double param;
MatrixWrapper::SymmetricMatrix sys_noise_cov(3);
sys_noise_cov = 0.0;
nh_.param<double>("prediction_model/system/sigma/x", param, 0.2);
sys_noise_cov(1,1) = param / refresh_frequency_;
sys_noise_cov(1,2) = 0.0;
sys_noise_cov(1,3) = 0.0;
sys_noise_cov(2,1) = 0.0;
nh_.param<double>("prediction_model/system/sigma/y", param, 0.2);
sys_noise_cov(2,2) = param / refresh_frequency_;
sys_noise_cov(2,3) = 0.0;
sys_noise_cov(3,1) = 0.0;
sys_noise_cov(3,2) = 0.0;
nh_.param<double>("prediction_model/system/sigma/z", param, 0.2);
sys_noise_cov(3,3) = param / refresh_frequency_;
system_uncertainty_.reset( new BFL::Gaussian(sys_noise_mu, sys_noise_cov) );
system_pdf_.reset(new BFL::LinearAnalyticConditionalGaussian(system_matrices, *system_uncertainty_.get()));
system_model_.reset(new BFL::LinearAnalyticSystemModelGaussianUncertainty(system_pdf_.get()));
//The measurement model and system model are the same in this case as we're getting
// a 3d position for the object.
MatrixWrapper::ColumnVector meas_noise_mu(3);
sys_noise_mu(1) = 0.0;
sys_noise_mu(2) = 0.0;
sys_noise_mu(3) = 0.0;
MatrixWrapper::SymmetricMatrix meas_noise_cov(3);
meas_noise_cov = 0.0;
nh_.param<double>("prediction_model/measurement/sigma/x", param, 0.2);
meas_noise_cov(1,1) = param;
meas_noise_cov(1,2) = 0.0;
meas_noise_cov(1,3) = 0.0;
meas_noise_cov(2,1) = 0.0;
nh_.param<double>("prediction_model/measurement/sigma/y", param, 0.2);
meas_noise_cov(2,2) = param;
meas_noise_cov(2,3) = 0.0;
meas_noise_cov(3,1) = 0.0;
meas_noise_cov(3,2) = 0.0;
nh_.param<double>("prediction_model/measurement/sigma/z", param, 0.2);
meas_noise_cov(3,3) = param;
measurement_uncertainty_.reset( new BFL::Gaussian(meas_noise_mu, meas_noise_cov) );
measurement_pdf_.reset( new BFL::LinearAnalyticConditionalGaussian(matrix_pos, *measurement_uncertainty_.get()) );
measurement_model_.reset(new BFL::LinearAnalyticMeasurementModelGaussianUncertainty(measurement_pdf_.get()));
//Initialising the prior knowledge
// (we don't know where the object is so using a really flat
// centered Gaussian)
MatrixWrapper::ColumnVector prior_mu(3);
nh_.param<double>("prediction_model/prior/mu/x", param, 0.0);
prior_mu(1) = param;
nh_.param<double>("prediction_model/prior/mu/y", param, 0.0);
prior_mu(2) = param;
nh_.param<double>("prediction_model/prior/mu/z", param, 0.0);
prior_mu(3) = param;
MatrixWrapper::SymmetricMatrix prior_cov(3);
nh_.param<double>("prediction_model/prior/sigma/x", param, 50.0);
prior_cov(1,1) = param;
prior_cov(1,2) = 0.0;
prior_cov(1,3) = 0.0;
prior_cov(2,1) = 0.0;
nh_.param<double>("prediction_model/prior/sigma/y", param, 50.0);
prior_cov(2,2) = param;
prior_cov(2,3) = 0.0;
prior_cov(3,1) = 0.0;
prior_cov(3,2) = 0.0;
nh_.param<double>("prediction_model/prior/sigma/z", param, 50.0);
prior_cov(3,3) = param;
prior_.reset(new BFL::Gaussian(prior_mu, prior_cov));
//finally initialising the filter
kalman_filter_.reset(new BFL::ExtendedKalmanFilter(prior_.get()));
}
