Skip to content

Commit

Permalink
✅ added more tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@TristanWolfram
TristanWolfram committed Jan 30, 2024
1 parent f7196b1 commit 52739b5
Show file tree
Hide file tree
Showing 2 changed files with 293 additions and 3 deletions.
1 change: 1 addition & 0 deletions vortex-filtering/include/vortex_filtering/filters/pdaf.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ class PDAF {
x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement));
}

// TODO: return gaussian mixture
Gauss_x x_final = get_weighted_average(
z_meas,
x_updated,
Expand Down
295 changes: 292 additions & 3 deletions vortex-filtering/test/pdaf_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ TEST(PDAF, apply_gate_is_calculating)

TEST(PDAF, apply_gate_is_separating_correctly)
{
double gate_threshold = 2;
double gate_threshold = 3;
SimplePDAF pdaf;

Eigen::Matrix2d cov;
Expand Down Expand Up @@ -266,6 +266,24 @@ TEST(PDAF, apply_gate_is_separating_correctly_2)

EXPECT_EQ(inside.size(), 5u);
EXPECT_EQ(outside.size(), 1u);

Gnuplot gp;
gp << "set xrange [-8:8]\nset yrange [-8:8]\n";
gp << "set size ratio -1\n";
gp << "set style circle radius 0.05\n";
gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n";
gp.send1d(meas);

int object_counter = 0;

gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n";
gp << "replot\n";

vortex::plotting::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold);

gp << "set object " << ++object_counter << " ellipse center " << prediction.x << "," << prediction.y << " size " << prediction.a << "," << prediction.b << " angle " << prediction.angle
<< "fs empty border lc rgb 'cyan'\n";
gp << "replot\n";
}

// testing the predict_next_state function
Expand Down Expand Up @@ -313,7 +331,7 @@ TEST(PDAF, predict_next_state_is_calculating)
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n";
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}
Expand Down Expand Up @@ -376,7 +394,7 @@ TEST(PDAF, predict_next_state_2)
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " front fc rgb 'skyblue' fs transparent solid 0.2 noborder\n";
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}
Expand All @@ -393,4 +411,275 @@ TEST(PDAF, predict_next_state_2)
<< " fs empty border lc rgb 'cyan'\n";

gp << "replot\n";
}

TEST(PDAF, predict_next_state_3_1)
{

double gate_threshold = 4;
double prob_of_detection = 0.9;
double clutter_intensity = 1.0;
SimplePDAF pdaf;

vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity());
std::vector<Eigen::Vector2d> meas = {{0.0, 0.5}, {0.2, 0.2}, {0.8, 2.3}, {0.5, 0.0}, {4.2, 2.7}, {1.4, 2.5}};

auto dyn_model = std::make_shared<vortex::models::ConstantVelocity<2>>(0.5);
auto sen_model = std::make_shared<vortex::models::IdentitySensorModel<4, 2>>(1.0);

auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(
x_est,
meas,
1.0,
dyn_model,
sen_model,
gate_threshold,
prob_of_detection,
clutter_intensity);
std::cout << "x_final: " << x_final.mean() << std::endl;

Gnuplot gp;
gp << "set xrange [-8:8]\nset yrange [-8:8]\n";
gp << "set size ratio -1\n";

gp << "set style circle radius 0.05\n";
gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n";
gp.send1d(meas);

int object_counter = 0;

for (const auto& state: x_updated)
{
vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2));
vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss);

gp << "set object " << ++object_counter
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}

gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n";
gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n";
gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n";
gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n";

gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n";
gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n";

vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold);
gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle
<< " fs empty border lc rgb 'cyan'\n";

gp << "replot\n";
}

TEST(PDAF, predict_next_state_3_2)
{

double gate_threshold = 4;
double prob_of_detection = 0.9;
double clutter_intensity = 1.0;
SimplePDAF pdaf;

vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity());
std::vector<Eigen::Vector2d> meas = {{-0.5, 2.0}, {-0.23, 0.5}, {-2.0, 3.4}, {0.0, 1.3}, {0.14, 0.5}, {-2.5, 0.89}};

auto dyn_model = std::make_shared<vortex::models::ConstantVelocity<2>>(0.5);
auto sen_model = std::make_shared<vortex::models::IdentitySensorModel<4, 2>>(1.0);

auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(
x_est,
meas,
1.0,
dyn_model,
sen_model,
gate_threshold,
prob_of_detection,
clutter_intensity);
std::cout << "x_final: " << x_final.mean() << std::endl;

