diff --git a/example/kalman/CMakeLists.txt b/example/kalman/CMakeLists.txt
index ccfb268df9..103891f971 100644
--- a/example/kalman/CMakeLists.txt
+++ b/example/kalman/CMakeLists.txt
@@ -16,3 +16,7 @@ foreach(cpp ${example_cpp})
     visp_add_dependency(${cpp} "examples")
   endif()
 endforeach()
+
+visp_add_test(ukf-linear-example ukf-linear-example ${OPTION_TO_DESACTIVE_DISPLAY})
+visp_add_test(ukf-nonlinear-example ukf-nonlinear-example ${OPTION_TO_DESACTIVE_DISPLAY})
+visp_add_test(ukf-nonlinear-complex-example ukf-nonlinear-complex-example ${OPTION_TO_DESACTIVE_DISPLAY})
diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp
index 664f877cad..030b0f2b95 100644
--- a/example/kalman/ukf-linear-example.cpp
+++ b/example/kalman/ukf-linear-example.cpp
@@ -104,8 +104,27 @@ vpColVector hx(const vpColVector &chi)
   return point;
 }
 
-int main(/*const int argc, const char *argv[]*/)
+int main(const int argc, const char *argv[])
 {
+  bool opt_useDisplay = true;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg(argv[i]);
+    if (arg == "-d") {
+      opt_useDisplay = false;
+    }
+    else if ((arg == "-h") || (arg == "--help")) {
+      std::cout << "SYNOPSIS" << std::endl;
+      std::cout << "  " << argv[0] << " [-d][-h]" << std::endl;
+      std::cout << std::endl << std::endl;
+      std::cout << "DETAILS" << std::endl;
+      std::cout << "  -d" << std::endl;
+      std::cout << "    Deactivate display." << std::endl;
+      std::cout << std::endl;
+      std::cout << "  -h, --help" << std::endl;
+      return 0;
+    }
+  }
+
   const double dt = 0.01; // Period of 1s
   const double gt_dx = 0.01; // Ground truth displacement along x axis between two measurements
   const double gt_dy = 0.005; // Ground truth displacement along x axis between two measurements
@@ -143,39 +162,42 @@ int main(/*const int argc, const char *argv[]*/)
   ukf.init(vpColVector({ 0., 0.75 * gt_vx, 0., 0.75 * gt_vy }), P0);
 
 #ifdef VISP_HAVE_DISPLAY
+  vpPlot *plot = nullptr;
   // Initialize the plot
-  vpPlot plot(4);
-  plot.initGraph(0, 3);
-  plot.setTitle(0, "Position along X-axis");
-  plot.setUnitX(0, "Time (s)");
-  plot.setUnitY(0, "Position (m)");
-  plot.setLegend(0, 0, "GT");
-  plot.setLegend(0, 1, "Measure");
-  plot.setLegend(0, 2, "Filtered");
-
-  plot.initGraph(1, 3);
-  plot.setTitle(1, "Velocity along X-axis");
-  plot.setUnitX(1, "Time (s)");
-  plot.setUnitY(1, "Velocity (m/s)");
-  plot.setLegend(1, 0, "GT");
-  plot.setLegend(1, 1, "Measure");
-  plot.setLegend(1, 2, "Filtered");
-
-  plot.initGraph(2, 3);
-  plot.setTitle(2, "Position along Y-axis");
-  plot.setUnitX(2, "Time (s)");
-  plot.setUnitY(2, "Position (m)");
-  plot.setLegend(2, 0, "GT");
-  plot.setLegend(2, 1, "Measure");
-  plot.setLegend(2, 2, "Filtered");
-
-  plot.initGraph(3, 3);
-  plot.setTitle(3, "Velocity along Y-axis");
-  plot.setUnitX(3, "Time (s)");
-  plot.setUnitY(3, "Velocity (m/s)");
-  plot.setLegend(3, 0, "GT");
-  plot.setLegend(3, 1, "Measure");
-  plot.setLegend(3, 2, "Filtered");
+  if (opt_useDisplay) {
+    plot = new vpPlot(4);
+    plot->initGraph(0, 3);
+    plot->setTitle(0, "Position along X-axis");
+    plot->setUnitX(0, "Time (s)");
+    plot->setUnitY(0, "Position (m)");
+    plot->setLegend(0, 0, "GT");
+    plot->setLegend(0, 1, "Measure");
+    plot->setLegend(0, 2, "Filtered");
+
+    plot->initGraph(1, 3);
+    plot->setTitle(1, "Velocity along X-axis");
+    plot->setUnitX(1, "Time (s)");
+    plot->setUnitY(1, "Velocity (m/s)");
+    plot->setLegend(1, 0, "GT");
+    plot->setLegend(1, 1, "Measure");
+    plot->setLegend(1, 2, "Filtered");
+
+    plot->initGraph(2, 3);
+    plot->setTitle(2, "Position along Y-axis");
+    plot->setUnitX(2, "Time (s)");
+    plot->setUnitY(2, "Position (m)");
+    plot->setLegend(2, 0, "GT");
+    plot->setLegend(2, 1, "Measure");
+    plot->setLegend(2, 2, "Filtered");
+
+    plot->initGraph(3, 3);
+    plot->setTitle(3, "Velocity along Y-axis");
+    plot->setUnitX(3, "Time (s)");
+    plot->setUnitY(3, "Velocity (m/s)");
+    plot->setLegend(3, 0, "GT");
+    plot->setLegend(3, 1, "Measure");
+    plot->setLegend(3, 2, "Filtered");
+  }
 #endif
 
   // Initialize measurement noise
