src/ukf.cpp
void UKF::UpdateRadar(MeasurementPackage meas_package) {
  /**
  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.
  */

  //Radar measument dimension
  int n_z = 3;

  //Step 1: Predict measurement
  MatrixXd Zsig = MatrixXd(n_z, n_sig_);

  //Transform sigma points into measurement space
  for (int i = 0; i < n_sig_; i++) {  //2n+1 simga points

    double p_x = Xsig_pred_(0, i);
    double p_y = Xsig_pred_(1, i);
    double v = Xsig_pred_(2, i);
    double yaw = Xsig_pred_(3, i);
    double v1 = cos(yaw)*v;
    double v2 = sin(yaw)*v;
    Zsig(0, i) = sqrt(p_x*p_x + p_y * p_y);                        //r
    Zsig(1, i) = atan2(p_y, p_x);                                 //phi
    Zsig(2, i) = (p_x*v1 + p_y * v2) / sqrt(p_x*p_x + p_y * p_y);   //r_dot
  }

  //Predicted mean
  VectorXd z_pred = VectorXd(n_z);
  z_pred.fill(0.0);
  for (int i = 0; i < n_sig_; i++) {
    z_pred = z_pred + weights_(i) * Zsig.col(i);
  }

  //Measurement covariance matrix S
  MatrixXd S = MatrixXd(n_z, n_z);
  S.fill(0.0);
  for (int i = 0; i < n_sig_; i++) {  //2n+1 simga points
    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 = S + weights_(i) * z_diff * z_diff.transpose();
  }

  //Add measurement noise covariance matrix
  S = S + R_radar_;

  //Step 2: Update state
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc.fill(0.0);
  for (int i = 0; i < n_sig_; 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;
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    while (x_diff(3) > M_PI) x_diff(3) -= 2.*M_PI;
    while (x_diff(3) < -M_PI) x_diff(3) += 2.*M_PI;
    Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
  }

  //Kalman gain K;
  MatrixXd K = Tc * S.inverse();

  //Residual
  VectorXd z_diff = meas_package.raw_measurements_ - z_pred;

  //Angle normalization
  while (z_diff(1) > M_PI) z_diff(1) -= 2.*M_PI;
  while (z_diff(1) < -M_PI) z_diff(1) += 2.*M_PI;

  //Update state mean and covariance matrix
  x_ = x_ + K * z_diff;
  P_ = P_ - K * S*K.transpose();

  //Update NIS
  NIS_radar_ = z_diff.transpose() * S.inverse() * z_diff;
}
/**
 * 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) {
  int n_z = 3;
  MatrixXd Zsig = MatrixXd(n_z, n_sig_);
  for (int i = 0; i < n_sig_; i++) {  
    double p_x = Xsig_pred_(0, i);
    double p_y = Xsig_pred_(1, i);
    double v = Xsig_pred_(2, i);
    double yaw = Xsig_pred_(3, i);
    double v1 = cos(yaw)*v;
    double v2 = sin(yaw)*v;
    Zsig(0, i) = sqrt(p_x*p_x + p_y * p_y);                        
    Zsig(1, i) = atan2(p_y, p_x);                                 
    Zsig(2, i) = (p_x*v1 + p_y * v2) / sqrt(p_x*p_x + p_y * p_y);   _dot
  }
  VectorXd z_pred = VectorXd(n_z);
  z_pred.fill(0.0);
  for (int i = 0; i < n_sig_; i++) {
    z_pred = z_pred + weights_(i) * Zsig.col(i);
  }
  MatrixXd S = MatrixXd(n_z, n_z);
  S.fill(0.0);
  for (int i = 0; i < n_sig_; 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 = S + weights_(i) * z_diff * z_diff.transpose();
  }
  S = S + R_radar_;
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc.fill(0.0);
  for (int i = 0; i < n_sig_; 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;
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    while (x_diff(3) > M_PI) x_diff(3) -= 2.*M_PI;
    while (x_diff(3) < -M_PI) x_diff(3) += 2.*M_PI;
    Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
  }
  MatrixXd K = Tc * S.inverse();
  VectorXd z_diff = meas_package.raw_measurements_ - 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();
  NIS_radar_ = z_diff.transpose() * S.inverse() * z_diff;
}
void UKF :: UpdateRadar ( MeasurementPackage meas_package ) { int n_z = 3 ; MatrixXd Zsig = MatrixXd ( n_z , n_sig_ ) ; for ( int i = 0 ; i < n_sig_ ; i ++ ) { double p_x = Xsig_pred_ ( 0 , i ) ; double p_y = Xsig_pred_ ( 1 , i ) ; double v = Xsig_pred_ ( 2 , i ) ; double yaw = Xsig_pred_ ( 3 , i ) ; double v1 = cos ( yaw ) * v ; double v2 = sin ( yaw ) * v ; Zsig ( 0 , i ) = sqrt ( p_x * p_x + p_y * p_y ) ; Zsig ( 1 , i ) = atan2 ( p_y , p_x ) ; Zsig ( 2 , i ) = ( p_x * v1 + p_y * v2 ) / sqrt ( p_x * p_x + p_y * p_y ) ; } VectorXd z_pred = VectorXd ( n_z ) ; z_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < n_sig_ ; i ++ ) { z_pred = z_pred + weights_ ( i ) * Zsig . col ( i ) ; } MatrixXd S = MatrixXd ( n_z , n_z ) ; S . fill ( 0.0 ) ; for ( int i = 0 ; i < n_sig_ ; 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 = S + weights_ ( i ) * z_diff * z_diff . transpose ( ) ; } S = S + R_radar_ ; MatrixXd Tc = MatrixXd ( n_x_ , n_z ) ; Tc . fill ( 0.0 ) ; for ( int i = 0 ; i < n_sig_ ; 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 ; VectorXd x_diff = Xsig_pred_ . col ( i ) - x_ ; while ( x_diff ( 3 ) > M_PI ) x_diff ( 3 ) -= 2. * M_PI ; while ( x_diff ( 3 ) < - M_PI ) x_diff ( 3 ) += 2. * M_PI ; Tc = Tc + weights_ ( i ) * x_diff * z_diff . transpose ( ) ; } MatrixXd K = Tc * S . inverse ( ) ; VectorXd z_diff = meas_package . raw_measurements_ - 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 ( ) ; NIS_radar_ = z_diff . transpose ( ) * S . inverse ( ) * z_diff ; }
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.
  */ //Radar measument dimension //Step 1: Predict measurement //Transform sigma points into measurement space //2n+1 simga points //r //phi //r_dot //Predicted mean //Measurement covariance matrix S //2n+1 simga points //Add measurement noise covariance matrix //Step 2: Update state //Kalman gain K; //Residual //Angle normalization //Update state mean and covariance matrix //Update NIS