Skip to content

Commit

Permalink
fixed tests. Missing documentation
Browse files Browse the repository at this point in the history
  • Loading branch information
@EirikKolas
EirikKolas committed Jul 4, 2024
1 parent 254c8f4 commit 71655d8
Show file tree
Hide file tree
Showing 3 changed files with 143 additions and 262 deletions.
52 changes: 10 additions & 42 deletions vortex-filtering/include/vortex_filtering/filters/ipda.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,11 @@
/**
* @file ipda.hpp
* @author Tristan Wolfram
* @brief File for the IPDA filter
* @version 1.0
* @date 2024-05-07
*
* @copyright Copyright (c) 2024
*
*/
#pragma once
#include <vector>
#include <Eigen/Dense>
#include <vector>
#include <vortex_filtering/filters/pdaf.hpp>
#include <vortex_filtering/models/dynamic_model_interfaces.hpp>
#include <vortex_filtering/models/sensor_model_interfaces.hpp>

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

namespace config {
struct IPDA {
Expand All @@ -26,14 +15,7 @@ struct IPDA {
};
} // namespace config

/**
* @brief The IPDA filter class
* @tparam DynModT The dynamic model type
* @tparam SensModT The sensor model type
*/
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
class IPDA
{
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class IPDA {
public:
static constexpr int N_DIM_x = DynModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
Expand All @@ -43,10 +25,10 @@ class IPDA

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

using Arr_zXd = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
using Arr_1Xb = Eigen::Array<bool, 1, Eigen::Dynamic>;
using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
using PDAF = vortex::filter::PDAF<DynModT, SensModT>;
using Arr_zXd = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
using Arr_1Xb = Eigen::Array<bool, 1, Eigen::Dynamic>;
using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
using PDAF = vortex::filter::PDAF<DynModT, SensModT>;

IPDA() = delete;

Expand Down Expand Up @@ -143,22 +125,8 @@ class IPDA
return 1 / V_k * (m_k - r_k * P_d); // (7.31)
}

/**
* @brief The IPDAF step function. Gets following parameters and calculates the next state with the probablity of
* existence.
* @param dyn_model The dynamic model.
* @param sen_model The sensor model.
* @param timestep The timestep.
* @param x_est The estimated state.
* @param z_meas The percieved measurements.
* @param existence_prob The estimated survival probability (current state).
* @param config configuration data - see Config struct of PDAF.
* @return A tuple containing the final state, the existence probability, the inside (of the gate) measurements, the
* outside (of the gate) measurements, the predicted state, the predicted measurements, and the updated state.
*/
static std::tuple<Gauss_x, double, std::vector<Vec_z>, std::vector<Vec_z>, Gauss_x, Gauss_z, std::vector<Gauss_x>>
step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const Gauss_x &x_est, const std::vector<Vec_z> &z_meas, double existence_prob_est,
IPDA::Config &config)
static Output step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_zXd &z_measurements,
Config &config)
{
double existence_prob_pred = existence_prediction(state_est_prev.existence_probability, config.ipda.prob_of_survival);

Expand Down Expand Up @@ -191,4 +159,4 @@ class IPDA
// clang-format on
}
};
} // namespace vortex::filter
} // namespace vortex::filter
137 changes: 30 additions & 107 deletions vortex-filtering/include/vortex_filtering/filters/pdaf.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,3 @@
/**
* @file ipda.hpp
* @author Tristan Wolfram
* @brief File for the PDAF filter
* @version 1.0
* @date 2024-05-07
*
* @copyright Copyright (c) 2024
*
*/
#pragma once

#include <Eigen/Dense>
Expand All @@ -19,6 +9,7 @@
#include <vector>
#include <vortex_filtering/vortex_filtering.hpp>

