From d5188879c4966d599f67a2c77c35534d63a79d82 Mon Sep 17 00:00:00 2001
From: Florian Polster <fpolster@uni-koblenz.de>
Date: Wed, 8 Mar 2017 20:01:55 +0100
Subject: [PATCH] more rotational diversity
---
homer_mapping/src/ParticleFilter/SlamFilter.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 72f741e4..459bfca4 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -482,7 +482,7 @@ void SlamFilter::drift() {
// m_Alpha2 * fabs(distance));
float estRotToPose =
- rotToPose + randomGauss(m_Alpha1 * fabs(poseRotation) +
+ rotToPose + randomGauss(m_Alpha1 * 0.5 * fabs(poseRotation) +
m_Alpha2 * fabs(distance));
float estDeltaRot =
@@ -495,7 +495,9 @@ void SlamFilter::drift() {
float posY = pose.y() + estDist * sin(pose.theta() + estRotToPose) +
randomGauss(m_Alpha5 * fabs(estDeltaRot)) +
randomGauss(0.0004);
- float theta = pose.theta() + estDeltaRot + randomGauss(0.00005);
+ float theta = pose.theta() + estDeltaRot +
+ randomGauss(2 * m_Alpha1 * fabs(poseRotation)) +
+ randomGauss(0.00005);
// save new pose
while (theta > M_PI) theta -= M_2PI;
--
GitLab