@@ -198,32 +220,51 @@ int main(/*const int argc, const char *argv[]*/)
     vpColVector Xest = ukf.getXest();
 
 #ifdef VISP_HAVE_DISPLAY
+    if (opt_useDisplay) {
     // Plot the ground truth, measurement and filtered state
-    plot.plot(0, 0, i, gt_X[0]);
-    plot.plot(0, 1, i, x_meas);
-    plot.plot(0, 2, i, Xest[0]);
-
-    double vX_meas = (x_meas - z_prec[0]) / dt;
-    plot.plot(1, 0, i, gt_vx);
-    plot.plot(1, 1, i, vX_meas);
-    plot.plot(1, 2, i, Xest[1]);
-
-    plot.plot(2, 0, i, gt_X[1]);
-    plot.plot(2, 1, i, y_meas);
-    plot.plot(2, 2, i, Xest[2]);
-
-    double vY_meas = (y_meas - z_prec[1]) / dt;
-    plot.plot(3, 0, i, gt_vy);
-    plot.plot(3, 1, i, vY_meas);
-    plot.plot(3, 2, i, Xest[3]);
+      plot->plot(0, 0, i, gt_X[0]);
+      plot->plot(0, 1, i, x_meas);
+      plot->plot(0, 2, i, Xest[0]);
+
+      double vX_meas = (x_meas - z_prec[0]) / dt;
+      plot->plot(1, 0, i, gt_vx);
+      plot->plot(1, 1, i, vX_meas);
+      plot->plot(1, 2, i, Xest[1]);
+
+      plot->plot(2, 0, i, gt_X[1]);
+      plot->plot(2, 1, i, y_meas);
+      plot->plot(2, 2, i, Xest[2]);
+
+      double vY_meas = (y_meas - z_prec[1]) / dt;
+      plot->plot(3, 0, i, gt_vy);
+      plot->plot(3, 1, i, vY_meas);
+      plot->plot(3, 2, i, Xest[3]);
+    }
 #endif
 
     // Update
     gt_X += gt_dX;
     z_prec = z;
   }
-  std::cout << "Press Enter to quit..." << std::endl;
-  std::cin.get();
+
+  if (opt_useDisplay) {
+    std::cout << "Press Enter to quit..." << std::endl;
+    std::cin.get();
+  }
+
+#ifdef VISP_HAVE_DISPLAY
+  if (opt_useDisplay) {
+    delete plot;
+  }
+#endif
+
+  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
index 0b0904c36c..adbdb4ef2a 100644
--- 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)
   { }
 
   /**
@@ -528,8 +528,27 @@ class vpLandmarksGrid
   std::vector<vpLandmarkMeasurements> m_landmarks; /*!< The list of landmarks forming the grid.*/
 };
 
-int main(/*const int argc, const char *argv[]*/)
+int main(const int argc, const char *argv[])
 {
+  bool opt_useDisplay = true;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg(argv[i]);
+    if (arg == "-d") {
+      opt_useDisplay = false;
+    }
+    else if ((arg == "-h") || (arg == "--help")) {
+      std::cout << "SYNOPSIS" << std::endl;
+      std::cout << "  " << argv[0] << " [-d][-h]" << std::endl;
+      std::cout << std::endl << std::endl;
+      std::cout << "DETAILS" << std::endl;
+      std::cout << "  -d" << std::endl;
+      std::cout << "    Deactivate display." << std::endl;
+      std::cout << std::endl;
+      std::cout << "  -h, --help" << std::endl;
+      return 0;
+    }
+  }
+
   const double dt = 0.1; // Period of 0.1s
   const double step = 1.; // Number of update of the robot position between two UKF filtering
   const double sigmaRange = 0.3; // Standard deviation of the range measurement: 0.3m
@@ -593,14 +612,17 @@ int main(/*const int argc, const char *argv[]*/)
   ukf.setStateResidualFunction(stateResidual);
 
 #ifdef VISP_HAVE_DISPLAY
