Skip to content

Commit

Permalink
[filter] add extended support (#49)
Browse files Browse the repository at this point in the history
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge committed Jun 25, 2022
1 parent 860a445 commit ab717dd
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 33 deletions.
47 changes: 33 additions & 14 deletions include/fcarouge/internal/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,9 +128,15 @@ struct kalman {
};
output_uncertainty r{ 0 *
Identity().template operator()<output_uncertainty>() };

//! @todo Should H be initialized to all ones?
output_model h{ Identity().template operator()<output_model>() };
state_transition f{ Identity().template operator()<state_transition>() };

//! @todo Should G be initialized to all ones?
input_control g{ Identity().template operator()<input_control>() };

//! @todo Should K be initialized to all ones?
gain k{ Identity().template operator()<gain>() };
innovation y{ 0 * Identity().template operator()<innovation>() };
innovation_uncertainty s{
Expand All @@ -150,6 +156,11 @@ struct kalman {
//! observation function. H = ∂h/∂X = ∂hj/∂xi that is each row i
//! contains the derivatives of the state observation function for every
//! element j in the state vector X.
//!
//! @todo Should we pass through the reference to the state x or have the user
//! access it through k.x() when needed? Where does the practical/performance
//! tradeoff leans toward? For the general case? For the specialized cases?
//! Same question applies to other parameters.
std::function<output_model(const state &)> observation_state_h{
[this](const state &x) -> output_model {
static_cast<void>(x);
Expand All @@ -158,9 +169,13 @@ struct kalman {
};

//! @brief Compute observation noise R matrix.
std::function<output_uncertainty()> noise_observation_r{
[this] -> output_uncertainty { return r; }
};
std::function<output_uncertainty(const state &, const output &)>
noise_observation_r{ [this](const state &x,
const output &z) -> output_uncertainty {
static_cast<void>(x);
static_cast<void>(z);
return r;
} };

//! @brief Compute the state transition F matrix.
//!
Expand All @@ -171,18 +186,23 @@ struct kalman {
//! state vector X.
//!
//! @todo Pass the arguments by universal reference?
std::function<state_transition(const PredictionArguments &...)>
std::function<state_transition(const state &, const PredictionArguments &...)>
transition_state_f{
[this](const PredictionArguments &...arguments) -> state_transition {
[this](const state &x,
const PredictionArguments &...arguments) -> state_transition {
static_cast<void>(x);
(static_cast<void>(arguments), ...);
return f;
}
};

//! @brief Compute process noise Q matrix.
std::function<process_uncertainty(const PredictionArguments &...)>
std::function<process_uncertainty(const state &,
const PredictionArguments &...)>
noise_process_q{
[this](const PredictionArguments &...arguments) -> process_uncertainty {
[this](const state &x,
const PredictionArguments &...arguments) -> process_uncertainty {
static_cast<void>(x);
(static_cast<void>(arguments), ...);
return q;
}
Expand Down Expand Up @@ -240,9 +260,10 @@ struct kalman {

z = output{ output_z... };
h = observation_state_h(x);
r = noise_observation_r();
r = noise_observation_r(x, z);
s = h * p * transpose(h) + r;
k = divide(p * transpose(h), s);
// Do we want to support custom y = output_difference(z, observation(x))?
y = z - observation(x);
x = x + k * y;
p = symmetrize(estimate_uncertainty{
Expand All @@ -256,19 +277,17 @@ struct kalman {
{
const auto u{ input{ input_u... } };

f = transition_state_f(arguments...);
q = noise_process_q(arguments...);
f = transition_state_f(x, arguments...);
q = noise_process_q(x, arguments...);
g = transition_control_g(arguments...);

x = f * x + g * u;
p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q });
}

inline constexpr void predict(const PredictionArguments &...arguments)
{
f = transition_state_f(arguments...);
q = noise_process_q(arguments...);

f = transition_state_f(x, arguments...);
q = noise_process_q(x, arguments...);
x = transition(x, arguments...);
p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q });
}
Expand Down
22 changes: 12 additions & 10 deletions include/fcarouge/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -310,8 +310,6 @@ class kalman
//! @return The observation vector Z.
//!
//! @complexity Constant.
//!
//! @todo Implement and test.
[[nodiscard("The returned observation vector Z is unexpectedly "
"discarded.")]] inline constexpr auto
z() const -> output;
Expand Down Expand Up @@ -430,7 +428,7 @@ class kalman
inline constexpr void q(const auto &callable) requires std::is_invocable_r_v <
process_uncertainty,
std::decay_t<decltype(callable)>,
const PredictionArguments &... >
const state &, const PredictionArguments &... >
{
filter.noise_process_q = callable;
}
Expand All @@ -447,7 +445,7 @@ class kalman
inline constexpr void
q(auto &&callable) requires std::is_invocable_r_v < process_uncertainty,
std::decay_t<decltype(callable)>,
const PredictionArguments &... >
const state &, const PredictionArguments &... >
{
filter.noise_process_q = std::forward<decltype(callable)>(callable);
}
Expand Down Expand Up @@ -509,8 +507,10 @@ class kalman
//! @complexity Constant.
//!
//! @todo Understand why Cland Tidy doesn't find the out-of-line definition.
inline constexpr void r(const auto &callable) requires std::is_invocable_r_v<
output_uncertainty, std::decay_t<decltype(callable)>>
inline constexpr void r(const auto &callable) requires std::is_invocable_r_v <
output_uncertainty,
std::decay_t<decltype(callable)>,
const state &, const output & >
{
filter.noise_observation_r = callable;
}
Expand All @@ -524,8 +524,10 @@ class kalman
//! on prediction steps.
//!
//! @complexity Constant.
inline constexpr void r(auto &&callable) requires std::is_invocable_r_v<
output_uncertainty, std::decay_t<decltype(callable)>>
inline constexpr void
r(auto &&callable) requires std::is_invocable_r_v < output_uncertainty,
std::decay_t<decltype(callable)>,
const state &, const output & >
{
filter.noise_observation_r = std::forward<decltype(callable)>(callable);
}
Expand Down Expand Up @@ -585,7 +587,7 @@ class kalman
inline constexpr void
f(const auto &callable) requires std::is_invocable_r_v < state_transition,
std::decay_t<decltype(callable)>,
const PredictionArguments &... >
const state &, const PredictionArguments &... >
{
filter.transition_state_f = callable;
}
Expand All @@ -602,7 +604,7 @@ class kalman
inline constexpr void
f(auto &&callable) requires std::is_invocable_r_v < state_transition,
std::decay_t<decltype(callable)>,
const PredictionArguments &... >
const state &, const PredictionArguments &... >
{
filter.transition_state_f = std::forward<decltype(callable)>(callable);
}
Expand Down
6 changes: 4 additions & 2 deletions sample/rocket_altitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,8 @@ namespace
// the system random acceleration. The accelerometer error v is much lower
// than system's random acceleration, therefore we use ϵ^2 as a multiplier of
// the process noise matrix. This makes our estimation uncertainty much lower!
k.q([](const std::chrono::milliseconds &delta_time) {
k.q([](const kalman::state &x, const std::chrono::milliseconds &delta_time) {
static_cast<void>(x);
const auto dt{ std::chrono::duration<double>(delta_time).count() };
return kalman::process_uncertainty{
{ 0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2 },
Expand All @@ -73,7 +74,8 @@ namespace
});

// The state transition matrix F would be:
k.f([](const std::chrono::milliseconds &delta_time) {
k.f([](const kalman::state &x, const std::chrono::milliseconds &delta_time) {
static_cast<void>(x);
const auto dt{ std::chrono::duration<double>(delta_time).count() };
return kalman::state_transition{ { 1, dt }, { 0, 1 } };
});
Expand Down
13 changes: 6 additions & 7 deletions sample/vehicule_location.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,12 @@ namespace

// Prediction
// The process noise matrix Q would be:
k.q(0.2 * 0.2 *
kalman::process_uncertainty{ { 0.25, 0.5, 0.5, 0, 0, 0 },
{ 0.5, 1, 1, 0, 0, 0 },
{ 0.5, 1, 1, 0, 0, 0 },
{ 0, 0, 0, 0.25, 0.5, 0.5 },
{ 0, 0, 0, 0.5, 1, 1 },
{ 0, 0, 0, 0.5, 1, 1 } });
kalman::process_uncertainty q{
{ 0.25, 0.5, 0.5, 0, 0, 0 }, { 0.5, 1, 1, 0, 0, 0 }, { 0.5, 1, 1, 0, 0, 0 },
{ 0, 0, 0, 0.25, 0.5, 0.5 }, { 0, 0, 0, 0.5, 1, 1 }, { 0, 0, 0, 0.5, 1, 1 }
};
q *= 0.2 * 0.2;
k.q(std::move(q));

// The state transition matrix F would be:
k.f(kalman::state_transition{ { 1, 1, 0.5, 0, 0, 0 },
Expand Down

0 comments on commit ab717dd

Please sign in to comment.