Improve observer when robot yaw is +/- pi

RoboTeamTwente · Jul 3, 2024 · a45512b · a45512b
1 parent 06561b8
commit a45512b
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/roboteam_observer/observer/include/observer/filters/vision/robot/RobotOrientationFilter.h b/roboteam_observer/observer/include/observer/filters/vision/robot/RobotOrientationFilter.h
@@ -30,6 +30,8 @@ class RobotOrientationFilter : public PosVelFilter1D {
      * @return the limited yaw
      */
     [[nodiscard]] static double limitAngle(double yaw);
+    double lastObservation = 0.0;
+    int orientationTurns = 0;
 };
 
 #endif  // RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_ROBOT_ROBOTORIENTATIONFILTER_H_
diff --git a/roboteam_observer/observer/src/filters/vision/robot/RobotOrientationFilter.cpp b/roboteam_observer/observer/src/filters/vision/robot/RobotOrientationFilter.cpp
@@ -14,14 +14,14 @@ double RobotOrientationFilter::limitAngle(double yaw) {
 void RobotOrientationFilter::update(const double &position) {
     // We need to do something about the rotation's discontinuities at -pi/pi so it works correctly.
     // We allow the state to go outside of bounds [-PI,PI) in between updates, but then simply make sure the observation difference is correct
-    double stateRot = filter.state()[0];
-    double limitedRot = limitAngle(stateRot);
-    if (stateRot != limitedRot) {
-        setPosition(limitedRot);  // We're adjusting the value of the Kalman Filter here, be careful.
-                                  // We're only doing this so we don't get flips from -inf to inf and loss of double precision at high values.
+    if (position < -0.5 * M_PI && lastObservation > 0.5 * M_PI) {
+        orientationTurns++;
     }
-    double difference = limitAngle(position - stateRot);
-    double correctedObservation = limitedRot + difference;
+    if (position > 0.5 * M_PI && lastObservation < -0.5 * M_PI) {
+        orientationTurns--;
+    }
+    lastObservation = position;
+    double correctedObservation = position + 2 * M_PI * orientationTurns;
     PosVelFilter1D::update(correctedObservation);
 }