+  vpPlot *plot = nullptr;
+  if (opt_useDisplay) {
   // Initialize the plot
-  vpPlot plot(1);
-  plot.initGraph(0, 2);
-  plot.setTitle(0, "Position of the robot");
-  plot.setUnitX(0, "Position along x(m)");
-  plot.setUnitY(0, "Position along y (m)");
-  plot.setLegend(0, 0, "GT");
-  plot.setLegend(0, 1, "Filtered");
+    plot = new vpPlot(1);
+    plot->initGraph(0, 2);
+    plot->setTitle(0, "Position of the robot");
+    plot->setUnitX(0, "Position along x(m)");
+    plot->setUnitY(0, "Position along y (m)");
+    plot->setLegend(0, 0, "GT");
+    plot->setLegend(0, 1, "Filtered");
+  }
 #endif
 
   // Initialize the simulation
@@ -616,19 +638,39 @@ int main(/*const int argc, const char *argv[]*/)
       ukf.filter(z, dt, cmds[i]);
 
 #ifdef VISP_HAVE_DISPLAY
-      // Plot the filtered state
-      vpColVector Xest = ukf.getXest();
-      plot.plot(0, 1, Xest[0], Xest[1]);
+      if (opt_useDisplay) {
+        // Plot the filtered state
+        vpColVector Xest = ukf.getXest();
+        plot->plot(0, 1, Xest[0], Xest[1]);
+      }
 #endif
     }
 
 #ifdef VISP_HAVE_DISPLAY
+    if (opt_useDisplay) {
     // Plot the ground truth
-    plot.plot(0, 0, robot_pos[0], robot_pos[1]);
+      plot->plot(0, 0, robot_pos[0], robot_pos[1]);
+    }
+#endif
+  }
+
+  if (opt_useDisplay) {
+    std::cout << "Press Enter to quit..." << std::endl;
+    std::cin.get();
+  }
+
+#ifdef VISP_HAVE_DISPLAY
+  if (opt_useDisplay) {
+    delete plot;
+  }
 #endif
+
+  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;
   }
-  std::cout << "Press Enter to quit..." << std::endl;
-  std::cin.get();
   return 0;
 }
 #else
diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp
index 35d1307a22..067ad1b6ad 100644
--- a/example/kalman/ukf-nonlinear-example.cpp
+++ b/example/kalman/ukf-nonlinear-example.cpp
@@ -300,8 +300,27 @@ class vpACSimulator
   vpGaussRand m_rngVel; // Random generator for slight variations of the velocity of the aircraft
 };
 
-int main(/*const int argc, const char *argv[]*/)
+int main(const int argc, const char *argv[])
 {
+  bool opt_useDisplay = true;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg(argv[i]);
+    if (arg == "-d") {
+      opt_useDisplay = false;
+    }
+    else if ((arg == "-h") || (arg == "--help")) {
+      std::cout << "SYNOPSIS" << std::endl;
+      std::cout << "  " << argv[0] << " [-d][-h]" << std::endl;
+      std::cout << std::endl << std::endl;
+      std::cout << "DETAILS" << std::endl;
+      std::cout << "  -d" << std::endl;
+      std::cout << "    Deactivate display." << std::endl;
+      std::cout << std::endl;
+      std::cout << "  -h, --help" << std::endl;
+      return 0;
+    }
+  }
+
   const double dt = 3.; // Period of 3s
   const double sigmaRange = 5; // Standard deviation of the range measurement: 5m
   const double sigmaElevAngle = vpMath::rad(0.5); // Standard deviation of the elevation angle measurent: 0.5deg
@@ -354,35 +373,38 @@ int main(/*const int argc, const char *argv[]*/)
   ukf.setMeasurementResidualFunction(measurementResidual);
 
 #ifdef VISP_HAVE_DISPLAY
