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

@@ -24,7 +24,7 @@ VectorXd Tools::CalculateRMSE(vector<VectorXd> const &estimations,
 	* Calculate the RMSE here.
 	*/
 
-	rmse_ << 0,0,0,0;
+	rmse_.fill (0.0);
 
 	if (estimations.empty () || ground_truth.empty ())
     {

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

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