Skip to content

Commit

Permalink
[filter] renaming state variable (#8)
Browse files Browse the repository at this point in the history
  • Loading branch information
FrancoisCarouge authored Apr 16, 2022
1 parent 5f0f29c commit a69392e
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 36 deletions.
59 changes: 30 additions & 29 deletions include/fcarouge/kalman.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,56 +69,57 @@ template <typename State, typename Output = State, typename Input = State,
class kalman
{
public:
using state_x = State;
using output_z = Output;
using input_u = Input;
using estimate_uncertainty_p =
using state = State;
using output = Output;
using input = Input;
using estimate_uncertainty =
std::invoke_result_t<Divide<State, State>, State, State>;
using process_noise_uncertainty_q =
using process_noise_uncertainty =
std::invoke_result_t<Divide<State, State>, State, State>;
using state_transition_f =
using state_transition =
std::invoke_result_t<Divide<State, State>, State, State>;
using observation_h =
using observation =
std::invoke_result_t<Divide<Output, State>, Output, State>;
using measurement_uncertainty_r =
using measurement_uncertainty =
std::invoke_result_t<Divide<Output, Output>, Output, Output>;
using control_g = std::invoke_result_t<Divide<State, Output>, State, Output>;
using control = std::invoke_result_t<Divide<State, Output>, State, Output>;

state_x state;
estimate_uncertainty_p estimate_uncertainty;
state state_x;
estimate_uncertainty estimate_uncertainty_p;

// Functors could be replaced by the standard general-purpose polymorphic
// function wrapper `std::function` if lambda captures are needed.
state_transition_f (*transition_state)(const PredictionArguments &...);
process_noise_uncertainty_q (*noise_process)(const PredictionArguments &...);
control_g (*transition_control)(const PredictionArguments &...);
state_transition (*transition_state_f)(const PredictionArguments &...);
process_noise_uncertainty (*noise_process_q)(const PredictionArguments &...);
control (*transition_control_g)(const PredictionArguments &...);

observation_h (*transition_observation)();
measurement_uncertainty_r (*noise_observation)();
observation (*transition_observation_h)();
measurement_uncertainty (*noise_observation_r)();

inline constexpr void predict(const PredictionArguments &...arguments)
{
const auto f{ transition_state(arguments...) };
const auto q{ noise_process(arguments...) };
fcarouge::predict<Transpose, Symmetrize>(state, estimate_uncertainty, f, q);
const auto f{ transition_state_f(arguments...) };
const auto q{ noise_process_q(arguments...) };
fcarouge::predict<Transpose, Symmetrize>(state_x, estimate_uncertainty_p, f,
q);
}

inline constexpr void predict(const input_u &input,
inline constexpr void predict(const input &input_u,
const PredictionArguments &...arguments)
{
const auto f{ transition_state(arguments...) };
const auto q{ noise_process(arguments...) };
const auto g{ transition_control(arguments...) };
fcarouge::predict<Transpose, Symmetrize>(state, estimate_uncertainty, f, q,
g, input);
const auto f{ transition_state_f(arguments...) };
const auto q{ noise_process_q(arguments...) };
const auto g{ transition_control_g(arguments...) };
fcarouge::predict<Transpose, Symmetrize>(state_x, estimate_uncertainty_p, f,
q, g, input_u);
}

inline constexpr void update(const output_z &output)
inline constexpr void update(const output &output_z)
{
const auto h{ transition_observation() };
const auto r{ noise_observation() };
const auto h{ transition_observation_h() };
const auto r{ noise_observation_r() };
fcarouge::update<Transpose, Symmetrize, Divide, Identity>(
state, estimate_uncertainty, h, r, output);
state_x, estimate_uncertainty_p, h, r, output_z);
}
};

Expand Down
14 changes: 7 additions & 7 deletions include/fcarouge/kalman_equation.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,9 @@ extrapolate_covariance(const auto &p, const auto &f, const auto &q)
{
using estimate_uncertainty_p =
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
using state_transition_f =
using state_transition =
std::remove_reference_t<std::remove_cv_t<decltype(f)>>;
Transpose<state_transition_f> transpose;
Transpose<state_transition> transpose;

return estimate_uncertainty_p{ f * p * transpose(f) + q };
}
Expand Down Expand Up @@ -129,12 +129,12 @@ template <template <typename> typename Transpose,
[[nodiscard]] inline constexpr auto weight_gain(const auto &p, const auto &h,
const auto &r)
{
using observation_h = std::remove_reference_t<std::remove_cv_t<decltype(h)>>;
using measurement_uncertainty_r =
using observation = std::remove_reference_t<std::remove_cv_t<decltype(h)>>;
using measurement_uncertainty =
std::remove_reference_t<std::remove_cv_t<decltype(r)>>;
using gain = std::invoke_result_t<Transpose<observation_h>, observation_h>;
Transpose<observation_h> transpose_h;
Divide<gain, measurement_uncertainty_r> divide;
using gain = std::invoke_result_t<Transpose<observation>, observation>;
Transpose<observation> transpose_h;
Divide<gain, measurement_uncertainty> divide;

return gain{ divide(p * transpose_h(h), h * p * transpose_h(h) + r) };
}
Expand Down

0 comments on commit a69392e

Please sign in to comment.