initialize student code

Author: cameronwp

.gitignore

@@ -3,3 +3,6 @@
 
 # build files
 build/
+
+# editor config
+.vscode/

Docs/Data_Flow_Doc.txt

@@ -0,0 +1,29 @@
+Data Flow:
+1) The measuremennt processor/matlab simulator is generating the FUSION .txt file:
+	"data/obj_pose-laser-radar-synthetic-ukf-input.txt";
+	OR
+	"../matlab_examples/obj_pose-laser-radar-synthetic-ukf-input.txt";
+
+The Input file format is:
+#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
+#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
+
+Example:
+R	8.60363	0.0290616	-2.99903	1477010443399637	8.6	0.25	-3.00029	0
+L	8.45	0.25	1477010443349642	8.45	0.25	-3.00027	0
+	
+2) The EKF Algorithm reads form file reads all the lines and generates measurement structures
+3) The MeasurementProcessor() is called with individual measurements (one by one). The results are saved
+(Attention: no file processing rutines are used inside MeasurementProcessor() all the file processing rutines are in the main function
+So the data read/wirte is decoupled from the algorithm
+4) The results are saved in an output file:
+"data/obj_pose-laser-radar-ekf-output.txt"
+
+Output file format:
+est_px est_py est_vx est_vy meas_px meas_py gt_px gt_py gt_vx gt_vy
+
+Example:
+4.53271	0.279	-0.842172	53.1339	4.29136	0.215312	2.28434	0.226323
+43.2222	2.65959	0.931181	23.2469	4.29136	0.215312	2.28434	0.226323
+
+ 

Docs/Input_Output File Format.txt

@@ -0,0 +1,10 @@
+#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
+#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
+-----------------------------
+Example
+-----------------------------
+R	8.60363	0.0290616	-2.99903	1477010443399637	8.6	0.25	-3.00029	0
+L	8.45	0.25	1477010443349642	8.45	0.25	-3.00027	0 
+
+#Output file format:
+est_px est_py est_vx est_vy meas_px meas_py gt_px gt_py gt_vx gt_vy

README.md

@@ -2,6 +2,7 @@
 Self-Driving Car Engineer Nanodegree Program
 
 ---
+
 ## Dependencies
 
 * cmake >= v3.5
@@ -17,6 +18,19 @@ Self-Driving Car Engineer Nanodegree Program
    some sample inputs in 'data/'.
     - eg. `./ExtendedKF ../data/obj_pose-laser-radar-ekf-input.txt output.txt`
 
+## Editor Settings
+
+We've purposefully kept editor configuration files out of this repo in order to
+keep it as simple and environment agnostic as possible. However, we recommend
+using the following settings:
+
+* indent using spaces
+* set tab width to 2 spaces (keeps the matrices in source code aligned)
+
+## Code Style
+
+Please (do your best to) stick to [Google's C++ style guide](https://google.github.io/styleguide/cppguide.html).
+
 ## Generating Additional Data
 
 This is optional!
@@ -30,3 +44,8 @@ Matlab scripts that can generate additional data.
 This information is only accessible by people who are already enrolled in Term 2
 of CarND. If you are enrolled, see [the project page](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/12dd29d8-2755-4b1b-8e03-e8f16796bea8)
 for instructions and the project rubric.
+
+## Hints!
+
+* You don't have to follow this directory structure, but if you do, your work
+  will span all of the .cpp files here. Keep an eye out for TODOs.

data/sample-laser-radar-measurement-data-1.txt

@@ -0,0 +1,100 @@
+#include "FusionEKF.h"
+#include "tools.h"
+#include "Eigen/Dense"
+#include <iostream>
+
+using namespace std;
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+using std::vector;
+
+/*
+ * Constructor.
+ */
+FusionEKF::FusionEKF() {
+  is_initialized_ = false;
+
+  previous_timestamp_ = 0;
+
+  // initializing matrices
+  R_laser_ = MatrixXd(2, 2);
+  R_radar_ = MatrixXd(3, 3);
+  H_laser_ = MatrixXd(2, 4);
+  Hj_ = MatrixXd(3, 4);
+
+  /**
+  TODO:
+    * Finish initializing the FusionEKF.
+  */
+}
+
+/**
+* Destructor.
+*/
+FusionEKF::~FusionEKF() {}
+
+void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
+  /*****************************************************************************
+   *  Initialization
+   ****************************************************************************/
+  if (!is_initialized_) {
+    /**
+    TODO:
+      * Initialize the state ekf_.x_ with the first measurement.
+      * Create the covariance matrix.
+      * Remember: you'll need to convert radar from polar to cartesian coordinates.
+    */
+    // first measurement
+    cout << "EKF: " << endl;
+    ekf_.x_ = VectorXd(4);
+    ekf_.x_ << 1, 1, 1, 1;
+
+    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
+      /**
+      Convert radar from polar to cartesian coordinates and initialize state.
+      */
+    }
+    else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
+      /**
+      Initialize state.
+      */
+    }
+
+    // done initializing, no need to predict or update
+    is_initialized_ = true;
+    return;
+  }
+
+  /*****************************************************************************
+   *  Prediction
+   ****************************************************************************/
+
+  /**
+   TODO:
+     * Update the state transition matrix F according to the new elapsed time.
+      - Time is measured in seconds.
+     * Update the process noise covariance matrix.
+   */
+
+  ekf_.Predict();
+
+  /*****************************************************************************
+   *  Update
+   ****************************************************************************/
+
+  /**
+   TODO:
+     * Use the sensor type to perform the update step.
+     * Update the state and covariance matrices.
+   */
+
+  if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
+    // Radar updates
+  } else {
+    // Laser updates
+  }
+
+  // print the output
+  cout << "x_ = " << ekf_.x_ << endl;
+  cout << "P_ = " << ekf_.P_ << endl;
+}

