src/ukf.cpp
/**
 * Updates the state and the state covariance matrix using a radar measurement.
 * @param {MeasurementPackage} meas_package
 */
Updates the state and the state covariance matrix using a radar measurement.
Updates the state and the state covariance matrix using a radar measurement .
void UKF::UpdateRadar(MeasurementPackage meas_package) {
	MatrixXd Zsig = MatrixXd(3, 2*n_aug_+1);
	Zsig.fill(0.0);
	for (int i=0; i < 2*n_aug_+1; ++i) {
		double px = Xsig_pred_(0,i);
		double py = Xsig_pred_(1,i);
		double v = Xsig_pred_(2,i);
		double yaw = Xsig_pred_(3,i);
		Zsig(0,i) = sqrt(px*px + py*py); 
		Zsig(1,i) = atan2(py,px); 
		Zsig(2,i) = (px*cos(yaw)*v + py*sin(yaw)*v)/ sqrt(px*px + py*py); _dot
	}
	VectorXd z_pred = VectorXd(3);
	z_pred.fill(0.0);
	for (int i=0; i < 2*n_aug_+1; ++i) {
		z_pred += weights_(i)*Zsig.col(i);		
	}
	MatrixXd S = MatrixXd(3,3);
	S.fill(0.0);
	for (int i=0; i < 2*n_aug_+1; ++i) {
		esidual
		VectorXd z_diff = Zsig.col(i) - z_pred;
		while(z_diff(1) > M_PI){z_diff(1) -= 2*M_PI;}
		while(z_diff(1) < -M_PI){z_diff(1) += 2*M_PI;}
		S += weights_(i)*z_diff*z_diff.transpose();
	}
	MatrixXd R = MatrixXd(3,3);
	R << std_radr_*std_radr_, 0, 0,
	 0, std_radphi_*std_radphi_, 0,
	0, 0, std_radrd_*std_radrd_; 
	S += R;
	VectorXd z = meas_package.raw_measurements_;
	MatrixXd T = MatrixXd(n_x_, 3);
	T.fill(0.0);
	for (int i=0; i < 2*n_aug_+1; ++i) {
		VectorXd x_diff = Xsig_pred_.col(i) - x_;
		VectorXd z_diff = Zsig.col(i) - z_pred;
		while(x_diff(3) > M_PI){x_diff(3) -= 2*M_PI;}
		while(x_diff(3) < -M_PI){x_diff(3) += 2*M_PI;}
		while(z_diff(1) > M_PI){z_diff(1) -= 2*M_PI;}
		while(z_diff(1) < -M_PI){z_diff(1) += 2*M_PI;}
		T += weights_(i)*x_diff*z_diff.transpose();
	}
	MatrixXd K = T * S.inverse();
	esidual
	VectorXd z_diff = z - z_pred;
	while(z_diff(1) > M_PI){z_diff(1) -= 2*M_PI;}
	while(z_diff(1) < -M_PI){z_diff(1) += 2*M_PI;}
	x_ = x_ + K * z_diff;
	P_ = P_ - K * S * K.transpose();
	double NIS = z_diff.transpose() * S.inverse() * z_diff;
	if (7.815 < NIS) {
		r_nis_(1)++;
		r_nis_(2)++;
		r_nis_(3)++;
		r_nis_(4)++;
	}
	else if (6.251 < NIS) {
		r_nis_(1)++;
		r_nis_(2)++;
		r_nis_(3)++;
	}
	else if (0.584 < NIS) {
		r_nis_(1)++;
		r_nis_(2)++;
	}
	else if (0.352 < NIS) {
		r_nis_(1)++;
	}
	r_nis_(0)++;
}
void UKF :: UpdateRadar ( MeasurementPackage meas_package ) { MatrixXd Zsig = MatrixXd ( 3 , 2 * n_aug_ + 1 ) ; Zsig . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; ++ i ) { double px = Xsig_pred_ ( 0 , i ) ; double py = Xsig_pred_ ( 1 , i ) ; double v = Xsig_pred_ ( 2 , i ) ; double yaw = Xsig_pred_ ( 3 , i ) ; Zsig ( 0 , i ) = sqrt ( px * px + py * py ) ; Zsig ( 1 , i ) = atan2 ( py , px ) ; Zsig ( 2 , i ) = ( px * cos ( yaw ) * v + py * sin ( yaw ) * v ) / sqrt ( px * px + py * py ) ; } VectorXd z_pred = VectorXd ( 3 ) ; z_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; ++ i ) { z_pred += weights_ ( i ) * Zsig . col ( i ) ; } MatrixXd S = MatrixXd ( 3 , 3 ) ; S . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; ++ i ) { VectorXd z_diff = Zsig . col ( i ) - z_pred ; while ( z_diff ( 1 ) > M_PI ) { z_diff ( 1 ) -= 2 * M_PI ; } while ( z_diff ( 1 ) < - M_PI ) { z_diff ( 1 ) += 2 * M_PI ; } S += weights_ ( i ) * z_diff * z_diff . transpose ( ) ; } MatrixXd R = MatrixXd ( 3 , 3 ) ; R << std_radr_ * std_radr_ , 0 , 0 , 0 , std_radphi_ * std_radphi_ , 0 , 0 , 0 , std_radrd_ * std_radrd_ ; S += R ; VectorXd z = meas_package . raw_measurements_ ; MatrixXd T = MatrixXd ( n_x_ , 3 ) ; T . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; ++ i ) { VectorXd x_diff = Xsig_pred_ . col ( i ) - x_ ; VectorXd z_diff = Zsig . col ( i ) - z_pred ; while ( x_diff ( 3 ) > M_PI ) { x_diff ( 3 ) -= 2 * M_PI ; } while ( x_diff ( 3 ) < - M_PI ) { x_diff ( 3 ) += 2 * M_PI ; } while ( z_diff ( 1 ) > M_PI ) { z_diff ( 1 ) -= 2 * M_PI ; } while ( z_diff ( 1 ) < - M_PI ) { z_diff ( 1 ) += 2 * M_PI ; } T += weights_ ( i ) * x_diff * z_diff . transpose ( ) ; } MatrixXd K = T * S . inverse ( ) ; VectorXd z_diff = z - z_pred ; while ( z_diff ( 1 ) > M_PI ) { z_diff ( 1 ) -= 2 * M_PI ; } while ( z_diff ( 1 ) < - M_PI ) { z_diff ( 1 ) += 2 * M_PI ; } x_ = x_ + K * z_diff ; P_ = P_ - K * S * K . transpose ( ) ; double NIS = z_diff . transpose ( ) * S . inverse ( ) * z_diff ; if ( 7.815 < NIS ) { r_nis_ ( 1 ) ++ ; r_nis_ ( 2 ) ++ ; r_nis_ ( 3 ) ++ ; r_nis_ ( 4 ) ++ ; } else if ( 6.251 < NIS ) { r_nis_ ( 1 ) ++ ; r_nis_ ( 2 ) ++ ; r_nis_ ( 3 ) ++ ; } else if ( 0.584 < NIS ) { r_nis_ ( 1 ) ++ ; r_nis_ ( 2 ) ++ ; } else if ( 0.352 < NIS ) { r_nis_ ( 1 ) ++ ; } r_nis_ ( 0 ) ++ ; }
Updates the state and the state covariance matrix using a radar measurement.
Updates the state and the state covariance matrix using a radar measurement .
/**
  TODO:

  Complete this function! Use radar data to update the belief about the object's
  position. Modify the state vector, x_, and covariance, P_.

  You'll also need to calculate the radar NIS.
  */ ///////////////////////////// // PREDICT MEASUREMENT SIGMAS ///////////////////////////// // TRANSFORM SIGMAS TO MEASUREMENT SPACE // intermediate variables for readibility // calculate h(x) for each sigma point // r // phi // r_dot // MEAN PREDICTION // COVARIANCE PREDICTION // residual // normalize angle // add measurement noise matrix ///////////////////// // MEASUREMENT UPDATE ///////////////////// // measurement  // cross-correlation matrix //normalize angles // kalman gain // residual // normalize angle // update mean and covariance // calculate NIS and place into correct bucket in r_nis_ // increment total NIS value