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