namespace vortex::filter {

namespace config {
struct PDAF {
Expand All @@ -30,16 +21,6 @@ struct PDAF {
};
} // namespace config

namespace vortex::filter
{
/**
* @brief The PDAF filter class
* @tparam DynModT The dynamic model
* @tparam SensModT The sensor model
*/
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
class PDAF
{
template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class PDAF {
public:
static constexpr int N_DIM_x = DynModT::N_DIM_x;
Expand All @@ -54,27 +35,14 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
using Gauss_x = typename T::Gauss_x;
using Vec_z = typename T::Vec_z;
using GaussMix_x = typename T::GaussMix_x;
using GaussMix_z = typename T::GaussMix_z;
using GaussMix_z = typename T::GaussMix_z;
using Arr_zXd = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
using Arr_1Xb = Eigen::Array<bool, 1, Eigen::Dynamic>;
using Gauss_xX = std::vector<Gauss_x>;
using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;

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;
};

struct Output {
Gauss_x x_;
Gauss_x x_prediction;
Gauss_z z_prediction;
Gauss_xX x_updates;
Arr_1Xb gated_measurements;
struct Config {
config::PDAF pdaf;
};

struct Output {
Expand All @@ -87,56 +55,23 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod

PDAF() = delete;

/**
* @brief The step function for the PDAF filter
* @param dyn_model The dynamic model
* @param sen_model The sensor model
* @param timestep The timestep
* @param x_est The estimated state (current state)
* @param z_meas The measurements
* @param config The configuration
* @return A tuple containing the (new) estimated state, the measurements that were inside the gate, the measurements
* that were outside the gate, the predicted state (from dynamic model), the predicted measurement (from sensor
* model), and the updated states (comparisson of prediced state and actual measurements)
*/
static std::tuple<Gauss_x, MeasurementsZd, MeasurementsZd, Gauss_x, Gauss_z, StatesXd>
step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est,
const MeasurementsZd& z_meas, const Config& config)
static Output step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const Arr_zXd &z_measurements,
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);

StatesXd x_updated;
for (const auto& measurement : inside)
{
x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement));
auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est);
auto gated_measurements = apply_gate(z_measurements, z_pred, config);
auto inside_meas = get_inside_measurements(z_measurements, gated_measurements);

Gauss_xX x_updated;
for (const auto &z_k : inside_meas.colwise()) {
x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k));
}

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_meas, x_updated, z_pred, x_pred, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity);
return {x_final, x_pred, z_pred, x_updated, gated_measurements};
}

/**
* @brief Applies the gate to the measurements
* @param z_meas The measurements
* @param z_pred The predicted measurement
* @param mahalanobis_threshold The threshold for the Mahalanobis distance
* @param min_gate_threshold The minimum threshold for the gate
* @param max_gate_threshold The maximum threshold for the gate
* @return A tuple containing the measurements that were inside the gate and the measurements that were outside the
* gate
*/
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 Arr_1Xb apply_gate(const Arr_zXd &z_measurements, const Gauss_z &z_pred, Config config)
{
double mahalanobis_threshold = config.pdaf.mahalanobis_threshold;
double min_gate_threshold = config.pdaf.min_gate_threshold;
Expand All @@ -147,27 +82,27 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
for (size_t a_k = 0; const Vec_z &z_k : z_measurements.colwise()) {
double mahalanobis_distance = z_pred.mahalanobis_distance(z_k);
double regular_distance = (z_pred.mean() - z_k).norm();
gated_measurements(a_k++) = (mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold;
gated_measurements(a_k++) =
(mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold;
}
return gated_measurements;
}

return { inside_meas, outside_meas };
static Arr_zXd get_inside_measurements(const Arr_zXd &z_measurements, const Arr_1Xb &gated_measurements)
{
Arr_zXd inside_meas(N_DIM_z, gated_measurements.count());
for (size_t i = 0, j = 0; bool gated : gated_measurements) {
if (gated) {
inside_meas.col(j++) = z_measurements.col(i);
}
i++;
}
return inside_meas;
}

/**
* @brief Calculates the weighted average of the measurements
* @param z_meas The measurements
* @param updated_states The updated states
* @param z_pred The predicted measurement
* @param x_pred The predicted state
* @param prob_of_detection The probability of detection
* @param clutter_intensity The clutter intensity
* @return The weighted average of the measurements
*/
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)
// Getting weighted average of the predicted states
static Gauss_x get_weighted_average(const Arr_zXd &z_measurements, const Gauss_xX &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred,
double prob_of_detection, double clutter_intensity)
{
Gauss_xX states;
states.push_back(x_pred);
Expand All @@ -178,19 +113,8 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
return GaussMix_x{weights, states}.reduce();
}

/**
* @brief Gets the weights for the measurements -> see association probabilities in textbook p. 123
* "Corollary 7.3.3
* @param z_meas The measurements
* @param z_pred The predicted measurement
* @param prob_of_detection The probability of detection
* @param clutter_intensity The clutter intensity
* @return The weights for the measurements -> same order as in z_meas
* The first element is the weight for no association
* length of weights = z_meas.size() + 1
*/
static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection,
double clutter_intensity)
// Getting association probabilities according to textbook p. 123 "Corollary 7.3.3"
static Eigen::VectorXd get_weights(const Arr_zXd &z_measurements, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity)
{
double lambda = clutter_intensity;
double P_d = prob_of_detection;
Expand Down Expand Up @@ -220,15 +144,14 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
Eigen::ArrayXd weights(m_k + 1);

// Accociation probabilities (Corrolary 7.3.3)
weights(0) = lambda * (1 - P_d);
weights(0) = lambda * (1 - P_d);
weights.tail(m_k) = P_d * z_likelyhoods;

// normalize weights
weights /= weights.sum();

return weights;

}
};

} // namespace vortex::filter
} // namespace vortex::filter
Loading

0 comments on commit 71655d8

Please sign in to comment.