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(MeasurementPackage meas_package) {
  const int n_z = 2; 
  MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);
  Zsig.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++) 
  {  
    double p_x = Xsig_pred_(0,i);
    double p_y = Xsig_pred_(1,i);
    Zsig(0,i) = p_x;  
    Zsig(1,i) = p_y;  
  }
  VectorXd z_pred = VectorXd(n_z);
  z_pred.fill(0.0);
  for (int i=0; i < 2*n_aug_+1; 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 < 2 * n_aug_ + 1; i++) 
  {
    VectorXd z_diff = Zsig.col(i) - z_pred;
    S = S + weights_(i) * z_diff * z_diff.transpose();
  }
  S = S + R_lidar_;
  VectorXd z = meas_package.raw_measurements_;
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++) 
  { 
    VectorXd z_diff = Zsig.col(i) - z_pred;
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
  }
  MatrixXd K = Tc * S.inverse();
  VectorXd z_delta = z - z_pred;
  x_ = x_ + K * z_delta;
  P_ = P_ - K * S * K.transpose();
  float NIS_LIDAR = z_delta.transpose() * S.inverse() * z_delta;
  std::cout << "NIS (LIDAR)/expected(95%)" << NIS_LIDAR << "/" << "5.991" << std::endl;
  lidar_samples_total++;
  if(NIS_LIDAR > 5.991)
  {
      lidar_samples_over_NIS_high_percenticle++;
  }
  std::cout << "NIS ratio above 95th percentile " 
            << lidar_samples_over_NIS_high_percenticle 
            << "/" 
            << lidar_samples_total 
            << std::endl;
}
void UKF :: UpdateLidar ( MeasurementPackage meas_package ) { const int n_z = 2 ; MatrixXd Zsig = MatrixXd ( n_z , 2 * n_aug_ + 1 ) ; Zsig . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { double p_x = Xsig_pred_ ( 0 , i ) ; double p_y = Xsig_pred_ ( 1 , i ) ; Zsig ( 0 , i ) = p_x ; Zsig ( 1 , i ) = p_y ; } VectorXd z_pred = VectorXd ( n_z ) ; z_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; 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 < 2 * n_aug_ + 1 ; i ++ ) { VectorXd z_diff = Zsig . col ( i ) - z_pred ; S = S + weights_ ( i ) * z_diff * z_diff . transpose ( ) ; } S = S + R_lidar_ ; VectorXd z = meas_package . raw_measurements_ ; MatrixXd Tc = MatrixXd ( n_x_ , n_z ) ; Tc . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { VectorXd z_diff = Zsig . col ( i ) - z_pred ; VectorXd x_diff = Xsig_pred_ . col ( i ) - x_ ; Tc = Tc + weights_ ( i ) * x_diff * z_diff . transpose ( ) ; } MatrixXd K = Tc * S . inverse ( ) ; VectorXd z_delta = z - z_pred ; x_ = x_ + K * z_delta ; P_ = P_ - K * S * K . transpose ( ) ; float NIS_LIDAR = z_delta . transpose ( ) * S . inverse ( ) * z_delta ; std :: cout << " " << NIS_LIDAR << " " << " " << std :: endl ; lidar_samples_total ++ ; if ( NIS_LIDAR > 5.991 ) { lidar_samples_over_NIS_high_percenticle ++ ; } std :: cout << " " << lidar_samples_over_NIS_high_percenticle << " " << lidar_samples_total << std :: endl ; }
Updates the state and the state covariance matrix using a laser measurement.
Updates the state and the state covariance matrix using a laser measurement .
// lidar has x, y //transform sigma points into measurement space //2n+1 simga points // measurement model //x //y //mean predicted measurement //innovation covariance matrix S //add measurement noise covariance matrix // Measurement Update // Cross correlation matrix (L30) // Update state mean and covariance //std::cout << "Update LIDAR" << std::endl; //std::cout << x_ << std::endl; //std::cout << P_ << std::endl;