Skip to content

Commit

Permalink
refactored pdaf to work with new type system
Browse files Browse the repository at this point in the history
  • Loading branch information
@EirikKolas
EirikKolas committed Mar 18, 2024
1 parent aeeb0f4 commit 68708dd
Show file tree
Hide file tree
Showing 5 changed files with 139 additions and 154 deletions.
38 changes: 21 additions & 17 deletions vortex-filtering/include/vortex_filtering/filters/ipda.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,20 +7,24 @@

namespace vortex::filter
{
template <models::concepts::DynamicModelLTV DynModT, models::concepts::SensorModelLTV SensModT>
class IPDA
{
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class IPDA {
public:
using SensModI = typename SensModT::SensModI;
using DynModI = typename DynModT::DynModI;
using DynModPtr = std::shared_ptr<DynModI>;
using SensModPtr = std::shared_ptr<SensModI>;
using EKF = vortex::filter::EKF<DynModI, SensModI>;
using Gauss_z = typename SensModI::Gauss_z;
using Gauss_x = typename DynModI::Gauss_x;
using Vec_z = typename SensModI::Vec_z;
using GaussMix = vortex::prob::GaussianMixture<DynModI::N_DIM_x>;
using PDAF = vortex::filter::PDAF<vortex::models::ConstantVelocity<2>, vortex::models::IdentitySensorModel<4, 2>>;
static constexpr int N_DIM_x = DynModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_u = DynModT::N_DIM_u;
static constexpr int N_DIM_v = DynModT::N_DIM_v;
static constexpr int N_DIM_w = SensModT::N_DIM_w;

using T = Types_xzuvw<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;

using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;

using Gauss_z = typename T::Gauss_z;
using Gauss_x = typename T::Gauss_x;
using Vec_z = typename T::Vec_z;
using GaussMix = vortex::prob::GaussianMixture<N_DIM_x>;
using PDAF = vortex::filter::PDAF<vortex::models::ConstantVelocity, vortex::models::IdentitySensorModel<4, 2>>;

IPDA() = delete;

struct Config : public PDAF::Config
Expand All @@ -41,18 +45,18 @@ class IPDA
{
double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps

double summed = 0;
double sum = 0.0;
for (const Vec_z& measurement : measurements)
{
summed += z_pred.pdf(measurement);
sum += z_pred.pdf(measurement);
}
double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * summed;
double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * sum;

return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability);
}

static std::tuple<Gauss_x, double, std::vector<Vec_z>, std::vector<Vec_z>, Gauss_x, Gauss_z, std::vector<Gauss_x>>
step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est,
step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est,
const std::vector<Vec_z>& z_meas, double survive_est, const IPDA::Config& config)
{
auto [x_final, inside, outside, x_pred, z_pred, x_updated] =
Expand Down
99 changes: 40 additions & 59 deletions vortex-filtering/include/vortex_filtering/filters/pdaf.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,90 +7,74 @@
#include <vector>
#include <vortex_filtering/vortex_filtering.hpp>

namespace vortex
{
namespace filter
{
namespace vortex::filter {

using std::string;
// using std::endl;

template <class DynModT, class SensModT>
class PDAF
{
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class PDAF {
public:
using SensModI = typename SensModT::SensModI;
using DynModI = typename DynModT::DynModI;
using DynModPtr = std::shared_ptr<DynModI>;
using SensModPtr = std::shared_ptr<SensModI>;
using EKF = vortex::filter::EKF<DynModI, SensModI>;
using Gauss_z = typename SensModI::Gauss_z;
using Gauss_x = typename DynModI::Gauss_x;
using Vec_z = typename SensModI::Vec_z;
static constexpr int N_DIM_x = DynModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_u = DynModT::N_DIM_u;
static constexpr int N_DIM_v = DynModT::N_DIM_v;
static constexpr int N_DIM_w = SensModT::N_DIM_w;

using T = Types_xzuvw<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;

using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
using Gauss_z = typename T::Gauss_z;
using Gauss_x = typename T::Gauss_x;
using Vec_z = typename T::Vec_z;
using MeasurementsZd = std::vector<Vec_z>;
using StatesXd = std::vector<Gauss_x>;
using GaussMixZd = vortex::prob::GaussianMixture<DynModI::N_DIM_x>;
using StatesXd = std::vector<Gauss_x>;
using GaussMixZd = vortex::prob::GaussianMixture<N_DIM_x>;

struct Config
{
struct Config {
double mahalanobis_threshold = 1.0;
double min_gate_threshold = 0.0;
double max_gate_threshold = HUGE_VAL;
double prob_of_detection = 1.0;
double clutter_intensity = 1.0;
double min_gate_threshold = 0.0;
double max_gate_threshold = HUGE_VAL;
double prob_of_detection = 1.0;
double clutter_intensity = 1.0;
};

PDAF() = delete;

static std::tuple<Gauss_x, MeasurementsZd, MeasurementsZd, Gauss_x, Gauss_z, StatesXd>
step(const DynModPtr& dyn_model, const SensModPtr& sen_model, double timestep, const Gauss_x& x_est,
const MeasurementsZd& z_meas, const Config& config)
step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const MeasurementsZd &z_meas, const Config &config)
{
auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est);
auto [inside, outside] =
apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold);
auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est);
auto [inside, outside] = apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold);

StatesXd x_updated;
for (const auto& measurement : inside)
{
for (const auto &measurement : inside) {
x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement));
}

Gauss_x x_final =
get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity);
return { x_final, inside, outside, x_pred, z_pred, x_updated };
Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity);
return {x_final, inside, outside, x_pred, z_pred, x_updated};
}

