Skip to content

Commit

Permalink
refactored imm model and filter to utilize ekf and ukf changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@EirikKolas
EirikKolas committed Mar 16, 2024
1 parent 6414e89 commit 18f9935
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 24 deletions.
14 changes: 10 additions & 4 deletions vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,15 +192,21 @@ template <concepts::SensorModelWithDefinedSizes SensModT, models::concepts::ImmM
static std::tuple<typename T<i>::Gauss_x, typename T<i>::Gauss_x, Gauss_z> step_kalman_filter(const DynModT<i> &dyn_model, const SensModT &sensor_model,
double dt, const T<i>::Gauss_x &x_est_prev, const Vec_z &z_meas)
{
static constexpr size_t N_DIM_x = ImmModelT::N_DIM_x(i);
static constexpr size_t N_DIM_z = SensModT::N_DIM_z;
static constexpr size_t N_DIM_u = ImmModelT::N_DIM_u(i);
static constexpr size_t N_DIM_v = ImmModelT::N_DIM_v(i);
static constexpr size_t N_DIM_w = SensModT::N_DIM_w;

if constexpr (concepts::DynamicModelLTVWithDefinedSizes<DynModT<i>> && concepts::SensorModelLTVWithDefinedSizes<SensModT>) {
using ImmSensMod = models::ImmSensorModelLTV<ImmModelT::N_DIM_x(i), SensModT>;
using EKF = filter::EKF<DynModT<i>, ImmSensMod>;
using ImmSensMod = models::ImmSensorModelLTV<N_DIM_x, SensModT>;
using EKF = filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
ImmSensMod imm_sens_mod{sensor_model};
return EKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas);
}
else {
using ImmSensMod = models::ImmSensorModel<ImmModelT::N_DIM_x(i), SensModT>;
using UKF = filter::UKF<DynModT<i>, ImmSensMod>;
using ImmSensMod = models::ImmSensorModel<N_DIM_x, SensModT>;
using UKF = filter::UKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
ImmSensMod imm_sens_mod{sensor_model};
return UKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas);
}
Expand Down
40 changes: 20 additions & 20 deletions vortex-filtering/include/vortex_filtering/models/imm_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,6 +195,8 @@ template <vortex::concepts::DynamicModelWithDefinedSizes... DynModels> class Imm
template <size_t i> T<i>::Mat_vv Q_d(double dt, const T<i>::Vec_x &x) const { return get_model<i>().Q_d(dt, x); }

static constexpr int N_DIM_x(size_t model_index) { return N_DIMS_x.at(model_index); }
static constexpr int N_DIM_u(size_t model_index) { return N_DIMS_u.at(model_index); }
static constexpr int N_DIM_v(size_t model_index) { return N_DIMS_v.at(model_index); }

StateNames get_all_state_names() const { return state_names_; }

Expand All @@ -215,65 +217,63 @@ template <vortex::concepts::DynamicModelWithDefinedSizes... DynModels> class Imm
*/
template <size_t n_dim_a, vortex::concepts::SensorModelWithDefinedSizes SensModT> class ImmSensorModel {
public:
static constexpr int N_DIM_x_real = SensModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_w = SensModT::N_DIM_w;
static constexpr int N_DIM_x = SensModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_w = SensModT::N_DIM_w;
static constexpr int N_DIM_a = (int)n_dim_a;
static constexpr int N_DIM_x = N_DIM_a; // For the consept to accept the state dimension of the sensor model (TODO: fix this in the future)

using T = Types_xzwa<N_DIM_x_real, N_DIM_z, N_DIM_w, N_DIM_a>;
using T = Types_xzwa<N_DIM_x, N_DIM_z, N_DIM_w, N_DIM_a>;

ImmSensorModel(SensModT sensor_model)
: sensor_model_(sensor_model)
{
static_assert(N_DIM_a >= SensModT::SensModI::N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model");
}

T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head<N_DIM_x_real>(), w); }
T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head<N_DIM_x>(), w); }

T::Mat_ww R(const T::Vec_x &x) const { return sensor_model_.R(x.template head<N_DIM_x_real>()); }
T::Mat_ww R(const T::Vec_x &x) const { return sensor_model_.R(x.template head<N_DIM_x>()); }

private:
SensModT sensor_model_;
};

template <size_t n_dim_a, vortex::concepts::SensorModelLTVWithDefinedSizes SensModT> class ImmSensorModelLTV {
public:
static constexpr int N_DIM_x_real = SensModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_w = SensModT::N_DIM_w;
static constexpr int N_DIM_x = SensModT::N_DIM_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_w = SensModT::N_DIM_w;
static constexpr int N_DIM_a = (int)n_dim_a;
static constexpr int N_DIM_x = N_DIM_a; // For the consept to accept the state dimension of the sensor model (TODO: fix this in the future)

using T = Types_xzwa<N_DIM_x_real, N_DIM_z, N_DIM_w, N_DIM_a>;
using T = Types_xzwa<N_DIM_x, N_DIM_z, N_DIM_w, N_DIM_a>;

ImmSensorModelLTV(SensModT sensor_model)
: sensor_model_(sensor_model)
{
static_assert(N_DIM_a >= N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model");
}

T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head<N_DIM_x_real>(), w); }
T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head<N_DIM_x>(), w); }

T::Mat_za C(const T::Vec_a &x) const
{
typename T::Mat_za C_a = T::Mat_za::Zero();
C_a.template leftCols<N_DIM_x_real>() = sensor_model_.C(x.template head<N_DIM_x_real>());
typename T::Mat_za C_a = T::Mat_za::Zero();
C_a.template leftCols<N_DIM_x>() = sensor_model_.C(x.template head<N_DIM_x>());
return C_a;
}

T::Mat_zw H(const T::Vec_a &x) const { return sensor_model_.H(x.template head<N_DIM_x_real>()); }
T::Mat_zw H(const T::Vec_a &x) const { return sensor_model_.H(x.template head<N_DIM_x>()); }

T::Mat_ww R(const T::Vec_a &x) const { return sensor_model_.R(x.template head<N_DIM_x_real>()); }
T::Mat_ww R(const T::Vec_a &x) const { return sensor_model_.R(x.template head<N_DIM_x>()); }

T::Gauss_z pred_from_est(const T::Gauss_a &x_est) const
{
typename T::Vec_x mean = x_est.mean().template head<N_DIM_x_real>();
typename T::Mat_xx cov = x_est.cov().template topLeftCorner<N_DIM_x_real, N_DIM_x_real>();
typename T::Vec_x mean = x_est.mean().template head<N_DIM_x>();
typename T::Mat_xx cov = x_est.cov().template topLeftCorner<N_DIM_x, N_DIM_x>();
return sensor_model_.pred_from_est({mean, cov});
}

T::Gauss_z pred_from_state(const T::Vec_a &x) const { return sensor_model_.pred_from_state(x.template head<N_DIM_x_real>()); }
T::Gauss_z pred_from_state(const T::Vec_a &x) const { return sensor_model_.pred_from_state(x.template head<N_DIM_x>()); }

private:
SensModT sensor_model_;
Expand Down

0 comments on commit 18f9935

Please sign in to comment.