Skip to content

user defined gate function setup #34

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

Open
jorgenfj wants to merge 1 commit into
base: main
Choose a base branch
from
Open

user defined gate function setup #34

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 1 addition & 1 deletion vortex-filtering/include/vortex_filtering/filters/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -273,7 +273,7 @@ The perceived measurements. This parameter consists of an `Eigen::Vector`. It sh
This will be a short description of what is happening inside the step function. For a more clear and specific explanation of the steps of the PDAF please look into the recommended textbooks.
* The step function will first use the dynamic model to calculate the next state (only considering the given model). Then the sensor model is used to convert the predicted state into the measurement space. That means to get the value, we would measure with our sensor if the perceived state is like the predicted state (of the dynamic model). Concerning our rocket example: We predict the drive temperature with the dynamic model and use the sensor model to convert the drive temperature to the temperature we would measure from the outside.
Both of those steps are done in one line of code by using the EKF explained earlier.
* The second step is the gating of the measurements. This is to exclude measurements that are too far away. So, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered. Additionally, **min_gate_threshold** and **max_gate_threshold** are used here. Since we scale the over-time-changing covariance, we implemented a min and max threshold. If the scaled covariance is too large or too small, we still take measurements in a fixed area into account.
* The second step is the gating of the measurements. This is to exclude measurements that are too far away. So, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered.
* The next step is the update states step. All measurements inside the gate will be compared to the predicted state estimate (by the dynamic model). This results in a Gaussian distribution for all of these measurements.
* In the last step, the weighted average of estimated Gaussian distributions will be calculated. This weighted average will be the final output of the PDAF and is considered to be the current state estimate. Therefore, it will be the previous state estimate in the next iteration.

