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(const MeasurementPackage& meas_package,
                      const MatrixXd& Xsig_pred, double dt) {
  MatrixXd Zsig = RadarMeasurement::Sigma2Meas(Xsig_pred);
  VectorXd z_pred = RadarMean(Xsig_pred, Zsig);
  MatrixXd S = RadarInno(Xsig_pred, Zsig, z_pred);
  S = S + radar_measurement_.R();
  MatrixXd K = RadarKalmanGain(S, Xsig_pred, Zsig, z_pred);
  VectorXd z = meas_package.raw_measurements_;
  VectorXd z_diff = z - z_pred;
  z_diff(1) = Tools::NormalizeAngle(z_diff(1));
  x_ = x_ + K * z_diff;
  P_ = P_ - K * S * K.transpose();
  VectorXd y = z - z_pred;
  nis_radar_ = y.transpose() * S.inverse() * y;
  logger_.log("radar", nis_radar_);
}
void UKF :: UpdateRadar ( const MeasurementPackage & meas_package , const MatrixXd & Xsig_pred , double dt ) { MatrixXd Zsig = RadarMeasurement :: Sigma2Meas ( Xsig_pred ) ; VectorXd z_pred = RadarMean ( Xsig_pred , Zsig ) ; MatrixXd S = RadarInno ( Xsig_pred , Zsig , z_pred ) ; S = S + radar_measurement_ . R ( ) ; MatrixXd K = RadarKalmanGain ( S , Xsig_pred , Zsig , z_pred ) ; VectorXd z = meas_package . raw_measurements_ ; VectorXd z_diff = z - z_pred ; z_diff ( 1 ) = Tools :: NormalizeAngle ( z_diff ( 1 ) ) ; x_ = x_ + K * z_diff ; P_ = P_ - K * S * K . transpose ( ) ; VectorXd y = z - z_pred ; nis_radar_ = y . transpose ( ) * S . inverse ( ) * y ; logger_ . log ( " " , nis_radar_ ) ; }
Updates the state and the state covariance matrix using a radar
 measurement.
Updates the state and the state covariance matrix using a radar measurement .
// create matrix for sigma points in measurement space // mean predicted measurement // innovation covariance matrix S // add measurement noise covariance matrix // residual // angle normalization // update state mean and covariance matrix // NIS