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) {
  const int n_z = 3; 
  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);
    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);                                 
    float denominator = p_x*p_x + p_y*p_y;
    //Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(denominator);   _dot
   if(fabs(denominator) < .001)
    {
        Zsig(2,i) = (p_x*v1 + p_y*v2) / .001;
    }
    else
    {
      Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(denominator);   _dot
    }
  }
  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++) {  
    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 = S + weights_(i) * z_diff * z_diff.transpose();
  }
  S = S + R_radar_;
  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;
    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(1)> M_PI){ x_diff(1)-=2.*M_PI;}
    while (x_diff(1)<-M_PI){ x_diff(1)+=2.*M_PI;}
    Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
  }
  MatrixXd K = Tc * S.inverse();
  VectorXd z_delta = z - z_pred;
  while (z_delta(1)> M_PI){ z_delta(1)-=2.*M_PI;}
  while (z_delta(1)<-M_PI){ z_delta(1)+=2.*M_PI;}
  x_ = x_ + K * z_delta;
  P_ = P_ - K * S * K.transpose();
  float NIS_RADAR = z_delta.transpose() * S.inverse() * z_delta;
  std::cout << "NIS (RADAR)/expected(95%)" << NIS_RADAR << "/" << "7.815" << std::endl;
  radar_samples_total++;
  if(NIS_RADAR > 7.815)
  {
      radar_samples_over_NIS_high_percenticle++;
  }
  std::cout << "NIS (RADAR) ratio above 95th percentile " 
            << radar_samples_over_NIS_high_percenticle 
            << "/" 
            << radar_samples_total 
            << std::endl;
}
void UKF :: UpdateRadar ( MeasurementPackage meas_package ) { const int n_z = 3 ; 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 ) ; 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 ) ; float denominator = p_x * p_x + p_y * p_y ; if ( fabs ( denominator ) < .001 ) { Zsig ( 2 , i ) = ( p_x * v1 + p_y * v2 ) / .001 ; } else { Zsig ( 2 , i ) = ( p_x * v1 + p_y * v2 ) / sqrt ( denominator ) ; } } 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 ; 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_ ; 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 ; 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 ( 1 ) > M_PI ) { x_diff ( 1 ) -= 2. * M_PI ; } while ( x_diff ( 1 ) < - M_PI ) { x_diff ( 1 ) += 2. * M_PI ; } Tc = Tc + weights_ ( i ) * x_diff * z_diff . transpose ( ) ; } MatrixXd K = Tc * S . inverse ( ) ; VectorXd z_delta = z - z_pred ; while ( z_delta ( 1 ) > M_PI ) { z_delta ( 1 ) -= 2. * M_PI ; } while ( z_delta ( 1 ) < - M_PI ) { z_delta ( 1 ) += 2. * M_PI ; } x_ = x_ + K * z_delta ; P_ = P_ - K * S * K . transpose ( ) ; float NIS_RADAR = z_delta . transpose ( ) * S . inverse ( ) * z_delta ; std :: cout << " " << NIS_RADAR << " " << " " << std :: endl ; radar_samples_total ++ ; if ( NIS_RADAR > 7.815 ) { radar_samples_over_NIS_high_percenticle ++ ; } std :: cout << " " << radar_samples_over_NIS_high_percenticle << " " << radar_samples_total << std :: endl ; }
Updates the state and the state covariance matrix using a radar measurement.
Updates the state and the state covariance matrix using a radar measurement .
//transform sigma points into measurement space // radar has r, phi, r_dot //2n+1 simga points // measurement model //r //phi // watch for divide by zero... //Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(denominator);   //r_dot //r_dot //mean predicted measurement //innovation covariance matrix S //2n+1 simga points //residual //angle normalization //add measurement noise covariance matrix // Measurement Update // Cross correlation matrix (L30) // state difference // Update state mean and covariance //std::cout << "Update RADAR" << std::endl; //std::cout << x_ << std::endl; //std::cout << P_ << std::endl;