data/sample-laser-radar-measurement-data-2.txt

@@ -0,0 +1,48 @@
+#ifndef FusionEKF_H_
+#define FusionEKF_H_
+
+#include "measurement_package.h"
+#include <vector>
+#include <string>
+#include <fstream>
+#include "kalman_filter.h"
+#include "tools.h"
+
+class FusionEKF {
+public:
+  /**
+  * Constructor.
+  */
+  FusionEKF();
+
+  /**
+  * Destructor.
+  */
+  virtual ~FusionEKF();
+
+  /**
+  * Run the whole flow of the Kalman Filter from here.
+  */
+  void ProcessMeasurement(const MeasurementPackage &measurement_pack);
+
+  /**
+  * Kalman Filter update and prediction math lives in here.
+  */
+  KalmanFilter ekf_;
+
+private:
+  // check whether the tracking toolbox was initiallized or not (first measurement)
+  bool is_initialized_;
+
+  // previous timestamp
+  long previous_timestamp_;
+
+  // tool object used to compute Jacobian and RMSE
+  Tools tools;
+  MatrixXd R_laser_;
+  MatrixXd R_radar_;
+  MatrixXd H_laser_;
+  MatrixXd Hj_;
+};
+
+#endif /* FusionEKF_H_ */

src/FusionEKF.cpp

@@ -0,0 +1,19 @@
+#ifndef GROUND_TRUTH_PACKAGE_H_
+#define GROUND_TRUTH_PACKAGE_H_
+
+#include "Eigen/Dense"
+
+class GroundTruthPackage {
+public:
+  long timestamp_;
+
+  enum SensorType{
+    LASER,
+    RADAR
+  } sensor_type_;
+
+  Eigen::VectorXd gt_values_;
+
+};
+
+#endif /* MEASUREMENT_PACKAGE_H_ */

src/FusionEKF.h

@@ -0,0 +1,36 @@
+#include "kalman_filter.h"
+
+KalmanFilter::KalmanFilter() {}
+
+KalmanFilter::~KalmanFilter() {}
+
+void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
+                        MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {
+  x_ = x_in;
+  P_ = P_in;
+  F_ = F_in;
+  H_ = H_in;
+  R_ = R_in;
+  Q_ = Q_in;
+}
+
+void KalmanFilter::Predict() {
+  /**
+  TODO:
+    * predict the state
+  */
+}
+
+void KalmanFilter::Update(const VectorXd &z) {
+  /**
+  TODO:
+    * update the state
+  */
+}
+
+void KalmanFilter::UpdateWithAlreadyPredictedMeasurements(const VectorXd& z, const VectorXd& z_pred) {
+  /**
+  TODO:
+    * update after predicting
+  */
+}

src/ground_truth_package.h

@@ -0,0 +1,72 @@
+#ifndef KALMAN_FILTER_H_
+#define KALMAN_FILTER_H_
+#include "Eigen/Dense"
+
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+class KalmanFilter {
+public:
+
+  // state vector
+  VectorXd x_;
+
+  // state covariance matrix
+  MatrixXd P_;
+
+  // state transistion matrix
+  MatrixXd F_;
+
+  // process covariance matrix
+  MatrixXd Q_;
+
+  // measurement matrix
+  MatrixXd H_;
+
+  // measurement covariance matrix
+  MatrixXd R_;
+
+  /**
+   * Constructor
+   */
+  KalmanFilter();
+
+  /**
+   * Destructor
+   */
+  virtual ~KalmanFilter();
+
+  /**
+   * Init Initializes Kalman filter
+   * @param x_in Initial state
+   * @param P_in Initial state covariance
+   * @param F_in Transition matrix
+   * @param H_in Measurement matrix
+   * @param R_in Measurement covariance matrix
+   * @param Q_in Process covariance matrix
+   */
+  void Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
+      MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in);
+
+  /**
+   * Prediction Predicts the state and the state covariance
+   * using the process model
+   * @param delta_T Time between k and k+1 in s
+   */
+  void Predict();
+
+  /**
+   * Updates the state and
+   * @param z The measurement at k+1
+   */
+  void Update(const VectorXd &z);
+
+  /**
+   * @param z The measurement at k+1
+   * @param z_pred The predicted measurements at k+1
+   */
+  void UpdateWithAlreadyPredictedMeasurements(const VectorXd &z, const VectorXd &z_pred);
+
+};
+
+#endif /* KALMAN_FILTER_H_ */