|
@@ -101,14 +101,12 @@ public class Models {
|
|
|
}
|
|
|
double teoricaldistance = mindist;
|
|
|
|
|
|
- if (dp.getDistance() < 4 || dp.getDistance() > 240)
|
|
|
+ // mapa nao tem parede - ignora medidas...
|
|
|
+ if (teoricaldistance == -1)
|
|
|
continue;
|
|
|
-
|
|
|
- if (teoricaldistance == -1) {
|
|
|
- prob = 0;
|
|
|
- break;
|
|
|
- }
|
|
|
- prob *= pdf(dp.getDistance()-teoricaldistance, SONAR_SIGMA)*0.8+0.2/255; // desvio padrao
|
|
|
+
|
|
|
+ double error = dp.getDistance() >= 255 || dp.getDistance() <= 0 ? 1.0: 0.0;
|
|
|
+ prob *= pdf(dp.getDistance()-teoricaldistance, SONAR_SIGMA)*0.7+0.2/255+.1*error; // desvio padrao
|
|
|
}
|
|
|
return prob;
|
|
|
}
|