diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp
index 9bb5347..4350d11 100644
--- a/nav/mesh/NavControllerMesh.cpp
+++ b/nav/mesh/NavControllerMesh.cpp
@@ -213,10 +213,8 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
 		static float avgSum = 0;
 		static int avgCount = 0;
 
-		// fixed update rate based on incoming sensor data
-		// allows working with live data and faster for offline data
-		const Timestamp diff = curObs.currentTime - lastTransition;
-		if (diff >= Settings::Filter::updateEvery) {
+        const Timestamp diff = curObs.currentTime - lastTransition;
+        if (curCtrl.numStepsSinceLastTransition > 0){
 
 			// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
 			const Timestamp err = diff - Settings::Filter::updateEvery;
@@ -245,31 +243,33 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
 	/** perform a filter-update (called from a background-loop) */
 	void MeshBased::NavControllerMesh::filterUpdate() {
 
-//		//lastEst = curEst;
-//		curEst = pf->update(&curCtrl, curObs);
-//		//Log::add("Nav", "cur est: " + curEst.position.asString());
+        //lastEst = curEst;
+        MyState sCurEst = pf->update(&curCtrl, curObs);
+        curEst.pos_m = sCurEst.loc.pos;
+        curEst.head = sCurEst.heading;
 
-//		// inform listeners about the new estimation
-//		for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}
+        // inform listeners about the new estimation
+        for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}
 
-//		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
-//		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
 
-//		// update estimated path
-//		estPath.push_back(curEst.position.inMeter());
-//		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
-//		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
+        // update estimated path
+        estPath.push_back(curEst.pos_m);
+        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
+        Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
 
-//		PFTrans* trans = (PFTrans*)pf->getTransition();
-//		const MyGridNode* node = grid->getNodePtrFor(curEst.position);
-//		if (node) {
-//			try {
-//				pathToDest = trans->modDestination.getShortestPath(*node);
-//				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
-//				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
-//			} catch (...) {;}
-//		}
-//		//	mainController->getMapView()->showGridImportance();
+        //TODO: Implement destination for Mesh
+//        PFTrans* trans = (PFTrans*)pf->getTransition();
+//        const MyGridNode* node = grid->getNodePtrFor(curEst.position);
+//        if (node) {
+//            try {
+//                pathToDest = trans->modDestination.getShortestPath(*node);
+//                Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+//                Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+//            } catch (...) {;}
+//        }
+        //	mainController->getMapView()->showGridImportance();
 
 	}