static std::tuple<MeasurementsZd, MeasurementsZd> apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred,
double mahalanobis_threshold,
double min_gate_threshold = 0.0,
double max_gate_threshold = HUGE_VAL)
static std::tuple<MeasurementsZd, MeasurementsZd> apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double mahalanobis_threshold,
double min_gate_threshold = 0.0, double max_gate_threshold = HUGE_VAL)
{
MeasurementsZd inside_meas;
MeasurementsZd outside_meas;

for (const auto& measurement : z_meas)
{
for (const auto &measurement : z_meas) {
double mahalanobis_distance = z_pred.mahalanobis_distance(measurement);
double regular_distance = (z_pred.mean() - measurement).norm();
if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) &&
regular_distance <= max_gate_threshold)
{
double regular_distance = (z_pred.mean() - measurement).norm();
if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold) {
inside_meas.push_back(measurement);
}
else
{
else {
outside_meas.push_back(measurement);
}
}

return { inside_meas, outside_meas };
return {inside_meas, outside_meas};
}

// Getting weighted average of the predicted states
static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states,
const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection,
double clutter_intensity)
static Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred,
double prob_of_detection, double clutter_intensity)
{
StatesXd states;
states.push_back(x_pred);
Expand All @@ -104,18 +88,16 @@ class PDAF
}

// Getting association probabilities according to textbook p. 123 "Corollary 7.3.3"
static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection,
double clutter_intensity)
static Eigen::VectorXd get_weights(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity)
{
Eigen::VectorXd weights(z_meas.size() + 1);

// in case no measurement assosiates with the target
double no_association = clutter_intensity * (1 - prob_of_detection);
weights(0) = no_association;
weights(0) = no_association;

// measurements associating with the target
for (size_t k = 1; k < z_meas.size() + 1; k++)
{
for (size_t k = 1; k < z_meas.size() + 1; k++) {
weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1)));
}

Expand All @@ -126,5 +108,4 @@ class PDAF
}
};

} // namespace filter
} // namespace vortex
} // namespace vortex::filter
2 changes: 2 additions & 0 deletions vortex-filtering/test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,9 @@ ament_add_gtest(${PROJECT_NAME}_test
dynamic_model_test.cpp
ekf_test.cpp
imm_test.cpp
ipda_test.cpp
numerical_integration_test.cpp
pdaf_test.cpp
probability_test.cpp
sensor_model_test.cpp
types_test.cpp
Expand Down
10 changes: 6 additions & 4 deletions vortex-filtering/test/ipda_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,11 @@
#include <gtest/gtest.h>
#include <iostream>
#include <vortex_filtering/filters/ipda.hpp>
#include <vortex_filtering/plotting/utils.hpp>
#include <vortex_filtering/utils/plotting.hpp>

using IPDA = vortex::filter::IPDA<vortex::models::ConstantVelocity<2>, vortex::models::IdentitySensorModel<4, 2>>;
using ConstantVelocity = vortex::models::ConstantVelocity;
using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>;
using IPDA = vortex::filter::IPDA<ConstantVelocity, IdentitySensorModel>;

TEST(IPDA, ipda_runs)
{
Expand All @@ -23,8 +25,8 @@ TEST(IPDA, ipda_runs)
std::vector<Eigen::Vector2d> meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 },
{ 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } };

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

auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] =
IPDA::step(dyn_model, sen_model, 1.0, x_est, meas, last_detection_probability, config);
Expand Down
Loading

0 comments on commit 68708dd

Please sign in to comment.