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