diff --git a/src/main.cpp b/src/main.cpp index d9504b22..49f5e624 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -142,62 +142,67 @@ int main(int argc, char* argv[]) { size_t number_of_measurements = measurement_pack_list.size(); // column names for output file - out_file_ << "px" << "\t"; - out_file_ << "py" << "\t"; - out_file_ << "v" << "\t"; - out_file_ << "yaw_angle" << "\t"; - out_file_ << "yaw_rate" << "\t"; + out_file_ << "time_stamp" << "\t"; + out_file_ << "px_state" << "\t"; + out_file_ << "py_state" << "\t"; + out_file_ << "v_state" << "\t"; + out_file_ << "yaw_angle_state" << "\t"; + out_file_ << "yaw_rate_state" << "\t"; + out_file_ << "sensor_type" << "\t"; + out_file_ << "NIS" << "\t"; out_file_ << "px_measured" << "\t"; out_file_ << "py_measured" << "\t"; - out_file_ << "px_true" << "\t"; - out_file_ << "py_true" << "\t"; - out_file_ << "vx_true" << "\t"; - out_file_ << "vy_true" << "\t"; - out_file_ << "NIS" << "\n"; + out_file_ << "px_ground_truth" << "\t"; + out_file_ << "py_ground_truth" << "\t"; + out_file_ << "vx_ground_truth" << "\t"; + out_file_ << "vy_ground_truth" << "\n"; for (size_t k = 0; k < number_of_measurements; ++k) { // Call the UKF-based fusion ukf.ProcessMeasurement(measurement_pack_list[k]); - // output the estimation + // timestamp + out_file_ << measurement_pack_list[k].timestamp_ << "\t"; // pos1 - est + + // output the state vector out_file_ << ukf.x_(0) << "\t"; // pos1 - est out_file_ << ukf.x_(1) << "\t"; // pos2 - est out_file_ << ukf.x_(2) << "\t"; // vel_abs -est out_file_ << ukf.x_(3) << "\t"; // yaw_angle -est out_file_ << ukf.x_(4) << "\t"; // yaw_rate -est - // output the measurements + // output lidar and radar specific data if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::LASER) { - // output the estimation + // sensor type + out_file_ << "lidar" << "\t"; - // p1 - meas - out_file_ << measurement_pack_list[k].raw_measurements_(0) << "\t"; + // NIS value + out_file_ << ukf.NIS_laser_ << "\t"; - // p2 - meas + // output the lidar sensor measurement px and py + out_file_ << measurement_pack_list[k].raw_measurements_(0) << "\t"; out_file_ << measurement_pack_list[k].raw_measurements_(1) << "\t"; + } else if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::RADAR) { - // output the estimation in the cartesian coordinates + // sensor type + out_file_ << "radar" << "\t"; + + // NIS value + out_file_ << ukf.NIS_radar_ << "\t"; + + // output radar measurement in cartesian coordinates float ro = measurement_pack_list[k].raw_measurements_(0); float phi = measurement_pack_list[k].raw_measurements_(1); - out_file_ << ro * cos(phi) << "\t"; // p1_meas - out_file_ << ro * sin(phi) << "\t"; // p2_meas + out_file_ << ro * cos(phi) << "\t"; // px measurement + out_file_ << ro * sin(phi) << "\t"; // py measurement } - // output the ground truth packages + // output the ground truth out_file_ << gt_pack_list[k].gt_values_(0) << "\t"; out_file_ << gt_pack_list[k].gt_values_(1) << "\t"; out_file_ << gt_pack_list[k].gt_values_(2) << "\t"; - out_file_ << gt_pack_list[k].gt_values_(3) << "\t"; - - // output the NIS values - - if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::LASER) { - out_file_ << ukf.NIS_laser_ << "\n"; - } else if (measurement_pack_list[k].sensor_type_ == MeasurementPackage::RADAR) { - out_file_ << ukf.NIS_radar_ << "\n"; - } - + out_file_ << gt_pack_list[k].gt_values_(3) << "\n"; // convert ukf x vector to cartesian to compare to ground truth VectorXd ukf_x_cartesian_ = VectorXd(4); @@ -216,7 +221,7 @@ int main(int argc, char* argv[]) { // compute the accuracy (RMSE) Tools tools; - cout << "Accuracy - RMSE:" << endl << tools.CalculateRMSE(estimations, ground_truth) << endl; + cout << "RMSE" << endl << tools.CalculateRMSE(estimations, ground_truth) << endl; // close files if (out_file_.is_open()) {