Merge pull request udacity#8 from andrewpaster/master

main.cpp
scherererer · Apr 18, 2017 · cb395e0 · cb395e0
2 parents 33cb7a5 + b94b7dd
commit cb395e0
Showing 1 changed file with 36 additions and 31 deletions.
diff --git 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()) {