Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

UKF: Unit tests + fix #1417

Merged
merged 4 commits into from
Jun 18, 2024
Merged

UKF: Unit tests + fix #1417

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a2de711
[FIX] Fix problem due to wrong initialization
Jun 17, 2024
e10a32b
[TEST] Added ukf tests to the unit tests
Jun 17, 2024
9ed7506
[TEST] Improved ukf tests
Jun 17, 2024
41c46be
[EXAMPLE] Moved the wait for user before deletion of displays
Jun 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions example/kalman/CMakeLists.txt
View file
Open in desktop
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
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,27 @@
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")) {

Check warning on line 115 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L115 

Added line #L115 was not covered by tests
std::cout << "SYNOPSIS" << std::endl;
std::cout << " " << argv[0] << " [-d][-h]" << std::endl;

Check warning on line 117 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L117 

Added line #L117 was not covered by tests
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 @@
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");

Check warning on line 175 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L168-L175 

Added lines #L168 - L175 were not covered by tests

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");

Check warning on line 183 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L177-L183 

Added lines #L177 - L183 were not covered by tests

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");

Check warning on line 191 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L185-L191 

Added lines #L185 - L191 were not covered by tests

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");

Check warning on line 199 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L193-L199 

Added lines #L193 - L199 were not covered by tests
}
#endif

// Initialize measurement noise
Expand All @@ -198,32 +220,51 @@
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]);

Check warning on line 227 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L225-L227 

Added lines #L225 - L227 were not covered by tests

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]);

Check warning on line 232 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L229-L232 

Added lines #L229 - L232 were not covered by tests

plot->plot(2, 0, i, gt_X[1]);
plot->plot(2, 1, i, y_meas);
plot->plot(2, 2, i, Xest[2]);

Check warning on line 236 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L234-L236 

Added lines #L234 - L236 were not covered by tests

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]);

Check warning on line 241 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L238-L241 

Added lines #L238 - L241 were not covered by tests
}
#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();

Check warning on line 252 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L252 

Added line #L252 was not covered by tests
}

#ifdef VISP_HAVE_DISPLAY
if (opt_useDisplay) {
delete plot;

Check warning on line 257 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L257 

Added line #L257 was not covered by tests
}
#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;

Check warning on line 265 in example/kalman/ukf-linear-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-linear-example.cpp#L265 

Added line #L265 was not covered by tests
return -1;
}
return 0;
}
#else
Expand Down
74 changes: 58 additions & 16 deletions example/kalman/ukf-nonlinear-complex-example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -387,8 +387,8 @@
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 @@
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")) {

Check warning on line 539 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L539 

Added line #L539 was not covered by tests
std::cout << "SYNOPSIS" << std::endl;
std::cout << " " << argv[0] << " [-d][-h]" << std::endl;

Check warning on line 541 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L541 

Added line #L541 was not covered by tests
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 @@
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");

Check warning on line 624 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L618-L624 

Added lines #L618 - L624 were not covered by tests
}
#endif

// Initialize the simulation
Expand All @@ -616,19 +638,39 @@
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]);

Check warning on line 644 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L644 

Added line #L644 was not covered by tests
}
#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]);

Check warning on line 652 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L652 

Added line #L652 was not covered by tests
}
#endif
}

if (opt_useDisplay) {
std::cout << "Press Enter to quit..." << std::endl;
std::cin.get();

Check warning on line 659 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L659 

Added line #L659 was not covered by tests
}

#ifdef VISP_HAVE_DISPLAY
if (opt_useDisplay) {
delete plot;

Check warning on line 664 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L664 

Added line #L664 was not covered by tests
}
#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;

Check warning on line 671 in example/kalman/ukf-nonlinear-complex-example.cpp

View check run for this annotation

Codecov / codecov/patch

example/kalman/ukf-nonlinear-complex-example.cpp#L671 

Added line #L671 was not covered by tests
return -1;
}
std::cout << "Press Enter to quit..." << std::endl;
std::cin.get();
return 0;
}
#else
Expand Down
Loading
Loading