+  vpPlot *plot = nullptr;
+  if (opt_useDisplay) {
   // Initialize the plot
-  vpPlot plot(4);
-  plot.initGraph(0, 2);
-  plot.setTitle(0, "Position along X-axis");
-  plot.setUnitX(0, "Time (s)");
-  plot.setUnitY(0, "Position (m)");
-  plot.setLegend(0, 0, "GT");
-  plot.setLegend(0, 1, "Filtered");
-
-  plot.initGraph(1, 2);
-  plot.setTitle(1, "Velocity along X-axis");
-  plot.setUnitX(1, "Time (s)");
-  plot.setUnitY(1, "Velocity (m/s)");
-  plot.setLegend(1, 0, "GT");
-  plot.setLegend(1, 1, "Filtered");
-
-  plot.initGraph(2, 2);
-  plot.setTitle(2, "Position along Y-axis");
-  plot.setUnitX(2, "Time (s)");
-  plot.setUnitY(2, "Position (m)");
-  plot.setLegend(2, 0, "GT");
-  plot.setLegend(2, 1, "Filtered");
-
-  plot.initGraph(3, 2);
-  plot.setTitle(3, "Velocity along Y-axis");
-  plot.setUnitX(3, "Time (s)");
-  plot.setUnitY(3, "Velocity (m/s)");
-  plot.setLegend(3, 0, "GT");
-  plot.setLegend(3, 1, "Filtered");
+    plot = new vpPlot(4);
+    plot->initGraph(0, 2);
+    plot->setTitle(0, "Position along X-axis");
+    plot->setUnitX(0, "Time (s)");
+    plot->setUnitY(0, "Position (m)");
+    plot->setLegend(0, 0, "GT");
+    plot->setLegend(0, 1, "Filtered");
+
+    plot->initGraph(1, 2);
+    plot->setTitle(1, "Velocity along X-axis");
+    plot->setUnitX(1, "Time (s)");
+    plot->setUnitY(1, "Velocity (m/s)");
+    plot->setLegend(1, 0, "GT");
+    plot->setLegend(1, 1, "Filtered");
+
+    plot->initGraph(2, 2);
+    plot->setTitle(2, "Position along Y-axis");
+    plot->setUnitX(2, "Time (s)");
+    plot->setUnitY(2, "Position (m)");
+    plot->setLegend(2, 0, "GT");
+    plot->setLegend(2, 1, "Filtered");
+
+    plot->initGraph(3, 2);
+    plot->setTitle(3, "Velocity along Y-axis");
+    plot->setUnitX(3, "Time (s)");
+    plot->setUnitY(3, "Velocity (m/s)");
+    plot->setLegend(3, 0, "GT");
+    plot->setLegend(3, 1, "Filtered");
+  }
 #endif
 
   // Initialize the simulation
@@ -394,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);
@@ -405,24 +428,45 @@ int main(/*const int argc, const char *argv[]*/)
     vpColVector Xest = ukf.getXest();
 
 #ifdef VISP_HAVE_DISPLAY
+    if (opt_useDisplay) {
     // Plot the ground truth, measurement and filtered state
-    plot.plot(0, 0, i, gt_X[0]);
-    plot.plot(0, 1, i, Xest[0]);
+      plot->plot(0, 0, i, gt_X[0]);
+      plot->plot(0, 1, i, Xest[0]);
 
-    plot.plot(1, 0, i, gt_V[0]);
-    plot.plot(1, 1, i, Xest[1]);
+      plot->plot(1, 0, i, gt_V[0]);
+      plot->plot(1, 1, i, Xest[1]);
 
-    plot.plot(2, 0, i, gt_X[1]);
-    plot.plot(2, 1, i, Xest[2]);
+      plot->plot(2, 0, i, gt_X[1]);
+      plot->plot(2, 1, i, Xest[2]);
 
-    plot.plot(3, 0, i, gt_V[1]);
-    plot.plot(3, 1, i, Xest[3]);
+      plot->plot(3, 0, i, gt_V[1]);
+      plot->plot(3, 1, i, Xest[3]);
+    }
 #endif
 
     gt_Xprec = gt_X;
+    gt_Vprec = gt_V;
+  }
+
+  if (opt_useDisplay) {
+    std::cout << "Press Enter to quit..." << std::endl;
+    std::cin.get();
+  }
+
+#ifdef VISP_HAVE_DISPLAY
+  if (opt_useDisplay) {
+    delete plot;
+  }
+#endif
+
+  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;
   }
-  std::cout << "Press Enter to quit..." << std::endl;
-  std::cin.get();
+
   return 0;
 }
 #else
diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp
index 1d4fe3f49b..05e1b2755b 100644
--- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp
+++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp
@@ -58,8 +58,16 @@ vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::
 
 void vpUnscentedKalman::init(const vpColVector &mu0, const vpMatrix &P0)
 {
+  if ((mu0.getRows() != P0.getCols()) || (mu0.getRows() != P0.getRows())) {
+    throw(vpException(vpException::dimensionError, "Initial state X0 and process covariance matrix P0 sizes mismatch"));
+  }
+  if ((m_Q.getCols() != P0.getCols()) || (m_Q.getRows() != P0.getRows())) {
+    throw(vpException(vpException::dimensionError, "Initial process covariance matrix P0 and Q matrix sizes mismatch"));
+  }
   m_Xest = mu0;
   m_Pest = P0;
+  m_mu = mu0;
+  m_Ppred = P0;
 }
 
 void vpUnscentedKalman::filter(const vpColVector &z, const double &dt, const vpColVector &u)