Added code for Kalman Filter (Lidar and Radar) and RMSE (error).

src/FusionEKF.cpp

@@ -31,13 +31,38 @@ FusionEKF::FusionEKF() {
             0, 0.0009, 0,
             0, 0, 0.09;
 
-    /**
-    TODO:
-      * Finish initializing the FusionEKF.
-      * Set the process and measurement noises
-    */
-
-
+    /*
+     * Initialize the FusionEKF.
+     * Set the process and measurement noises
+     */
+    // measurement function matrix - laser
+    H_laser_ << 1, 0, 0, 0,
+                0, 1, 0, 0;
+
+    //state covariance matrix: low values for high certainity
+    MatrixXd P_ = MatrixXd(4, 4);
+    P_ << 1, 0, 0, 0,
+          0, 1, 0, 0,
+          0, 0, 1000, 0,
+          0, 0, 0, 1000;
+
+    // state transition matrix
+    MatrixXd F_ = MatrixXd(4, 4);
+    F_ << 1, 0, 1, 0,
+          0, 1, 0, 1,
+          0, 0, 1, 0,
+          0, 0, 0, 1;
+
+    // new covariance matrix based on noise vector
+    MatrixXd Q_ = MatrixXd(4, 4);
+    Q_ << 1, 0, 1, 0,
+          0, 1, 0, 1,
+          1, 0, 1, 0,
+          0, 1, 0, 1;
+
+    VectorXd x_ = VectorXd(4);
+    x_ << 1, 1, 1, 1;
+    ekf_.Init(x_, P_, F_, H_laser_, R_laser_, Q_);
 }
 
 /**
@@ -53,7 +78,6 @@ void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
      ****************************************************************************/
     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.
@@ -63,16 +87,28 @@ void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
         ekf_.x_ = VectorXd(4);
         ekf_.x_ << 1, 1, 1, 1;
 
+        // initialize position and velocity
+        float px = 0.0;
+        float py = 0.0;
+        float vx = 0.0;
+        float vy = 0.0;
+
         if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
-            /**
-            Convert radar from polar to cartesian coordinates and initialize state.
-            */
+            // Convert radar from polar to cartesian coordinates and initialize state.
+            float rho = measurement_pack.raw_measurements_[0];
+            float phi = measurement_pack.raw_measurements_[1];
+            float rho_dot = measurement_pack.raw_measurements_[2];
+
+            px = rho * cos(phi);
+            py = rho * sin(phi);
         } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
-            /**
-            Initialize state.
-            */
+            px = measurement_pack.raw_measurements_[0];
+            py = measurement_pack.raw_measurements_[1];
         }
 
+        ekf_.x_ << px, py, vx, vy;
+        previous_timestamp_ = measurement_pack.timestamp_;
+
         // done initializing, no need to predict or update
         is_initialized_ = true;
         return;
@@ -83,32 +119,59 @@ void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
      ****************************************************************************/
 
     /**
-     TODO:
        * Update the state transition matrix F according to the new elapsed time.
         - Time is measured in seconds.
        * Update the process noise covariance matrix.
        * Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
      */
 
+    // calculating time elapsed
+    float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
+    previous_timestamp_ = measurement_pack.timestamp_;
+
+    // Update the state transition matrix F according to the new elapsed time
+    ekf_.F_(0, 2) = dt;
+    ekf_.F_(1, 3) = dt;
+
+    float dt_2 = dt * dt;
+    float dt_3 = dt_2 * dt / 2.0;
+    float dt_4 = dt_3 * dt / 2.0;
+
+    //set the process covariance matrix Q
+    ekf_.Q_ = MatrixXd(4, 4);
+    ekf_.Q_ << dt_4 * noise_ax, 0, dt_3 * noise_ax, 0,
+               0, dt_4 * noise_ay, 0, dt_3 * noise_ay,
+               dt_3 * noise_ax, 0, dt_2 * noise_ax, 0,
+               0, dt_3 * noise_ay, 0, dt_2 * noise_ay;
+
     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
