diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 07340bc..fe375e9 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -12,11 +12,11 @@
 #include "LinearAlgebra/AngleUsing.h"
 #include "LinearAlgebra/Spherical.h"
 
-void NetworkSync::SendVector3(unsigned char *data, unsigned int &startIndex,
+void NetworkSync::SendVector3(unsigned char *data, unsigned int startIndex,
                               const Vector3 v) {
   SendSingle100(data, startIndex, v.x);
-  SendSingle100(data, startIndex, v.y);
-  SendSingle100(data, startIndex, v.z);
+  SendSingle100(data, startIndex += 4, v.y);
+  SendSingle100(data, startIndex += 4, v.z);
 }
 
 void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
@@ -61,6 +61,7 @@ void NetworkSync::SendQuat32(unsigned char *data, int startIndex,
     qz = -qz;
     qw = -qw;
   }
+  // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
   data[startIndex++] = qx;
   data[startIndex++] = qy;
   data[startIndex++] = qz;
diff --git a/NetworkSync.h b/NetworkSync.h
index c9efed8..efca1e9 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -61,7 +61,7 @@ protected:
                    const float value);
   void SendAngle32(unsigned char *data, unsigned int startIndex,
                    const float value);
-  void SendVector3(unsigned char *data, unsigned int &startIndex,
+  void SendVector3(unsigned char *data, unsigned int startIndex,
                    const Vector3 v);
   void SendQuaternion(unsigned char *data, const int startIndex,
                       const Quaternion q);
diff --git a/Perception.cpp b/Perception.cpp
index 9fb797e..53760ad 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -201,11 +201,11 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
     else {
       if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
 #ifdef RC_DEBUG2
-        Serial.print((int)this->trackedObjects[objIx]->id);
-        Serial.println(": update tracked object");
+        // Serial.print((int)this->trackedObjects[objIx]->id);
+        // Serial.println(": update tracked object");
 #endif
 
-        this->trackedObjects[objIx]->Refresh(obj->position);
+        this->trackedObjects[objIx]->Refresh(obj->position, obj->orientation);
         delete obj;
         return this->trackedObjects[objIx];
       }
@@ -333,12 +333,12 @@ void Perception::Update(float currentTimeMs) {
 
     if (obj->DegradeConfidence(deltaTime) == false) {
       // delete obj
-      // if (roboid != nullptr && roboid->networkSync != nullptr) {
-      //   printf("Cleanup obj %d\n", obj->id);
-      //   roboid->networkSync->DestroyObject(obj);
-      // }
-      // this->trackedObjects[objIx] = nullptr;
-      // delete obj;
+      if (roboid != nullptr && roboid->networkSync != nullptr) {
+        printf("Cleanup obj %d\n", obj->id);
+        roboid->networkSync->DestroyObject(obj);
+      }
+      this->trackedObjects[objIx] = nullptr;
+      delete obj;
     }
   }
 }
diff --git a/TrackedObject.h b/TrackedObject.h
index 65ed968..f4dbdb9 100644
--- a/TrackedObject.h
+++ b/TrackedObject.h
@@ -48,7 +48,7 @@ public:
   /// = 1.2F and position->distance = 1.6 will be considered different, but
   /// objects with coordinates position->distance = 1.2 and position->distance
   /// = 1.0 can be the same.
-  static constexpr float equalityDistance = 0.01F; // 0.3F;
+  static constexpr float equalityDistance = 0.3F;
   /// @brief The maximum difference in angle from the roboids orientation in
   /// which two objects may be considered the same
   /// @details When the difference in angle is exactly this value, the objects
@@ -56,7 +56,7 @@ public:
   /// With a value of 5.0, object with coordinates position->angle = 30
   /// and position->angle = 36 will be considered different, but object with
   /// coordinated position->angle = 30 and position->angle = 27 can be the same.
-  static constexpr float equalityAngle = 0.01F; // 5.0F;
+  static constexpr float equalityAngle = 5.0F;
 
   /// @brief The id of the tracked object
   char id;