Gnuplot gp;
gp << "set xrange [-8:8]\nset yrange [-8:8]\n";
gp << "set size ratio -1\n";

gp << "set style circle radius 0.05\n";
gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n";
gp.send1d(meas);

int object_counter = 0;

for (const auto& state: x_updated)
{
vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2));
vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss);

gp << "set object " << ++object_counter
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}

gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n";
gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n";
gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n";

gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n";
gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n";

// old state from 3_1
gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n";

vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold);
gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle
<< " fs empty border lc rgb 'cyan'\n";

gp << "replot\n";
}

TEST(PDAF, predict_next_state_3_3)
{

double gate_threshold = 4;
double prob_of_detection = 0.9;
double clutter_intensity = 1.0;
SimplePDAF pdaf;

vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity());
std::vector<Eigen::Vector2d> meas = {{-1.5, 2.5}, {-1.2, 2.7}, {-0.8, 2.3},{-1.7, 1.9}, {-2.0, 3.0}, {-1.11, 2.04}};

auto dyn_model = std::make_shared<vortex::models::ConstantVelocity<2>>(0.5);
auto sen_model = std::make_shared<vortex::models::IdentitySensorModel<4, 2>>(1.0);

auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(
x_est,
meas,
1.0,
dyn_model,
sen_model,
gate_threshold,
prob_of_detection,
clutter_intensity);
std::cout << "x_final: " << x_final.mean() << std::endl;

Gnuplot gp;
gp << "set xrange [-8:8]\nset yrange [-8:8]\n";
gp << "set size ratio -1\n";

gp << "set style circle radius 0.05\n";
gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n";
gp.send1d(meas);

int object_counter = 0;

for (const auto& state: x_updated)
{
vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2));
vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss);

gp << "set object " << ++object_counter
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}

gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n";
gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n";
gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n";

gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n";
gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n";

// old state from 3_1, 3_2
gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n";
gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n";

vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold);
gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle
<< " fs empty border lc rgb 'cyan'\n";

gp << "replot\n";
}

TEST(PDAF, predict_next_state_3_4)
{

double gate_threshold = 4;
double prob_of_detection = 0.9;
double clutter_intensity = 1.0;
SimplePDAF pdaf;

vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity());
std::vector<Eigen::Vector2d> meas = {{-2.0, 2.2}, {-1.8, 2.3}, {-2.3, 2.0}, {0.6, 1.5}, {-2.0, 2.0}, {-1.4, 2.5}};

auto dyn_model = std::make_shared<vortex::models::ConstantVelocity<2>>(0.5);
auto sen_model = std::make_shared<vortex::models::IdentitySensorModel<4, 2>>(1.0);

auto [x_final, inside, outside, x_pred, z_pred, x_updated] = pdaf.predict_next_state(
x_est,
meas,
1.0,
dyn_model,
sen_model,
gate_threshold,
prob_of_detection,
clutter_intensity);
std::cout << "x_final: " << x_final.mean() << std::endl;

Gnuplot gp;
gp << "set xrange [-8:8]\nset yrange [-8:8]\n";
gp << "set size ratio -1\n";

gp << "set style circle radius 0.05\n";
gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n";
gp.send1d(meas);

int object_counter = 0;

for (const auto& state: x_updated)
{
vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2,2));
vortex::plotting::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss);

gp << "set object " << ++object_counter
<< " ellipse center " << ellipse.x << "," << ellipse.y
<< " size " << ellipse.a << "," << ellipse.b
<< " angle " << ellipse.angle
<< " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n";

gp << "set object " << ++object_counter << " circle center " << ellipse.x << "," << ellipse.y << " size " << 0.02 << " fs empty border lc rgb 'blue'\n";
}

gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'black'\n";
gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'pink'\n";
gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 << " fs empty border lc rgb 'green'\n";

gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) << " nohead lc rgb 'pink'\n";
gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n";

// old state from 3_1, 3_2, 3_3
gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n";
gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n";
gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n";
gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n";

vortex::plotting::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, gate_threshold);
gp << "set object " << ++object_counter << " ellipse center " << gate.x << "," << gate.y << " size " << gate.a << "," << gate.b << " angle " << gate.angle
<< " fs empty border lc rgb 'cyan'\n";

gp << "replot\n";
}

0 comments on commit 52739b5

Please sign in to comment.