Skip to content

Commit

Permalink
Merge pull request #1417 from rolalaro/feat_ukf_improvements
Browse files Browse the repository at this point in the history
UKF: Unit tests  + fix
  • Loading branch information
fspindle authored Jun 18, 2024
2 parents 8283097 + 41c46be commit c31996f
Show file tree
Hide file tree
Showing 5 changed files with 246 additions and 107 deletions.
4 changes: 4 additions & 0 deletions example/kalman/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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})
145 changes: 93 additions & 52 deletions example/kalman/ukf-linear-example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
74 changes: 58 additions & 16 deletions example/kalman/ukf-nonlinear-complex-example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{ }

/**
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
Loading

0 comments on commit c31996f

Please sign in to comment.