activity now debugable

filter is updated every step and every 1000 ms an evaluation is calculated
also added sink or swim method from frank, works great

nav/mesh/State.h

@@ -38,6 +38,32 @@ namespace MeshBased {
             return copy;
         }
 
+        float getX(){
+            return loc.pos.x;
+        }
+
+        float getY() {
+            return loc.pos.y;
+        }
+
+        float getZ() {
+            return loc.pos.z;
+        }
+
+        void setPosition(Point3 pos){
+            loc.pos = pos;
+        }
+
+        float getBinValue(const int dim) const {
+            switch (dim) {
+                case 0: return this->loc.pos.x;
+                case 1: return this->loc.pos.y;
+                case 2: return this->loc.pos.z;
+                case 3: return this->heading.getRAD();
+            }
+            throw "cant find this value within the bin";
+        }
+
 	};