+        try {
+            ekf_.R_ = R_radar_;
+            ekf_.H_ = tools.CalculateJacobian(ekf_.x_);
+            ekf_.UpdateEKF(measurement_pack.raw_measurements_);
+        } catch (...) {
+            return;
+        }
     } else {
         // Laser updates
+        ekf_.R_ = R_laser_;
+        ekf_.H_ = H_laser_;
+        ekf_.Update(measurement_pack.raw_measurements_);
     }
 
     // print the output
     cout << "x_ = " << ekf_.x_ << endl;
     cout << "P_ = " << ekf_.P_ << endl;
-}
+}

src/FusionEKF.h

@@ -44,6 +44,10 @@ class FusionEKF {
     Eigen::MatrixXd R_radar_;
     Eigen::MatrixXd H_laser_;
     Eigen::MatrixXd Hj_;
+
+    // Noise parameters as suggested.
+    float noise_ax = 9.0;
+    float noise_ay = 9.0;
 };
 
-#endif /* FusionEKF_H_ */
+#endif /* FusionEKF_H_ */

src/kalman_filter.cpp

@@ -18,22 +18,61 @@ void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
 }
 
 void KalmanFilter::Predict() {
-    /**
-    TODO:
-      * predict the state
-    */
+    // predict the state
+    x_ = F_ * x_;
+    P_ = F_ * P_ * F_.transpose() + Q_;
 }
 
 void KalmanFilter::Update(const VectorXd &z) {
-    /**
-    TODO:
-      * update the state by using Kalman Filter equations
-    */
+    // update the state by using Kalman Filter equations
+
+    // measurement update
+    VectorXd z_pred = H_ * x_;
+    VectorXd y = z - z_pred;
+    MatrixXd PHt = P_ * H_.transpose();
+    MatrixXd S = H_ * PHt + R_;
+    MatrixXd K = PHt * S.inverse();
+
+    //new state
+    x_ = x_ + (K * y);
+    int x_size = x_.size();
+    MatrixXd I = MatrixXd::Identity(x_size, x_size);
+    P_ = (I - K * H_) * P_;
 }
 
 void KalmanFilter::UpdateEKF(const VectorXd &z) {
-    /**
-    TODO:
-      * update the state by using Extended Kalman Filter equations
-    */
-}
+    // update the state by using Extended Kalman Filter equations
+
+    // get components of predicted state
+    float px = x_(0);
+    float py = x_(1);
+    float vx = x_(2);
+    float vy = x_(3);
+
+    // get components of radar measurement space
+    float rho = sqrt(px * px + py * py);
+    float phi = atan2(py, px);
+    float rho_dot = (rho != 0 ? (px * vx + py * vy) / rho : 0);
+
+    // define predicted position and speed
+    VectorXd z_pred(3);
+    z_pred << rho, phi, rho_dot;
+
+    // measurement update
+    VectorXd y = z - z_pred;
+
+    // normalize angle
+    double width = 2 * M_PI;   //
+    double offsetValue = y(1) + M_PI;   // value relative to 0
+    y(1) = (offsetValue - (floor(offsetValue / width) * width)) - M_PI;
+
+    MatrixXd PHt = P_ * H_.transpose();
+    MatrixXd S = H_ * PHt + R_;
+    MatrixXd K = PHt * S.inverse();
+
+    // new state
+    x_ = x_ + (K * y);
+    int x_size = x_.size();
+    MatrixXd I = MatrixXd::Identity(x_size, x_size);
+    P_ = (I - K * H_) * P_;
+}

src/kalman_filter.h

@@ -12,7 +12,7 @@ class KalmanFilter {
     // state covariance matrix
     Eigen::MatrixXd P_;
 
-    // state transition matrix
+    // state transistion matrix
     Eigen::MatrixXd F_;
 
     // process covariance matrix

src/main.cpp

@@ -175,91 +175,4 @@ int main() {
         return -1;
     }
     h.run();
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+}