diff --git a/output_images/1.png b/output_images/1.png new file mode 100644 index 0000000..e3c701a Binary files /dev/null and b/output_images/1.png differ diff --git a/output_images/2.png b/output_images/2.png new file mode 100644 index 0000000..9cd5d73 Binary files /dev/null and b/output_images/2.png differ diff --git a/output_images/3.png b/output_images/3.png new file mode 100644 index 0000000..66d4731 Binary files /dev/null and b/output_images/3.png differ diff --git a/output_images/4.png b/output_images/4.png new file mode 100644 index 0000000..55672f0 Binary files /dev/null and b/output_images/4.png differ diff --git a/src/FusionEKF.cpp b/src/FusionEKF.cpp index 4342e62..4e47223 100644 --- a/src/FusionEKF.cpp +++ b/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,13 +119,31 @@ 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(); /***************************************************************************** @@ -97,18 +151,27 @@ void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) { ****************************************************************************/ /** - 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; -} +} \ No newline at end of file diff --git a/src/FusionEKF.h b/src/FusionEKF.h index 6d40c8a..dd6ab97 100644 --- a/src/FusionEKF.h +++ b/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_ */ \ No newline at end of file diff --git a/src/kalman_filter.cpp b/src/kalman_filter.cpp index eac61cd..8b91839 100644 --- a/src/kalman_filter.cpp +++ b/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_; +} \ No newline at end of file diff --git a/src/kalman_filter.h b/src/kalman_filter.h index ecb29dd..e6dc4e9 100644 --- a/src/kalman_filter.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 81e770e..61e5554 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -175,91 +175,4 @@ int main() { return -1; } h.run(); -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +} \ No newline at end of file diff --git a/src/tools.cpp b/src/tools.cpp index 7d2c781..bd3b77e 100644 --- a/src/tools.cpp +++ b/src/tools.cpp @@ -11,15 +11,58 @@ Tools::~Tools() {} VectorXd Tools::CalculateRMSE(const vector &estimations, const vector &ground_truth) { - /** - TODO: - * Calculate the RMSE here. - */ + // Calculate the RMSE here. + VectorXd rmse(4); + rmse << 0,0,0,0; + + if(estimations.size() != ground_truth.size() || estimations.size() == 0){ + cout << "Invalid estimation / ground_truth data" << endl; + return rmse; + } + + // accumulate squared residuals + for(int i=0; i < estimations.size(); i++){ + VectorXd residual = estimations[i] - ground_truth[i]; + residual = residual.array()*residual.array(); + rmse += residual; + } + + // calculate mean + rmse = rmse/estimations.size(); + + // calculate square-root + rmse = rmse.array().sqrt(); + + return rmse; } MatrixXd Tools::CalculateJacobian(const VectorXd &x_state) { - /** - TODO: - * Calculate a Jacobian here. - */ -} + // Calculate a Jacobian here. + MatrixXd Hj(3,4); + Hj.fill(0.0); + + float px = x_state(0); + float py = x_state(1); + float vx = x_state(2); + float vy = x_state(3); + + //pre-compute a set of terms to avoid repeated calculation + float c1 = px*px+py*py; + float c2 = sqrt(c1); + float c3 = (c1*c2); + + //check division by zero + if(fabs(c1) < 0.0001){ + cout << "CalculateJacobian () - Error - Division by Zero" << endl; + return Hj; + } + + float c4 = vx*py - vy*px; + + //compute the Jacobian matrix + Hj << (px/c2), (py/c2), 0, 0, + -(py/c1), (px/c1), 0, 0, + py*c4/c3, -px*c4/c3, px/c2, py/c2; + + return Hj; +} \ No newline at end of file