Skip to content

Commit

Permalink
Merge pull request #3 from FrancoisCarouge/variable-time-support
Browse files Browse the repository at this point in the history 
[time] delta time and other variable parameters
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge authored Apr 9, 2022
2 parents 06093a2 + c443ae5 commit 74564f6
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 38 deletions.
22 changes: 12 additions & 10 deletions include/fcarouge/eigen.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,17 +46,17 @@ For more information, please refer to <https://unlicense.org> */
namespace fcarouge::eigen
{
template <typename Type> struct transpose {
Eigen::Matrix<typename Type::Scalar, Type::ColsAtCompileTime,
Type::RowsAtCompileTime>
[[nodiscard]] inline constexpr Eigen::Matrix<
typename Type::Scalar, Type::ColsAtCompileTime, Type::RowsAtCompileTime>
operator()(const Type &value)
{
return value.transpose();
}
};

template <typename Type> struct symmetrize {
Eigen::Matrix<typename Type::Scalar, Type::RowsAtCompileTime,
Type::ColsAtCompileTime>
[[nodiscard]] inline constexpr Eigen::Matrix<
typename Type::Scalar, Type::RowsAtCompileTime, Type::ColsAtCompileTime>
operator()(const Type &value)
{
const auto e{ value.eval() };
Expand All @@ -65,8 +65,9 @@ template <typename Type> struct symmetrize {
};

template <typename Numerator, typename Denominator> struct divide {
Eigen::Matrix<typename Numerator::Scalar, Numerator::RowsAtCompileTime,
Denominator::RowsAtCompileTime>
[[nodiscard]] inline constexpr Eigen::Matrix<typename Numerator::Scalar,
Numerator::RowsAtCompileTime,
Denominator::RowsAtCompileTime>
operator()(const Numerator &numerator, const Denominator &denominator)
{
return denominator.transpose()
Expand All @@ -77,19 +78,20 @@ template <typename Numerator, typename Denominator> struct divide {
};

template <typename Type> struct identity {
Eigen::Matrix<typename Type::Scalar, Type::RowsAtCompileTime,
Type::ColsAtCompileTime>
[[nodiscard]] inline constexpr Eigen::Matrix<
typename Type::Scalar, Type::RowsAtCompileTime, Type::ColsAtCompileTime>
operator()()
{
return Type::Identity();
}
};

template <typename Type, int State, int Output, int Input>
template <typename Type, int State, int Output, int Input,
typename... PredictionArguments>
using kalman =
fcarouge::kalman<Eigen::Vector<Type, State>, Eigen::Vector<Type, Output>,
Eigen::Vector<Type, Input>, transpose, symmetrize, divide,
identity>;
identity, PredictionArguments...>;

} // namespace fcarouge::eigen

Expand Down
66 changes: 38 additions & 28 deletions include/fcarouge/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,50 +47,53 @@ For more information, please refer to <https://unlicense.org> */
namespace fcarouge
{
template <typename Type> struct transpose {
auto operator()(const Type &value)
[[nodiscard]] inline constexpr auto operator()(const Type &value)
{
return value;
}
};

template <typename Type> struct symmetrize {
auto operator()(const Type &value)
[[nodiscard]] inline constexpr auto operator()(const Type &value)
{
return value;
}
};

template <typename Numerator, typename Denominator> struct divide {
auto operator()(const Numerator &numerator, const Denominator &denominator)
[[nodiscard]] inline constexpr auto operator()(const Numerator &numerator,
const Denominator &denominator)
{
return numerator / denominator;
}
};

template <typename Type> struct identity {
Type operator()()
[[nodiscard]] inline constexpr Type operator()()
{
return 1;
}
};

auto extrapolate_state(const auto &x, const auto &f)
[[nodiscard]] inline constexpr auto extrapolate_state(const auto &x,
const auto &f)
{
using State = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;

return State{ f * x };
}

auto extrapolate_state(const auto &x, const auto &f, const auto &g,
const auto &u)
[[nodiscard]] inline constexpr auto
extrapolate_state(const auto &x, const auto &f, const auto &g, const auto &u)
{
using State = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;

return State{ f * x + g * u };
}

template <template <typename> class Transpose>
auto extrapolate_covariance(const auto &p, const auto &f, const auto &q)
[[nodiscard]] inline constexpr auto
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)>>;
Expand All @@ -103,7 +106,7 @@ auto extrapolate_covariance(const auto &p, const auto &f, const auto &q)

template <template <typename> typename Transpose,
template <typename> typename Symmetrize>
void predict(auto &x, auto &p, const auto &f, const auto &q)
inline constexpr void predict(auto &x, auto &p, const auto &f, const auto &q)
{
x = extrapolate_state(x, f);

Expand All @@ -115,8 +118,8 @@ void predict(auto &x, auto &p, const auto &f, const auto &q)

template <template <typename> typename Transpose,
template <typename> typename Symmetrize>
void predict(auto &x, auto &p, const auto &f, const auto &q, const auto &g,
const auto &u)
inline constexpr void predict(auto &x, auto &p, const auto &f, const auto &q,
const auto &g, const auto &u)
{
x = extrapolate_state(x, f, g, u);

Expand All @@ -126,7 +129,8 @@ void predict(auto &x, auto &p, const auto &f, const auto &q, const auto &g,
p = symmetrize(extrapolate_covariance<Transpose>(p, f, q));
}

auto update_state(const auto &x, const auto &k, const auto &z, const auto &h)
[[nodiscard]] inline constexpr auto update_state(const auto &x, const auto &k,
const auto &z, const auto &h)
{
using State = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;

Expand All @@ -135,8 +139,8 @@ auto update_state(const auto &x, const auto &k, const auto &z, const auto &h)

template <template <typename> typename Transpose,
template <typename> typename Identity>
auto update_covariance(const auto &p, const auto &k, const auto &h,
const auto &r)
[[nodiscard]] inline constexpr auto
update_covariance(const auto &p, const auto &k, const auto &h, const auto &r)
{
using estimate_uncertainty_p =
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
Expand All @@ -151,7 +155,8 @@ auto update_covariance(const auto &p, const auto &k, const auto &h,

template <template <typename> typename Transpose,
template <typename, typename> typename Divide>
auto weight_gain(const auto &p, const auto &h, const auto &r)
[[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 =
Expand All @@ -167,7 +172,8 @@ template <template <typename> typename Transpose,
template <typename> typename Symmetrize,
template <typename, typename> typename Divide,
template <typename> typename Identity>
void update(auto &x, auto &p, const auto &h, const auto &r, const auto &z)
inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r,
const auto &z)
{
const auto k{ weight_gain<Transpose, Divide>(p, h, r) };

Expand All @@ -183,7 +189,8 @@ template <typename State, typename Output = State, typename Input = State,
template <typename> typename Transpose = transpose,
template <typename> typename Symmetrize = symmetrize,
template <typename, typename> typename Divide = divide,
template <typename> typename Identity = identity>
template <typename> typename Identity = identity,
typename... PredictionArguments>
class kalman
{
public:
Expand All @@ -204,29 +211,32 @@ class kalman

state_x state;
estimate_uncertainty_p estimate_uncertainty;
state_transition_f (*transition_state)();
process_noise_uncertainty_q (*noise_process)();

state_transition_f (*transition_state)(const PredictionArguments &...);
process_noise_uncertainty_q (*noise_process)(const PredictionArguments &...);
control_g (*transition_control)(const PredictionArguments &...);

observation_h (*transition_observation)();
measurement_uncertainty_r (*noise_observation)();
control_g (*transition_control)();

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

void predict(const input_u &input)
inline constexpr void predict(const input_u &input,
const PredictionArguments &...arguments)
{
const auto f{ transition_state() };
const auto q{ noise_process() };
const auto g{ transition_control() };
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);
}

void update(const output_z &output)
inline constexpr void update(const output_z &output)
{
const auto h{ transition_observation() };
const auto r{ noise_observation() };
Expand Down

0 comments on commit 74564f6

Please sign in to comment.