diff --git a/.gitignore b/.gitignore
index 8c7a6e8..69be309 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,5 @@
project4/calibration.pk
model.h5
+term2/project1/cmake-build-debug
+workspace.xml
diff --git a/term2/project1/.idea/misc.xml b/term2/project1/.idea/misc.xml
new file mode 100644
index 0000000..79b3c94
--- /dev/null
+++ b/term2/project1/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/term2/project1/.idea/modules.xml b/term2/project1/.idea/modules.xml
new file mode 100644
index 0000000..6d2bab6
--- /dev/null
+++ b/term2/project1/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/term2/project1/.idea/project1.iml b/term2/project1/.idea/project1.iml
new file mode 100644
index 0000000..f08604b
--- /dev/null
+++ b/term2/project1/.idea/project1.iml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/term2/project1/.idea/vcs.xml b/term2/project1/.idea/vcs.xml
new file mode 100644
index 0000000..b2bdec2
--- /dev/null
+++ b/term2/project1/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/term2/project1/CMakeLists.txt b/term2/project1/CMakeLists.txt
new file mode 100644
index 0000000..3d4a629
--- /dev/null
+++ b/term2/project1/CMakeLists.txt
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.7)
+project(project1)
+
+include_directories(/Users/Shared/SDC/eigen-eigen-67e894c6cd8f/Eigen)
+
+set(CMAKE_CXX_STANDARD 14)
+
+set(SOURCE_FILES main.cpp kalman_filter.cpp kalman_filter.h tracking.cpp tracking.h measurement_package.h)
+add_executable(project1 ${SOURCE_FILES})
\ No newline at end of file
diff --git a/term2/project1/kalman_filter.cpp b/term2/project1/kalman_filter.cpp
new file mode 100644
index 0000000..57c0deb
--- /dev/null
+++ b/term2/project1/kalman_filter.cpp
@@ -0,0 +1,30 @@
+#include "kalman_filter.h"
+
+KalmanFilter::KalmanFilter() {
+}
+
+KalmanFilter::~KalmanFilter() {
+}
+
+void KalmanFilter::Predict() {
+ x_ = F_ * x_;
+ MatrixXd Ft = F_.transpose();
+ P_ = F_ * P_ * Ft + Q_;
+}
+
+void KalmanFilter::Update(const VectorXd &z) {
+ VectorXd z_pred = H_ * x_;
+ VectorXd y = z - z_pred;
+ MatrixXd Ht = H_.transpose();
+ MatrixXd S = H_ * P_ * Ht + R_;
+ MatrixXd Si = S.inverse();
+ MatrixXd PHt = P_ * Ht;
+ MatrixXd K = PHt * Si;
+
+ //new estimate
+ x_ = x_ + (K * y);
+ long x_size = x_.size();
+ MatrixXd I = MatrixXd::Identity(x_size, x_size);
+ P_ = (I - K * H_) * P_;
+}
+
diff --git a/term2/project1/kalman_filter.h b/term2/project1/kalman_filter.h
new file mode 100644
index 0000000..d759236
--- /dev/null
+++ b/term2/project1/kalman_filter.h
@@ -0,0 +1,54 @@
+#ifndef KALMAN_FILTER_H_
+#define KALMAN_FILTER_H_
+#include "Dense"
+
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+class KalmanFilter {
+public:
+
+ ///* state vector
+ VectorXd x_;
+
+ ///* state covariance matrix
+ MatrixXd P_;
+
+ ///* state transition matrix
+ MatrixXd F_;
+
+ ///* process covariance matrix
+ MatrixXd Q_;
+
+ ///* measurement matrix
+ MatrixXd H_;
+
+ ///* measurement covariance matrix
+ MatrixXd R_;
+
+ /**
+ * Constructor
+ */
+ KalmanFilter();
+
+ /**
+ * Destructor
+ */
+ virtual ~KalmanFilter();
+
+ /**
+ * Predict Predicts the state and the state covariance
+ * using the process model
+ */
+ void Predict();
+
+ /**
+ * Updates the state and
+ * @param z The measurement at k+1
+ */
+ void Update(const VectorXd &z);
+
+};
+
+#endif /* KALMAN_FILTER_H_ */
+
diff --git a/term2/project1/main.cpp b/term2/project1/main.cpp
new file mode 100644
index 0000000..a1f1c10
--- /dev/null
+++ b/term2/project1/main.cpp
@@ -0,0 +1,76 @@
+#include
+#include
+#include
+#include
+#include "Dense"
+#include "measurement_package.h"
+#include "tracking.h"
+
+using namespace std;
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+using std::vector;
+
+
+int main() {
+
+ /*******************************************************************************
+ * Set Measurements *
+ *******************************************************************************/
+ vector measurement_pack_list;
+
+ // hardcoded input file with laser and radar measurements
+ string in_file_name_ = "obj_pose-laser-radar-synthetic-input.txt";
+ ifstream in_file(in_file_name_.c_str(),std::ifstream::in);
+
+ if (!in_file.is_open()) {
+ cout << "Cannot open input file: " << in_file_name_ << endl;
+ }
+
+ string line;
+ // set i to get only first 3 measurments
+ int i = 0;
+ while(getline(in_file, line) && (i<=3)){
+
+ MeasurementPackage meas_package;
+
+ istringstream iss(line);
+ string sensor_type;
+ iss >> sensor_type; //reads first element from the current line
+ long timestamp;
+ if(sensor_type.compare("L") == 0){ //laser measurement
+ //read measurements
+ meas_package.sensor_type_ = MeasurementPackage::LASER;
+ meas_package.raw_measurements_ = VectorXd(2);
+ float x;
+ float y;
+ iss >> x;
+ iss >> y;
+ meas_package.raw_measurements_ << x,y;
+ iss >> timestamp;
+ meas_package.timestamp_ = timestamp;
+ measurement_pack_list.push_back(meas_package);
+
+ }else if(sensor_type.compare("R") == 0){
+ //Skip Radar measurements
+ continue;
+ }
+ i++;
+
+ }
+
+ //Create a Tracking instance
+ Tracking tracking;
+
+ //call the ProcessingMeasurement() function for each measurement
+ size_t N = measurement_pack_list.size();
+ for (size_t k = 0; k < N; ++k) { //start filtering from the second frame (the speed is unknown in the first frame)
+ tracking.ProcessMeasurement(measurement_pack_list[k]);
+
+ }
+
+ if(in_file.is_open()){
+ in_file.close();
+ }
+ return 0;
+}
diff --git a/term2/project1/measurement_package.h b/term2/project1/measurement_package.h
new file mode 100644
index 0000000..a5d49d6
--- /dev/null
+++ b/term2/project1/measurement_package.h
@@ -0,0 +1,18 @@
+#ifndef MEASUREMENT_PACKAGE_H_
+#define MEASUREMENT_PACKAGE_H_
+
+#include "Dense"
+
+class MeasurementPackage {
+public:
+ long timestamp_;
+
+ enum SensorType {
+ LASER, RADAR
+ } sensor_type_;
+
+ Eigen::VectorXd raw_measurements_;
+
+};
+
+#endif /* MEASUREMENT_PACKAGE_H_ */
\ No newline at end of file
diff --git a/term2/project1/tracking.cpp b/term2/project1/tracking.cpp
new file mode 100644
index 0000000..d204976
--- /dev/null
+++ b/term2/project1/tracking.cpp
@@ -0,0 +1,99 @@
+#include "Dense"
+#include
+#include "tracking.h"
+
+using namespace std;
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+using std::vector;
+
+Tracking::Tracking() {
+ is_initialized_ = false;
+ previous_timestamp_ = 0;
+
+ //create a 4D state vector, we don't know yet the values of the x state
+ kf_.x_ = VectorXd(4);
+
+ //state covariance matrix P
+ kf_.P_ = MatrixXd(4, 4);
+ kf_.P_ << 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1000, 0,
+ 0, 0, 0, 1000;
+
+
+ //measurement covariance
+ kf_.R_ = MatrixXd(2, 2);
+ kf_.R_ << 0.0225, 0,
+ 0, 0.0225;
+
+ //measurement matrix
+ kf_.H_ = MatrixXd(2, 4);
+ kf_.H_ << 1, 0, 0, 0,
+ 0, 1, 0, 0;
+
+ //the initial transition matrix F_
+ kf_.F_ = MatrixXd(4, 4);
+ kf_.F_ << 1, 0, 1, 0,
+ 0, 1, 0, 1,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1;
+
+ //set the acceleration noise components
+ noise_ax = 5;
+ noise_ay = 5;
+
+}
+
+Tracking::~Tracking() {
+
+}
+
+// Process a single measurement
+void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
+ if (!is_initialized_) {
+ //cout << "Kalman Filter Initialization " << endl;
+
+ //set the state with the initial location and zero velocity
+ kf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
+
+ previous_timestamp_ = measurement_pack.timestamp_;
+ is_initialized_ = true;
+ return;
+ }
+
+ //compute the time elapsed between the current and previous measurements
+ float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0; //dt - expressed in seconds
+ previous_timestamp_ = measurement_pack.timestamp_;
+
+ // TODO: YOUR CODE HERE
+ //1. Modify the F matrix so that the time is integrated
+ kf_.F_ << 1, 0, dt, 0,
+ 0, 1, 0, dt,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1;
+
+ //2. Set the process covariance matrix Q
+ float dt2 = dt * dt;
+ float dt3 = dt2 * dt / 2;
+ float dt4 = dt3 * dt / 2;
+
+ kf_.Q_ = MatrixXd(4, 4);
+ kf_.Q_ << dt4 * noise_ax, 0, dt3 * noise_ax, 0,
+ 0, dt4 * noise_ay, 0, dt3 * noise_ay,
+ dt3 * noise_ax, 0, dt2 * noise_ax, 0,
+ 0, dt3 * noise_ay, 0, dt2 * noise_ay;
+
+ //3. Call the Kalman Filter predict() function
+ kf_.Predict();
+
+
+ //4. Call the Kalman Filter update() function
+ // with the most recent raw measurements_
+ kf_.Update(measurement_pack.raw_measurements_);
+
+ std::cout << "x_= " << kf_.x_ << std::endl;
+ std::cout << "P_= " << kf_.P_ << std::endl;
+
+}
+
diff --git a/term2/project1/tracking.h b/term2/project1/tracking.h
new file mode 100644
index 0000000..9d53327
--- /dev/null
+++ b/term2/project1/tracking.h
@@ -0,0 +1,28 @@
+
+#ifndef TRACKING_H_
+#define TRACKING_H_
+
+#include "measurement_package.h"
+#include
+#include
+#include
+#include "kalman_filter.h"
+
+class Tracking {
+public:
+ Tracking();
+ virtual ~Tracking();
+ void ProcessMeasurement(const MeasurementPackage &measurement_pack);
+ KalmanFilter kf_;
+
+private:
+ bool is_initialized_;
+ long previous_timestamp_;
+
+ //acceleration noise components
+ float noise_ax;
+ float noise_ay;
+
+};
+
+#endif /* TRACKING_H_ */
diff --git a/term2/quiz1.cpp b/term2/quiz1.cpp
new file mode 100644
index 0000000..e69de29