evaluateScanLikelihood() more robust formula

MRPT · Mar 12, 2018 · 464a527 · 464a527
1 parent 11a0cde
commit 464a527
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/libs/obs/src/CObservation2DRangeScanWithUncertainty.cpp b/libs/obs/src/CObservation2DRangeScanWithUncertainty.cpp
@@ -48,15 +48,15 @@ double CObservation2DRangeScanWithUncertainty::evaluateScanLikelihood(const CObs
 
 		const double otherScanRange = otherScan.validRange[i] ? otherScan.scan[i] : otherScan.maxRange;
 
-		const double likGauss = mrpt::math::normalPDF(otherScanRange, rangesMean[i], std::sqrt(prediction_total_var) );
+		const double likGauss = std::exp(-0.5*mrpt::utils::square(otherScanRange - rangesMean[i]) / prediction_total_var);
 		double pi;
 		if (otherScan.scan[i] > rangesMean[i]) {
 			if (otherScan.validRange[i])
 			     pi = likGauss;
 			else pi = std::max(likGauss, params.prob_lost_ray);
 		}
 		else {
-			pi = std::max( likGauss, std::min(1.0, params.prob_outliers *otherScanRange/rangesMean[i] ) );
+			pi = std::max( likGauss, std::min(1.0, params.prob_outliers) );
 		}
 
 		double lpi = std::max(params.min_ray_log_lik, log(pi));