src/ukf.cpp
/**
 * Updates the state and the state covariance matrix using a laser
 * measurement.
 * @param {MeasurementPackage} meas_package
 */
Updates the state and the state covariance matrix using a laser
measurement.
Updates the state and the state covariance matrix using a laser measurement .
void UKF::UpdateLidar(const MeasurementPackage& meas_package, double dt) {
  const MatrixXd H = lidar_measurement_.H();
  VectorXd z_pred = H * x_;
  VectorXd z = meas_package.raw_measurements_;
  VectorXd y = z - z_pred;
  MatrixXd Ht = H.transpose();  
  MatrixXd S = H * P_ * Ht + lidar_measurement_.R();
  MatrixXd PHt = P_ * Ht;
  MatrixXd K = PHt * S.inverse();
  x_ = x_ + (K * y);
  long x_size = x_.size();
  MatrixXd I = MatrixXd::Identity(x_size, x_size);
  P_ = (I - K * H) * P_;
  nis_lidar_ = y.transpose() * S.inverse() * y;
  logger_.log("lidar", nis_lidar_);
}
void UKF :: UpdateLidar ( const MeasurementPackage & meas_package , double dt ) { const MatrixXd H = lidar_measurement_ . H ( ) ; VectorXd z_pred = H * x_ ; VectorXd z = meas_package . raw_measurements_ ; VectorXd y = z - z_pred ; MatrixXd Ht = H . transpose ( ) ; MatrixXd S = H * P_ * Ht + lidar_measurement_ . R ( ) ; MatrixXd PHt = P_ * Ht ; MatrixXd K = PHt * S . inverse ( ) ; x_ = x_ + ( K * y ) ; long x_size = x_ . size ( ) ; MatrixXd I = MatrixXd :: Identity ( x_size , x_size ) ; P_ = ( I - K * H ) * P_ ; nis_lidar_ = y . transpose ( ) * S . inverse ( ) * y ; logger_ . log ( " " , nis_lidar_ ) ; }
Updates the state and the state covariance matrix using a laser
 measurement.
Updates the state and the state covariance matrix using a laser measurement .
// used twice // new estimate // NIS