From cfcfecafa3c32e26185707d10842855a41c88b28 Mon Sep 17 00:00:00 2001 From: "Michael P. Scherer" Date: Thu, 13 Jul 2017 21:13:52 -0500 Subject: [PATCH] Functional code, needs tuning to achieve targets - Resolved wrapping issues - Brought down initial values for P_ of 1000 which were too large - Implemented LIDAR update step --- src/tools.cpp | 2 +- src/ukf.cpp | 171 +++++++++++++++++++++++++++++++++++--------------- src/ukf.h | 3 + 3 files changed, 124 insertions(+), 52 deletions(-) diff --git a/src/tools.cpp b/src/tools.cpp index 7601f15b..3435e1c4 100644 --- a/src/tools.cpp +++ b/src/tools.cpp @@ -24,7 +24,7 @@ VectorXd Tools::CalculateRMSE(vector const &estimations, * Calculate the RMSE here. */ - rmse_ << 0,0,0,0; + rmse_.fill (0.0); if (estimations.empty () || ground_truth.empty ()) { diff --git a/src/ukf.cpp b/src/ukf.cpp index 5dfe259e..e9121c67 100755 --- a/src/ukf.cpp +++ b/src/ukf.cpp @@ -11,7 +11,7 @@ using namespace std; namespace { -float const EPSILON = 0.001f; +float constexpr EPSILON = 0.001f; inline float wrap (float a_) { @@ -21,6 +21,19 @@ inline float wrap (float a_) return a_ - M_PI; } +bool detectNan (string const &prefix_, MatrixXd const &A) +{ + bool hasnan = false; + for (unsigned i = 0; i < A.rows (); ++i) + for (unsigned j = 0; j < A.cols (); ++j) + if (isnan (A(i,j))) + { + cerr << prefix_ << " NAN at index (" << i << "," << j << ")\n"; + hasnan = true; + } + return hasnan; +} + } /** @@ -29,7 +42,7 @@ inline float wrap (float a_) UKF::UKF() : is_initialized_(false), // if this is false, laser measurements will be ignored (except during init) - use_laser_(false), + use_laser_(true), // if this is false, radar measurements will be ignored (except during init) use_radar_(true), n_x_(5), @@ -45,10 +58,10 @@ UKF::UKF() : time_us_(0), // Process noise standard deviation longitudinal acceleration in m/s^2 //std_a_(30), ///< TODO This looks wildly off .... - std_a_(10), ///< TODO This is a random guess... + std_a_(30), ///< TODO This is a random guess... // Process noise standard deviation yaw acceleration in rad/s^2 //std_yawdd_(30), - std_yawdd_(30), + std_yawdd_(1), // Laser measurement noise standard deviation position1 in m std_laspx_(0.15), // Laser measurement noise standard deviation position2 in m @@ -101,8 +114,6 @@ void UKF::ProcessMeasurement(MeasurementPackage const &meas_package) if (! use_radar_ && meas_package.sensor_type_ == MeasurementPackage::RADAR) return; - cout << meas_package.timestamp_ / 1000000.0 << " Processing Measurement..." << endl; - double const dt = (meas_package.timestamp_ - time_us_) / 1000000.0; time_us_ = meas_package.timestamp_; @@ -117,7 +128,7 @@ void UKF::ProcessMeasurement(MeasurementPackage const &meas_package) if (dt > 0.0) Prediction (dt); else - cout << "Skipping prediction due to 0s time difference" << endl; + cerr << "Skipping prediction due to 0s time difference\n"; switch (meas_package.sensor_type_) { @@ -136,9 +147,9 @@ void UKF::Initialize (MeasurementPackage const &meas_package) P_.fill(0.0f); P_(0,0) = 1; // x P_(1,1) = 1; // y - P_(2,2) = 1000; // vel - P_(3,3) = 1000; // yaw - P_(4,4) = 1000; // yaw rate + P_(2,2) = 100; // vel + P_(3,3) = 100; // yaw + P_(4,4) = 100; // yaw rate switch (meas_package.sensor_type_) { @@ -184,19 +195,13 @@ void UKF::Initialize (MeasurementPackage const &meas_package) */ void UKF::Prediction(double delta_t) { - assert (delta_t > 0.0); + assert(delta_t > 0.0); - /** - TODO: - - Complete this function! Estimate the object's location. Modify the state - vector, x_. Predict sigma points, the state, and the state covariance matrix. - */ - - /// \todo Generate augmented sigma points //create augmented mean state VectorXd x_aug = VectorXd::Zero(7); + assert(std::abs (x_(3)) <= M_PI); + x_aug.head(5) = x_; x_aug(5) = 0.0f; x_aug(6) = 0.0f; @@ -211,16 +216,12 @@ void UKF::Prediction(double delta_t) //create augmented sigma points //calculate square root of P - /// \todo NaN risk here? MatrixXd const A = P_aug.llt().matrixL(); - for (unsigned i = 0; i < A.rows (); ++i) - for (unsigned j = 0; j < A.cols (); ++j) - if (isnan (A(i,j))) - cerr << "NAN NAN NAN" << endl; + assert(! detectNan ("A", A)); float const scale = sqrt(lambda_ + n_aug_); MatrixXd const sA = scale * A; - MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1); + MatrixXd Xsig_aug = MatrixXd::Zero(n_aug_, 2 * n_aug_ + 1); Xsig_aug.col (0) = x_aug; @@ -230,6 +231,8 @@ void UKF::Prediction(double delta_t) Xsig_aug.col (i + 1 + n_aug_) = x_aug - sA.col (i); } + assert(! detectNan ("Xsig_aug", Xsig_aug)); + //predict sigma points //avoid division by zero //write predicted sigma points into right column @@ -240,7 +243,7 @@ void UKF::Prediction(double delta_t) float const px = x(0); float const py = x(1); float const v = x(2); - float const phi = x(3); + float const phi = wrap (x(3)); float const phid = x(4); float const va = x(5); float const vphidd = x(6); @@ -249,11 +252,11 @@ void UKF::Prediction(double delta_t) noise << 0.5f * delta_t * delta_t * cos (phi) * va, 0.5f * delta_t * delta_t * sin (phi) * va, delta_t * va, - 0.5f * delta_t * delta_t * vphidd, + 0.5f * delta_t * delta_t * vphidd, ///< \todo not sure if I should wrap delta_t * vphidd; // if phi dot is not zero - if (fabs (phid) > EPSILON) + if (std::abs (phid) > EPSILON) { VectorXd t2 = VectorXd (5); t2 << (v / phid) * (sin (phi + phid * delta_t) - sin (phi)), @@ -275,18 +278,17 @@ void UKF::Prediction(double delta_t) Xsig_pred_.col (i) = x.head (5) + t2 + noise; } - - /// \todo Not sure if this is necessary - //Xsig_pred_.col(i)(3) = wrap(Xsig_pred_.col(i)(3)); } - /// \todo Predicted mean and covariance + assert(! detectNan ("Xsig_pred_", Xsig_pred_)); + + // Predicted mean x_.fill (0.0f); for (unsigned i = 0; i < weights_.rows (); ++i) x_ = x_ + (weights_(i) * Xsig_pred_.col(i)); - /// \todo not sure if this wrapping is necessary - //x_(3) = wrap(x_(3)); - //predict state covariance matrix + x_(3) = wrap(x_(3)); + + /// Predict state covariance matrix P_.fill (0.0f); for (unsigned i = 0; i < weights_.rows (); ++i) { @@ -312,7 +314,70 @@ void UKF::UpdateLidar(MeasurementPackage const &meas_package) You'll also need to calculate the lidar NIS. */ - cout << "UPDATE LIDAR" << endl; + + int constexpr n_z = 2; + + VectorXd z = VectorXd (n_z); + z << meas_package.raw_measurements_[0], // px + meas_package.raw_measurements_[1]; // py + + MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1); + + for (unsigned i = 0; i < Xsig_pred_.cols (); ++i) + { + float const px = Xsig_pred_.col(i)(0); + float const py = Xsig_pred_.col(i)(1); + + // px + Zsig.col(i)(0) = px; + // py + Zsig.col(i)(1) = py; + } + + //calculate mean predicted measurement + VectorXd z_pred = VectorXd::Zero(n_z); + + for (unsigned i = 0; i < weights_.rows (); ++i) + z_pred += (weights_(i) * Zsig.col(i)); + + //calculate measurement covariance matrix S + MatrixXd S = MatrixXd::Zero(n_z, n_z); + + for (unsigned i = 0; i < weights_.rows (); ++i) + { + VectorXd zdiff = Zsig.col(i) - z_pred; + + S = S + (weights_(i) * zdiff * zdiff.transpose ()); + } + + MatrixXd R = MatrixXd(n_z, n_z); + + R << std_laspx_ * std_laspx_, 0, + 0, std_laspy_ * std_laspy_; + S = S + R; + + //calculate cross correlation matrix + MatrixXd Tc = MatrixXd::Zero(n_x_, n_z); + + for (unsigned i = 0; i < weights_.rows (); ++i) + { + VectorXd zdiff = Zsig.col (i) - z_pred; + VectorXd xdiff = Xsig_pred_.col (i) - x_; + + xdiff(3) = wrap(xdiff(3)); + + Tc = Tc + (weights_ (i) * xdiff * zdiff.transpose ()); + } + //calculate Kalman gain K; + MatrixXd const K = Tc * S.inverse (); + //update state mean and covariance matrix + VectorXd zdiff = z - z_pred; + + zdiff(1) = wrap(zdiff(1)); + x_ = x_ + K * zdiff; + /// \note I'm doing this but it feels like I'm covering something else up + x_(3) = wrap(x_(3)); + P_ = P_ - (K * S * K.transpose ()); } /** @@ -345,35 +410,30 @@ void UKF::UpdateRadar(MeasurementPackage const &meas_package) float const px = Xsig_pred_.col(i)(0); float const py = Xsig_pred_.col(i)(1); float const v = Xsig_pred_.col(i)(2); - float const phi = Xsig_pred_.col(i)(3); + float const psi = Xsig_pred_.col(i)(3); float const phid = Xsig_pred_.col(i)(4); // rho Zsig.col(i)(0) = sqrt (px * px + py * py); // phi - Zsig.col(i)(1) = wrap (atan2 (py, px)); + Zsig.col(i)(1) = atan2 (py, px); // rho-dot - /// \todo divide by zero risk? - if (fabs(Zsig.col(i)(0)) < EPSILON) - cerr << "DIVIDE BY ZERO" << endl; - Zsig.col(i)(2) = (px * cos (phi) * v + py * sin (phi) * v) / Zsig.col(i)(0); + if (std::abs(Zsig.col(i)(0)) > EPSILON) + Zsig.col(i)(2) = (px * cos(psi) * v + py * sin(psi) * v) / Zsig.col(i)(0); + else + // If rho is near 0 then assume it has no measurable radial velocity. I don't think + // this should happen though because that would imply the target is on top of us. + Zsig.col(i)(2) = 0.0f; } //calculate mean predicted measurement VectorXd z_pred = VectorXd::Zero(n_z); for (unsigned i = 0; i < weights_.rows (); ++i) - z_pred = z_pred + (weights_(i) * Zsig.col(i)); - z_pred(1) = wrap (z_pred (1)); + z_pred += (weights_(i) * Zsig.col(i)); //calculate measurement covariance matrix S - MatrixXd S = MatrixXd::Zero(n_z,n_z); - MatrixXd R = MatrixXd::Zero(3, 3); - - R << std_radr_ * std_radr_, 0, 0, - 0, std_radphi_ * std_radphi_, 0, - 0, 0, std_radrd_ * std_radrd_; - S = S + R; + MatrixXd S = MatrixXd::Zero(n_z, n_z); for (unsigned i = 0; i < weights_.rows (); ++i) { @@ -384,6 +444,13 @@ void UKF::UpdateRadar(MeasurementPackage const &meas_package) S = S + (weights_(i) * zdiff * zdiff.transpose ()); } + MatrixXd R = MatrixXd(n_z, n_z); + + R << std_radr_ * std_radr_, 0, 0, + 0, std_radphi_ * std_radphi_, 0, + 0, 0, std_radrd_ * std_radrd_; + S = S + R; + //calculate cross correlation matrix MatrixXd Tc = MatrixXd::Zero(n_x_, n_z); @@ -406,5 +473,7 @@ void UKF::UpdateRadar(MeasurementPackage const &meas_package) zdiff(1) = wrap(zdiff(1)); x_ = x_ + K * zdiff; + /// \note I'm doing this but it feels like I'm covering something else up + x_(3) = wrap(x_(3)); P_ = P_ - (K * S * K.transpose ()); } diff --git a/src/ukf.h b/src/ukf.h index f5ef261f..95504220 100755 --- a/src/ukf.h +++ b/src/ukf.h @@ -52,6 +52,9 @@ class UKF */ void UpdateRadar(MeasurementPackage const &meas_package); + /// \brief Dump state of x_ for debugging + void dumpState (std::string const &prefix_); + ///* initially set to false, set to true in first call of ProcessMeasurement bool is_initialized_;