diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index 518ef8d3b..64d97f535 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -139,12 +139,10 @@ struct kalman, //! cases? //! @todo Do we want to pass z to `observation()`? What are the use cases? //! @todo Use operator `+=` for the state update? - template inline constexpr void update(const UpdateTypes &...update_pack, - const Output0 &output_z, - const OutputN &...outputs_z) { + const auto &...output_pack_z) { update_arguments = {update_pack...}; - z = output{output_z, outputs_z...}; + z = output{output_pack_z...}; h = observation_state_h(x, update_pack...); r = noise_observation_r(x, z, update_pack...); s = innovation_uncertainty{h * p * t(h) + r}; @@ -257,12 +255,10 @@ struct kalman, //! @todo Do we want to pass z to `observation_state_h()`? What are the use //! cases? //! @todo Do we want to pass z to `observation()`? What are the use cases? - template inline constexpr void update(const UpdateTypes &...update_pack, - const Output0 &output_z, - const OutputN &...outputs_z) { + const auto &...output_pack_z) { update_arguments = {update_pack...}; - z = output{output_z, outputs_z...}; + z = output{output_pack_z...}; h = observation_state_h(x, update_pack...); r = noise_observation_r(x, z, update_pack...); s = h * p * t(h) + r; @@ -280,18 +276,30 @@ struct kalman, //! @todo Do we want to pass u to `noise_process_q()`? What are the use cases? //! @todo Do we want to pass x, u to `transition_control_g()`? What are the //! use cases? - template inline constexpr void predict(const PredictionTypes &...prediction_pack, - const Input0 &input_u, - const InputN &...inputs_u) { + const auto &...input_pack_u) + requires(sizeof...(PredictionTypes) > 0) + { prediction_arguments = {prediction_pack...}; - u = input{input_u, inputs_u...}; + u = input{input_pack_u...}; f = transition_state_f(x, u, prediction_pack...); q = noise_process_q(x, prediction_pack...); g = transition_control_g(prediction_pack...); x = transition(x, u, prediction_pack...); p = estimate_uncertainty{f * p * t(f) + q}; } + + //! @todo Collapse the two predict implementations when MSVC supports it. + inline constexpr void predict(const auto &...input_pack_u) + requires(sizeof...(PredictionTypes) == 0) + { + u = input{input_pack_u...}; + f = transition_state_f(x, u); + q = noise_process_q(x); + g = transition_control_g(); + x = transition(x, u); + p = estimate_uncertainty{f * p * t(f) + q}; + } }; } // namespace fcarouge::internal