[TEST] Improved ukf tests

lagadic · Jun 17, 2024 · 9ed7506 · 9ed7506
1 parent e10a32b
commit 9ed7506
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 2 deletions.
diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp
@@ -256,6 +256,14 @@ int main(const int argc, const char *argv[])
     std::cout << "Press Enter to quit..." << std::endl;
     std::cin.get();
   }
+
+  vpColVector X_GT({ gt_X[0], gt_vx, gt_X[1], gt_vy });
+  vpColVector finalError = ukf.getXest() - X_GT;
+  const double maxError = 0.12;
+  if (finalError.frobeniusNorm() > maxError) {
+    std::cerr << "Error: max tolerated error = " << maxError << ", final error = " << finalError.frobeniusNorm() << std::endl;
+    return -1;
+  }
   return 0;
 }
 #else

diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp
@@ -387,8 +387,8 @@ class vpLandmarkMeasurements
   vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
     : m_x(x)
     , m_y(y)
-    , m_rngRange(range_std, 0., vpTime::measureTimeMicros())
-    , m_rngRelativeAngle(rel_angle_std, 0., vpTime::measureTimeMicros() + 4221)
+    , m_rngRange(range_std, 0., 4224)
+    , m_rngRelativeAngle(rel_angle_std, 0., 2112)
   { }
 
   /**
@@ -663,6 +663,13 @@ int main(const int argc, const char *argv[])
     std::cout << "Press Enter to quit..." << std::endl;
     std::cin.get();
   }
+
+  vpColVector finalError = grid.state_to_measurement(ukf.getXest()) - grid.measureGT(robot_pos);
+  const double maxError = 0.3;
+  if (finalError.frobeniusNorm() > maxError) {
+    std::cerr << "Error: max tolerated error = " << maxError << ", final error = " << finalError.frobeniusNorm() << std::endl;
+    return -1;
+  }
   return 0;
 }
 #else

diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp
@@ -416,6 +416,7 @@ int main(const int argc, const char *argv[])
   ac_vel[1] = gt_vY_init;
   vpACSimulator ac(ac_pos, ac_vel, stdevAircraftVelocity);
   vpColVector gt_Xprec = ac_pos;
+  vpColVector gt_Vprec = ac_vel;
   for (int i = 0; i < 500; ++i) {
     // Perform the measurement
     vpColVector gt_X = ac.update(dt);
@@ -444,6 +445,7 @@ int main(const int argc, const char *argv[])
 #endif
 
     gt_Xprec = gt_X;
+    gt_Vprec = gt_V;
   }
 
 #ifdef VISP_HAVE_DISPLAY
@@ -455,6 +457,15 @@ int main(const int argc, const char *argv[])
     std::cout << "Press Enter to quit..." << std::endl;
     std::cin.get();
   }
+
+  vpColVector X_GT({ gt_Xprec[0], gt_Vprec[0], gt_Xprec[1], gt_Vprec[1] });
+  vpColVector finalError = ukf.getXest() - X_GT;
+  const double maxError = 2.5;
+  if (finalError.frobeniusNorm() > maxError) {
+    std::cerr << "Error: max tolerated error = " << maxError << ", final error = " << finalError.frobeniusNorm() << std::endl;
+    return -1;
+  }
+
   return 0;
 }
 #else