cpp quizes

.gitignore

@@ -3,3 +3,5 @@
 
 project4/calibration.pk
 model.h5
+term2/project1/cmake-build-debug
+workspace.xml

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

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

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_ */
+

term2/project1/main.cpp

@@ -0,0 +1,76 @@
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <vector>
+#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<MeasurementPackage> 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;
+}

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_ */

term2/project1/tracking.cpp

@@ -0,0 +1,99 @@
+#include "Dense"
+#include <iostream>
+#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;
+
+}
+

term2/project1/tracking.h

@@ -0,0 +1,28 @@
+
+#ifndef TRACKING_H_
+#define TRACKING_H_
+
+#include "measurement_package.h"
+#include <vector>
+#include <string>
+#include <fstream>
+#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_ */