Skip to content

Commit

Permalink
[filter] order operator arguments
Browse files Browse the repository at this point in the history
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge committed Sep 8, 2022
1 parent 1bea992 commit b33255d
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 72 deletions.
16 changes: 8 additions & 8 deletions include/fcarouge/internal/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,9 +191,9 @@ struct kalman<State, Output, void, Transpose, Symmetrize, Divide, Identity,
}

template <typename... Outputs>
inline constexpr void operator()(const PredictionTypes &...prediction_pack,
const UpdateTypes &...update_pack,
const Outputs &...output_z) {
inline constexpr void operator()(const UpdateTypes &...update_pack,
const Outputs &...output_z,
const PredictionTypes &...prediction_pack) {
update(update_pack..., output_z...);
predict(prediction_pack...);
}
Expand Down Expand Up @@ -357,11 +357,11 @@ struct kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
p = symmetrize(estimate_uncertainty{f * p * transpose(f) + q});
}

template <typename... Inputs>
inline constexpr void operator()(const PredictionTypes &...prediction_pack,
const UpdateTypes &...update_pack,
const Inputs &...input_u,
const auto &...output_z) {
template <typename... Outputs>
inline constexpr void operator()(const UpdateTypes &...update_pack,
const Outputs &...output_z,
const PredictionTypes &...prediction_pack,
const auto &...input_u) {
update(update_pack..., output_z...);
predict(prediction_pack..., input_u...);
}
Expand Down
54 changes: 24 additions & 30 deletions include/fcarouge/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -881,43 +881,37 @@ class kalman final {

//! @brief Runs a full step of the filter.
//!
//! @details Predicts and updates the estimates per arguments,
//! control input, and measurement output.
//! @details Updates and predict the estimates per update arguments,
//! measurement output, prediction arguments, and control input.
//!
//! @tparam InputTypes The template parameter pack types passed in the
//! `arguments` parameters after the update arguments and before the output
//! arguments. `InputTypes` must be compatible with the `Input` type template
//! parameter of the control u.
//! @tparam Outputs The template parameter pack types passed in the
//! `arguments` parameters after the update arguments and before the
//! prediction arguments. The `Outputs` types must be compatible with the
//! `Output` type template parameter of the measurement output z.
//!
//! @param arguments The prediction, update, input, and output parameters of
//! @param arguments The update, output, prediction, and input parameters of
//! the filter, in that order. The arguments need to be compatible with the
//! filter types. The prediction parameters convertible to the
//! `PredictionTypes` template pack types are passed through for computations
//! of prediction matrices. The update parameters convertible to the
//! filter types. The update parameters convertible to the
//! `UpdateTypes` template pack types are passed through for computations of
//! update matrices. The control parameter pack types convertible to the
//! `Input` template type. The observation parameter pack types convertible to
//! the `Output` template type. The update and prediction types are explicitly
//! defined with the class definition and the observation parameter pack types
//! are always deduced per the greedy matching rule. However the control
//! parameter pack types must always be explicitly defined per the fair
//! matching rule.
//!
//! @note Called as `k(...);` with prediction values and output values when
//! the filter has no input parameters. The input type list is explicitly
//! empty. Otherwise can be called as `k.template operator()<input1_t,
//! input2_t, ...>(...);` with prediction values, input values, and output
//! values. The input type list being explicitly specified per the fair
//! matching rule. A lambda can come in handy to reduce the verbose call
//! `const auto kf{ [&k](const auto
//! &...args) { k.template operator()<input1_t, input2_t,
//! ...>(args...); } };` then called as `kf(...);`.
//! update matrices. The observation parameter pack types convertible to
//! the `Output` template type. The prediction parameters convertible to the
//! `PredictionTypes` template pack types are passed through for computations
//! of prediction matrices. The control parameter pack types convertible to
//! the `Input` template type. The update and prediction arguments types are
//! explicitly defined with the class definition and the control input
//! parameter pack types are always deduced per the parameter pack greedy
//! matching rule. However the measurement output parameter pack types must
//! always be explicitly defined per the parameter pack fair matching rule.
//!
//! @note Call as `k.template operator()<output1_t, output2_t, ...>(...);` to
//! lift ambiguity. A lambda can come in handy to reduce the verbose call
//! `const auto kf{ [&k](const auto &...args) { k.template
//! operator()<output1_t, output2_t, ...>(args...); } };` then called as
//! `kf(...);`.
//!
//! @todo Consider if returning the state column vector X would be preferable?
//! Or fluent interface? Would be compatible with an ES-EKF implementation?
//! @todo Understand why the implementation cannot be moved out of the class.
//! @todo What should be the order of the parameters? Update first?
template <typename... InputTypes>
template <typename... Outputs>
inline constexpr void operator()(const auto &...arguments);

//! @}
Expand Down
11 changes: 5 additions & 6 deletions sample/ekf_4x1x0_ardupilot_soaring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,18 +96,17 @@ namespace {
return h;
});

// Measure and Update
float drift_x{0};
float drift_y{0};
k.predict(drift_x, drift_y);

float variometer{0};
float position_x{0};
float position_y{0};
k.update(variometer, position_x, position_y);

float drift_x{0};
float drift_y{0};
k.predict(drift_x, drift_y);

// Or so on.
k(drift_x, drift_y, position_x, position_y, variometer);
k(position_x, position_y, variometer, drift_x, drift_y);

return 0;
}()};
Expand Down
56 changes: 28 additions & 28 deletions sample/kf_2x1x1_rocket_altitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ namespace {
const auto step{
[&k](const auto &...args) { k.template operator()<double>(args...); }};

step(delta_time, 40.02 + gravity, -11.1);
step(-11.1, delta_time, 40.02 + gravity);

assert(std::abs(1 - k.x()[0] / -12.3) < 0.002 &&
std::abs(1 - k.x()[1] / 14.8) < 0.002 &&
Expand All @@ -151,33 +151,33 @@ namespace {
std::abs(1 - k.p()(1, 1) / 438.8) < 0.001 &&
"The estimate uncertainty expected at 0.1% accuracy.");

step(delta_time, 39.97 + gravity, 18.);
step(delta_time, 39.81 + gravity, 22.9);
step(delta_time, 39.75 + gravity, 19.5);
step(delta_time, 39.6 + gravity, 28.5);
step(delta_time, 39.77 + gravity, 46.5);
step(delta_time, 39.83 + gravity, 68.9);
step(delta_time, 39.73 + gravity, 48.2);
step(delta_time, 39.87 + gravity, 56.1);
step(delta_time, 39.81 + gravity, 90.5);
step(delta_time, 39.92 + gravity, 104.9);
step(delta_time, 39.78 + gravity, 140.9);
step(delta_time, 39.98 + gravity, 148.);
step(delta_time, 39.76 + gravity, 187.6);
step(delta_time, 39.86 + gravity, 209.2);
step(delta_time, 39.61 + gravity, 244.6);
step(delta_time, 39.86 + gravity, 276.4);
step(delta_time, 39.74 + gravity, 323.5);
step(delta_time, 39.87 + gravity, 357.3);
step(delta_time, 39.63 + gravity, 357.4);
step(delta_time, 39.67 + gravity, 398.3);
step(delta_time, 39.96 + gravity, 446.7);
step(delta_time, 39.8 + gravity, 465.1);
step(delta_time, 39.89 + gravity, 529.4);
step(delta_time, 39.85 + gravity, 570.4);
step(delta_time, 39.9 + gravity, 636.8);
step(delta_time, 39.81 + gravity, 693.3);
step(delta_time, 39.81 + gravity, 707.3);
step(18., delta_time, 39.97 + gravity);
step(22.9, delta_time, 39.81 + gravity);
step(19.5, delta_time, 39.75 + gravity);
step(28.5, delta_time, 39.6 + gravity);
step(46.5, delta_time, 39.77 + gravity);
step(68.9, delta_time, 39.83 + gravity);
step(48.2, delta_time, 39.73 + gravity);
step(56.1, delta_time, 39.87 + gravity);
step(90.5, delta_time, 39.81 + gravity);
step(104.9, delta_time, 39.92 + gravity);
step(140.9, delta_time, 39.78 + gravity);
step(148., delta_time, 39.98 + gravity);
step(187.6, delta_time, 39.76 + gravity);
step(209.2, delta_time, 39.86 + gravity);
step(244.6, delta_time, 39.61 + gravity);
step(276.4, delta_time, 39.86 + gravity);
step(323.5, delta_time, 39.74 + gravity);
step(357.3, delta_time, 39.87 + gravity);
step(357.4, delta_time, 39.63 + gravity);
step(398.3, delta_time, 39.67 + gravity);
step(446.7, delta_time, 39.96 + gravity);
step(465.1, delta_time, 39.8 + gravity);
step(529.4, delta_time, 39.89 + gravity);
step(570.4, delta_time, 39.85 + gravity);
step(636.8, delta_time, 39.9 + gravity);
step(693.3, delta_time, 39.81 + gravity);
step(707.3, delta_time, 39.81 + gravity);

k.update(748.5);

Expand Down

0 comments on commit b33255d

Please sign in to comment.