Expand Down
5 changes: 3 additions & 2 deletions vortex-filtering/include/vortex_filtering/filters/immipda.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,9 @@ class IMMIPDA {
// Gate measurements
Arr_nXb gated_measurements(ImmModelT::N_MODELS, m_k);
[&]<size_t... s_k>(std::index_sequence<s_k...>) {
((gated_measurements.row(s_k) = PDAF_<s_k>::apply_gate(
z_measurements, z_est_preds.at(s_k), {config.pdaf})),
((gated_measurements.row(s_k) =
PDAF_<s_k>::apply_gate(z_measurements, z_est_preds.at(s_k),
config.pdaf.mahalanobis_threshold)),
...);
}(std::make_index_sequence<ImmModelT::N_MODELS>{});

Expand Down
77 changes: 67 additions & 10 deletions vortex-filtering/include/vortex_filtering/filters/ipda.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class IPDA {
using Gauss_x = typename T::Gauss_x;
using Gauss_xX = std::vector<Gauss_x>;

using Vec_z = typename T::Vec_z;
using Arr_zXd = Eigen::Array<double, N_DIM_z, Eigen::Dynamic>;
using Arr_1Xb = Eigen::Array<bool, 1, Eigen::Dynamic>;
using EKF =
Expand Down Expand Up @@ -169,14 +170,18 @@ class IPDA {
* @param z_measurements Array of measurements
* @param existence_prob_pred The predicted existence probability
* @param config Configuration for the IPDA
* @param gate_fn Function to apply the gate
* @return `UpdateResult`
*/
template <typename GateFn>
requires Gate<GateFn, Vec_z, Gauss_z>
static UpdateResult update(const SensModT& sens_mod,
const Gauss_x& x_pred,
const Gauss_z& z_pred,
const Arr_zXd& z_measurements,
double existence_prob_pred,
const Config& config) {
const Config& config,
GateFn&& gate_fn) {
double clutter_intensity = config.pdaf.clutter_intensity;

if (config.ipda.estimate_clutter) {
Expand All @@ -190,7 +195,8 @@ class IPDA {

auto [x_post, x_updates, gated_measurements, z_inside_meas,
z_likelihoods] =
PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg);
PDAF::update(sens_mod, x_pred, z_pred, z_measurements, pdaf_cfg,
std::forward<GateFn>(gate_fn));

double existence_prob_upd = existence_prob_pred;
if (z_measurements.cols() > 0 ||
Expand All @@ -199,12 +205,35 @@ class IPDA {
z_likelihoods, existence_prob_pred, pdaf_cfg.pdaf);
}

return UpdateResult{.x_post = x_post,
.x_updates = x_updates,
.gated_measurements = gated_measurements,
.z_inside_meas = z_inside_meas,
.existence_prob_upd = existence_prob_upd,
.clutter_intensity = clutter_intensity};
return {.x_post = x_post,
.x_updates = x_updates,
.gated_measurements = gated_measurements,
.z_inside_meas = z_inside_meas,
.existence_prob_upd = existence_prob_upd,
.clutter_intensity = clutter_intensity};
}

/**
* @brief Performs one IPDA update step
* @param sens_mod The sensor model
* @param x_pred The predicted state
* @param z_pred The predicted measurement
* @param z_measurements Array of measurements
* @param existence_prob_pred The predicted existence probability
* @param config Configuration for the IPDA
* @param gate_fn Function to apply the gate
* @return `UpdateResult`
*/
static UpdateResult update(const SensModT& sens_mod,
const Gauss_x& x_pred,
const Gauss_z& z_pred,
const Arr_zXd& z_measurements,
double existence_prob_pred,
const Config& config) {
return update(
sens_mod, x_pred, z_pred, z_measurements, existence_prob_pred,
config,
typename PDAF::MahalanobisGate{config.pdaf.mahalanobis_threshold});
}

/**
Expand All @@ -217,22 +246,26 @@ class IPDA {
* @param state_est_prev The previous estimated state
* @param z_measurements Array of measurements
* @param config Configuration for the IPDA
* @param gate_fn Function to apply the gate
* @return `StepResult` The result of the IPDA step and some intermediate
* results
*/
template <typename GateFn>
requires Gate<GateFn, Vec_z, Gauss_z>
static StepResult step(const DynModT& dyn_mod,
const SensModT& sens_mod,
double dt,
const State& state_est_prev,
const Arr_zXd& z_measurements,
const Config& config) {
const Config& config,
GateFn&& gate_fn) {
auto [x_pred, z_pred, existence_prob_pred] =
predict(dyn_mod, sens_mod, dt, state_est_prev, config);

auto [x_post, x_updates, gated_measurements, z_inside_meas,
existence_prob_upd, clutter_intensity] =
update(sens_mod, x_pred, z_pred, z_measurements,
existence_prob_pred, config);
existence_prob_pred, config, std::forward<GateFn>(gate_fn));

// clang-format off
return {
Expand All @@ -249,6 +282,30 @@ class IPDA {
// clang-format on
}

/**
* @brief Perform one step of the Integrated Probabilistic Data Association
* Filter
*
* @param dyn_mod The dynamic model
* @param sens_mod The sensor model
* @param dt Time step in seconds
* @param state_est_prev The previous estimated state
* @param z_measurements Array of measurements
* @param config Configuration for the IPDA
* @return `StepResult` The result of the IPDA step and some intermediate
* results
*/
static StepResult step(const DynModT& dyn_mod,
const SensModT& sens_mod,
double dt,
const State& state_est_prev,
const Arr_zXd& z_measurements,
const Config& config) {
return step(
dyn_mod, sens_mod, dt, state_est_prev, z_measurements, config,
typename PDAF::MahalanobisGate{config.pdaf.mahalanobis_threshold});
}

/**
* @brief Calculates the predicted existence probability
* @param existence_prob_est (r_{k-1}) The previous existence probability.
Expand Down
113 changes: 94 additions & 19 deletions vortex-filtering/include/vortex_filtering/filters/pdaf.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include <Eigen/Dense>
#include <concepts>
#include <iostream>
#include <limits>
#include <memory>
Expand All @@ -14,13 +15,17 @@ namespace vortex::filter {
namespace config {
struct PDAF {
double mahalanobis_threshold = 1.0;
double min_gate_threshold = 0.0;
double max_gate_threshold = std::numeric_limits<double>::max();
double prob_of_detection = 1.0;
double clutter_intensity = 1.0;
};
} // namespace config

/**
* @brief Concept for gating functions used in PDAF
*/
template <typename GateFn, typename Vec_z, typename Gauss_z>
concept Gate = std::predicate<GateFn, const Vec_z&, const Gauss_z&>;

template <concepts::DynamicModelLTVWithDefinedSizes DynModT,
concepts::SensorModelLTVWithDefinedSizes SensModT>
class PDAF {
Expand Down Expand Up @@ -110,6 +115,19 @@ class PDAF {
Arr_1Xb gated_measurements;
};

/**
* @brief Default gating function based on Mahalanobis distance
*/
struct MahalanobisGate {
double threshold;

bool operator()(const Vec_z& z, const Gauss_z& z_pred) const {
double mahal = z_pred.mahalanobis_distance(z);

return mahal <= threshold;
}
};

PDAF() = delete;

/**
Expand Down Expand Up @@ -141,14 +159,19 @@ class PDAF {
* @param z_pred The predicted measurement
* @param z_measurements Array of measurements
* @param config Configuration for the PDAF
* @param gate_fn Function to apply the gate
* @return `UpdateResult`
*/
template <typename GateFn>
requires Gate<GateFn, Vec_z, Gauss_z>
static UpdateResult update(const SensModT& sens_mod,
const Gauss_x& x_pred,
const Gauss_z& z_pred,
const Arr_zXd& z_measurements,
const Config& config) {
auto gated_measurements = apply_gate(z_measurements, z_pred, config);
const Config& config,
GateFn&& gate_fn) {
auto gated_measurements =
apply_gate(z_measurements, z_pred, std::forward<GateFn>(gate_fn));
auto z_inside_meas =
get_inside_measurements(z_measurements, gated_measurements);

Expand All @@ -173,6 +196,25 @@ class PDAF {
};
}

/**
* @brief Perform one PDAF update step
*
* @param sens_mod The sensor model
* @param x_pred The predicted state
* @param z_pred The predicted measurement
* @param z_measurements Array of measurements
* @param config Configuration for the PDAF
* @return `UpdateResult`
*/
static UpdateResult update(const SensModT& sens_mod,
const Gauss_x& x_pred,
const Gauss_z& z_pred,
const Arr_zXd& z_measurements,
const Config& config) {
return update(sens_mod, x_pred, z_pred, z_measurements, config,
MahalanobisGate{config.pdaf.mahalanobis_threshold});
}

/**
* @brief Perform one step of the Probabilistic Data Association Filter
*
Expand All @@ -182,20 +224,24 @@ class PDAF {
* @param x_est The estimated state
* @param z_measurements Array of measurements
* @param config Configuration for the PDAF
* @param gate_fn Function to apply the gate
* @return `StepResult` The result of the PDAF step and some intermediate
* results
*/
template <typename GateFn>
requires Gate<GateFn, Vec_z, Gauss_z>
static StepResult step(const DynModT& dyn_mod,
const SensModT& sens_mod,
double dt,
const Gauss_x& x_est,
const Arr_zXd& z_measurements,
const Config& config) {
const Config& config,
GateFn&& gate_fn) {
auto [x_pred, z_pred] = predict(dyn_mod, sens_mod, dt, x_est);

auto [x_post, x_updates, gated_measurements, z_inside_meas,
z_likelihoods] =
update(sens_mod, x_pred, z_pred, z_measurements, config);
z_likelihoods] = update(sens_mod, x_pred, z_pred, z_measurements,
config, std::forward<GateFn>(gate_fn));
return StepResult{
.x_post = x_post,
.x_pred = x_pred,
Expand All @@ -205,34 +251,63 @@ class PDAF {
};
}

/**
* @brief Perform one step of the Probabilistic Data Association Filter
*
* @param dyn_mod The dynamic model
* @param sens_mod The sensor model
* @param dt Time step in seconds
* @param x_est The estimated state
* @param z_measurements Array of measurements
* @param config Configuration for the PDAF
* @return `StepResult` The result of the PDAF step and some intermediate
* results
*/
static StepResult step(const DynModT& dyn_mod,
const SensModT& sens_mod,
double dt,
const Gauss_x& x_est,
const Arr_zXd& z_measurements,
const Config& config) {
return step(dyn_mod, sens_mod, dt, x_est, z_measurements, config,
MahalanobisGate{config.pdaf.mahalanobis_threshold});
}

/**
* @brief Apply gate to the measurements
*
* @param z_measurements Array of measurements
* @param z_pred Predicted measurement
* @param config Configuration for the PDAF
* @param gate_fn Function to apply the gate
* @return `Arr_1Xb` Indices of the measurements that are inside the gate
*/
template <typename GateFn>
requires Gate<GateFn, Vec_z, Gauss_z>
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;
double max_gate_threshold = config.pdaf.max_gate_threshold;

GateFn&& gate_fn) {
Arr_1Xb gated_measurements(1, z_measurements.cols());

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++) = gate_fn(z_k, z_pred);
}
return gated_measurements;
}

/**
* @brief Apply gate to the measurements
*
* @param z_measurements Array of measurements
* @param z_pred Predicted measurement
* @param config Configuration for the PDAF
* @return `Arr_1Xb` Indices of the measurements that are inside the gate
*/
static Arr_1Xb apply_gate(const Arr_zXd& z_measurements,
const Gauss_z& z_pred,
double threshold) {
return apply_gate(z_measurements, z_pred, MahalanobisGate{threshold});
}

/**
* @brief Get the measurements that are inside the gate
*
Expand Down
Loading