virtual void operate() {
//// pi = boost::math::constants::pi<double>();
//
inputjpjv.resize(DOF * 2, 1); /*!Has to be scaled 10 times*/
inputjpjv[0] = 10 * this->jpInputActual.getValue()[0];
inputjpjv[1] = 10 * this->jpInputActual.getValue()[1];
inputjpjv[2] = 10 * this->jpInputActual.getValue()[2];
inputjpjv[3] = 10 * this->jpInputActual.getValue()[3];
inputjpjv[4] = 10 * this->jvInputActual.getValue()[0];
inputjpjv[5] = 10 * this->jvInputActual.getValue()[1];
inputjpjv[6] = 10 * this->jvInputActual.getValue()[2];
inputjpjv[7] = 10 * this->jvInputActual.getValue()[3];
//
// //-----------------------------------SECTION ONE-----------------------------------------------------//
//
int num_priors = priors.rows(); // priors comes as a rows
int num_inp = 8; //inputjpjv.rows(); ----------------------------------SHOULDNOT DO THIS ------------//
int k = 0;
p.resize(num_priors, 1);
h.resize(num_priors, 1);
long double prob_input = 0;
// //-------------------------------SECTION TWO---------------------------------------------------//
//
for (k = 0; k < num_priors; k++) {
double coeff = pow((2 * 3.14), (inputjpjv.rows()) / 2) // replace pi by 3.14
* det_tmp_tmp[k];
double exponential_part = (-0.5
* (((inputjpjv - input_mean_matrix.col(k)).transpose())
* (tmp_inv.block(num_inp * k, 0, num_inp, num_inp)))
* (inputjpjv - input_mean_matrix.col(k)))(0, 0);
double result = (1 / coeff) * exp(exponential_part);
double prob_input_tmp = result;/*gausspdf(inputjpjv,*/
double tmp_Pk = priors(k) * prob_input_tmp;
p(k) = tmp_Pk;
prob_input = prob_input + tmp_Pk;
}
//
// //SUB
//
// p[0] = 0.14;
// p[1] = 0.14;
// p[2] = 0.14;
// p[3] = 0.14;
// p[4] = 0.14;
// p[5] = 0.14;
// p[6] = 0.16;
// prob_input = 1.0;
//
// //-------------------------------SECTION THREE---------------------------------------------------//
//
for (k = 0; k < num_priors; k++) {
double h_k = p(k) / prob_input;
h(k) = h_k;
}
//
// h[0] = 0.14;
// h[1] = 0.14;
// h[2] = 0.14;
// h[3] = 0.14;
// h[4] = 0.14;
// h[5] = 0.14;
// h[6] = 0.16;
// //-------------------------------SECTION FOUR---------------------------------------------------//
//
f_hat.resize(num_op, 1);
f_hat.fill(0.0);
for (k = 0; k < num_priors; k++) {
f_hat = f_hat
+ h(k)
* (A_matrix.block(num_op * k, 0, num_op, num_inp)
* inputjpjv + B_matrix.col(k));
}
// f_hat.resize(num_op, 1);
//
// f_hat[0] = 0.5;
// f_hat[1] = 0.5;
// f_hat[2] = 0.5;
// f_hat[3] = 0.5;
//
// //-------------------------------SECTION FIVE---------------------------------------------------//
//
tmp_jt[0] = f_hat[0];
tmp_jt[1] = f_hat[1];
tmp_jt[2] = f_hat[2];
tmp_jt[3] = f_hat[3];
// tmp_jt[0] = 0;
// tmp_jt[1] = 0;
// tmp_jt[2] = 0;
// tmp_jt[3] = 0;
this->error_outputValue->setData(&tmp_jt);
}
