void GaussianProcessInterpolation::compute_ldlt() {
// get Omega=W+S/N
IMP_Eigen::MatrixXd WpS = get_Omega();
IMP_LOG_TERSE(" compute_ldlt: Cholesky" << std::endl);
// compute Cholesky decomp
IMP_Eigen::LDLT<IMP_Eigen::MatrixXd, IMP_Eigen::Upper> ldlt;
ldlt.compute(WpS);
if (!ldlt.isPositive())
IMP_THROW("Matrix is not positive semidefinite!", ModelException);
ldlt_ = ldlt;
}
/**
* Checks for "param" command
* return 0 if the command was "param"
*/
int check_for_params(void){
if(veh.server_reply[0] == 'p'){
if (veh.state == VEH_INIT){
// set params
printf("**************************************************\n");
printf("PARAMETERS RECEIVED\n");
printf("SERVER REPLY: %s;;;\n",veh.server_reply);
int parsed;
parsed = sscanf(&veh.server_reply[2], "%f;%f;%f;%f", &veh.rx_delta_left_rad, &veh.rx_delta_right_rad, &veh.rx_omega_left_rad, &veh.rx_omega_right_rad);
printf("Parsed numbers = %d\n", parsed);
printf("PARSED VALUES: %f;%f;%f;%f;\n", veh.rx_delta_left_rad, veh.rx_delta_right_rad, veh.rx_omega_left_rad, veh.rx_omega_right_rad);
printf("**************************************************\n");
// check for values
//if ((veh.delta_left_rad==0) || (veh.delta_right_rad=0) || (veh.omega_left_rad==0) || (veh.omega_right_rad==0)) {
// printf("Received zero value for parameters.\n");
//}
//else {
// values OK
// Left frequency
printf("%s%f\n", "RX O_L: ", veh.rx_omega_left_rad);
printf("SETTING OMEGA LEFT PARAMETER\n");
#if !DEBUG
set_freq_b(LEFT, veh.rx_omega_left_rad, 0);
set_freq_b(LEFT, veh.rx_omega_left_rad, 1);
veh.omega_left_rad = get_Omega(LEFT, 0);
#else
veh.omega_left_rad = veh.rx_omega_left_rad;
#endif
printf("%s%f\n", "TX O_L: ", veh.omega_left_rad);
// Right frequency
printf("%s%f\n", "RX O_R: ", veh.rx_omega_right_rad);
printf("SETTING OMEGA right PARAMETER\n");
#if !DEBUG
set_freq_b(RIGHT, veh.rx_omega_right_rad, 0);
set_freq_b(RIGHT, veh.rx_omega_right_rad, 1);
veh.omega_right_rad = get_Omega(RIGHT, 0);
#else
veh.omega_right_rad = veh.rx_omega_right_rad;
#endif
printf("%s%f\n", "TX O_R: ", veh.omega_right_rad);
// Left delta
printf("%s%f\n", "RX D_L: ", veh.rx_delta_left_rad);
printf("SETTING DELTA LEFT PARAMETER\n");
#if !DEBUG
set_delta_b(LEFT, veh.rx_delta_left_rad, 0);
set_delta_b(LEFT, veh.rx_delta_left_rad, 1);
veh.delta_left_rad = get_Delta(LEFT, 0);
#else
veh.delta_left_rad = veh.rx_delta_left_rad;
#endif
printf("%s%f\n", "TX D_L: ", veh.delta_left_rad);
// Right delta
printf("%s%f\n", "RX D_R: ", veh.rx_delta_right_rad);
printf("SETTING DELTA RIGHT PARAMETER\n");
#if !DEBUG
set_delta_b(RIGHT, (double)veh.rx_delta_right_rad, 0);
set_delta_b(RIGHT, (double)veh.rx_delta_right_rad, 1);
veh.delta_right_rad = get_Delta(RIGHT, 0);
#else
veh.delta_right_rad = veh.rx_delta_right_rad;
#endif
printf("%s%f\n", "TX D_R: ", veh.delta_right_rad);
//}
}
else {
// vehicle not running
printf("**************************************************\n");
printf("Not ready (stopped or not initialized.\n");
printf("**************************************************\n");
}
// prepare response
sprintf(veh.message, "p;%f;%f;%f;%f;\n", veh.delta_left_rad, veh.delta_right_rad, veh.omega_left_rad, veh.omega_right_rad);
// command found
return 0;
}
else {
// continue search
return 1;
}
}
