Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[naming] rename update and tweaks #19

Merged
merged 1 commit into from
Apr 29, 2022
Merged

[naming] rename update and tweaks #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 14 additions & 5 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ k.noise_observation_r = [] {
return kalman::observation_noise_uncertainty{ 25 };
};

k.update(48.54);
k.observe(48.54);
```

### Multi-Dimensional
Expand Down Expand Up @@ -99,12 +99,21 @@ k.noise_observation_r = [] {
return kalman::observation_noise_uncertainty{ 400 };
};

k.predict(kalman::input{ gravitational_acceleration }, delta_time);
k.update(kalman::output{ -32.40 });
k.predict(kalman::input{ 39.72 }, delta_time);
k.update(kalman::output{ -11.1 });
k.predict(delta_time, gravitational_acceleration);
k.observe(-32.40 );
k.predict(delta_time, 39.72);
k.observe(-11.1);
```

# Motivation

Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud.

This package explores what could be a Kalman filter implementation a la standard library. The following concerns and tradeoffs are considered:
- Separation of the application domain.
- Separation of the algebra implementation.
- Generalization of the support.

# Resources

Awesome resources to learn about Kalman filters:
Expand Down
18 changes: 15 additions & 3 deletions include/fcarouge/kalman.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,8 +102,9 @@ class kalman
//! @name Public Member Function Objects
//! @{

// Functors could be replaced by variables if data is constant.
// Functors could be replaced by the standard general-purpose polymorphic
// function wrapper `std::function` if lambda captures are needed.
// function wrappers `std::function` if lambda captures were needed.

observation (*transition_observation_h)() = [] {
return observation{ Identity<observation>()() };
Expand Down Expand Up @@ -137,14 +138,14 @@ class kalman
//! @{

template <typename... Outputs>
inline constexpr void update(const Outputs &...output_z)
inline constexpr void observe(const Outputs &...output_z)
{
auto &x{ state_x };
auto &p{ estimate_uncertainty_p };
const auto h{ transition_observation_h() };
const auto r{ noise_observation_r() };
const auto z{ output{ output_z... } };
fcarouge::update<Transpose, Symmetrize, Divide, Identity>(x, p, h, r, z);
fcarouge::observe<Transpose, Symmetrize, Divide, Identity>(x, p, h, r, z);
}

template <typename... Inputs>
Expand All @@ -160,6 +161,17 @@ class kalman
fcarouge::predict<Transpose, Symmetrize>(x, p, f, q, g, u);
}

inline constexpr void
predict(const PredictionArguments &...arguments) requires(
std::is_same_v<Input, void>)
{
auto &x{ state_x };
auto &p{ estimate_uncertainty_p };
const auto f{ transition_state_f(arguments...) };
const auto q{ noise_process_q(arguments...) };
fcarouge::predict<Transpose, Symmetrize>(x, p, f, q);
}

//! @}
};

Expand Down
4 changes: 2 additions & 2 deletions include/fcarouge/kalman_equation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,8 +143,8 @@ template <template <typename> typename Transpose,
template <typename> typename Symmetrize,
template <typename, typename> typename Divide,
template <typename> typename Identity>
inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r,
const auto &z)
inline constexpr void observe(auto &x, auto &p, const auto &h, const auto &r,
const auto &z)
{
const auto k{ weight_gain<Transpose, Divide>(p, h, r) };

Expand Down
20 changes: 10 additions & 10 deletions sample/building_height.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,18 +52,18 @@ namespace
k.noise_observation_r = [] {
return kalman::observation_noise_uncertainty{ 25 };
};
k.update(48.54f);
k.observe(48.54f);

// And so on.
k.update(47.11f);
k.update(55.01f);
k.update(55.15f);
k.update(49.89f);
k.update(40.85f);
k.update(46.72f);
k.update(50.05f);
k.update(51.27f);
k.update(49.95f);
k.observe(47.11f);
k.observe(55.01f);
k.observe(55.15f);
k.observe(49.89f);
k.observe(40.85f);
k.observe(46.72f);
k.observe(50.05f);
k.observe(51.27f);
k.observe(49.95f);

// After 10 measurements the filter estimates the height of the building
// at 49.57m.
Expand Down
20 changes: 10 additions & 10 deletions sample/liquid_temperature.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,27 +71,27 @@ namespace
return kalman::observation_noise_uncertainty{ 0.1 * 0.1 };
};

k.update(49.95f);
k.observe(49.95f);

// And so on, every measurements period: Δt = 5s (constant).
k.predict();
k.update(49.967f);
k.observe(49.967f);
k.predict();
k.update(50.1f);
k.observe(50.1f);
k.predict();
k.update(50.106f);
k.observe(50.106f);
k.predict();
k.update(49.992f);
k.observe(49.992f);
k.predict();
k.update(49.819f);
k.observe(49.819f);
k.predict();
k.update(49.933f);
k.observe(49.933f);
k.predict();
k.update(50.007f);
k.observe(50.007f);
k.predict();
k.update(50.023f);
k.observe(50.023f);
k.predict();
k.update(49.99f);
k.observe(49.99f);

// The estimate uncertainty quickly goes down, after 10 measurements:
assert(0.0013 - 0.0001 < k.estimate_uncertainty_p &&
Expand Down
60 changes: 30 additions & 30 deletions sample/rocket_altitude.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,65 +104,65 @@ namespace

// The measurement values: z1 = -32.40, u1 = 39.72.
// And so on, every measurements period: Δt = 250ms (constant, as variable).
k.update(-32.40);
k.observe(-32.40);
k.predict(delta_time, 39.72);
k.update(-11.1);
k.observe(-11.1);
k.predict(delta_time, 40.02);
k.update(18);
k.observe(18);
k.predict(delta_time, 39.97);
k.update(22.9);
k.observe(22.9);
k.predict(delta_time, 39.81);
k.update(19.5);
k.observe(19.5);
k.predict(delta_time, 39.75);
k.update(28.5);
k.observe(28.5);
k.predict(delta_time, 39.6);
k.update(46.5);
k.observe(46.5);
k.predict(delta_time, 39.77);
k.update(68.9);
k.observe(68.9);
k.predict(delta_time, 39.83);
k.update(48.2);
k.observe(48.2);
k.predict(delta_time, 39.73);
k.update(56.1);
k.observe(56.1);
k.predict(delta_time, 39.87);
k.update(90.5);
k.observe(90.5);
k.predict(delta_time, 39.81);
k.update(104.9);
k.observe(104.9);
k.predict(delta_time, 39.92);
k.update(140.9);
k.observe(140.9);
k.predict(delta_time, 39.78);
k.update(148);
k.observe(148);
k.predict(delta_time, 39.98);
k.update(187.6);
k.observe(187.6);
k.predict(delta_time, 39.76);
k.update(209.2);
k.observe(209.2);
k.predict(delta_time, 39.86);
k.update(244.6);
k.observe(244.6);
k.predict(delta_time, 39.61);
k.update(276.4);
k.observe(276.4);
k.predict(delta_time, 39.86);
k.update(323.5);
k.observe(323.5);
k.predict(delta_time, 39.74);
k.update(357.3);
k.observe(357.3);
k.predict(delta_time, 39.87);
k.update(357.4);
k.observe(357.4);
k.predict(delta_time, 39.63);
k.update(398.3);
k.observe(398.3);
k.predict(delta_time, 39.67);
k.update(446.7);
k.observe(446.7);
k.predict(delta_time, 39.96);
k.update(465.1);
k.observe(465.1);
k.predict(delta_time, 39.8);
k.update(529.4);
k.observe(529.4);
k.predict(delta_time, 39.89);
k.update(570.4);
k.observe(570.4);
k.predict(delta_time, 39.85);
k.update(636.8);
k.observe(636.8);
k.predict(delta_time, 39.9);
k.update(693.3);
k.observe(693.3);
k.predict(delta_time, 39.81);
k.update(707.3);
k.observe(707.3);
k.predict(delta_time, 39.81);
k.update(748.5);
k.observe(748.5);

// The Kalman gain for altitude converged to 0.12, which means that the
// estimation weight is much higher than the measurement weight.
Expand Down
70 changes: 35 additions & 35 deletions sample/vehicule_location.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,77 +85,77 @@ namespace
};

// The measurement values: z1 = [-393.66, 300.4]
k.update(-393.66, 300.4);
k.observe(-393.66, 300.4);

// And so on, every measurements period: Δt = 1s (constant, built-in).
k.predict();
k.update(-375.93, 301.78);
k.observe(-375.93, 301.78);
k.predict();
k.update(-351.04, 295.1);
k.observe(-351.04, 295.1);
k.predict();
k.update(-328.96, 305.19);
k.observe(-328.96, 305.19);
k.predict();
k.update(-299.35, 301.06);
k.observe(-299.35, 301.06);
k.predict();
k.update(-273.36, 302.05);
k.observe(-273.36, 302.05);
k.predict();
k.update(-245.89, 300);
k.observe(-245.89, 300);
k.predict();
k.update(-222.58, 303.57);
k.observe(-222.58, 303.57);
k.predict();
k.update(-198.03, 296.33);
k.observe(-198.03, 296.33);
k.predict();
k.update(-174.17, 297.65);
k.observe(-174.17, 297.65);
k.predict();
k.update(-146.32, 297.41);
k.observe(-146.32, 297.41);
k.predict();
k.update(-123.72, 299.61);
k.observe(-123.72, 299.61);
k.predict();
k.update(-103.47, 299.6);
k.observe(-103.47, 299.6);
k.predict();
k.update(-78.23, 302.39);
k.observe(-78.23, 302.39);
k.predict();
k.update(-52.63, 295.04);
k.observe(-52.63, 295.04);
k.predict();
k.update(-23.34, 300.09);
k.observe(-23.34, 300.09);
k.predict();
k.update(25.96, 294.72);
k.observe(25.96, 294.72);
k.predict();
k.update(49.72, 298.61);
k.observe(49.72, 298.61);
k.predict();
k.update(76.94, 294.64);
k.observe(76.94, 294.64);
k.predict();
k.update(95.38, 284.88);
k.observe(95.38, 284.88);
k.predict();
k.update(119.83, 272.82);
k.observe(119.83, 272.82);
k.predict();
k.update(144.01, 264.93);
k.observe(144.01, 264.93);
k.predict();
k.update(161.84, 251.46);
k.observe(161.84, 251.46);
k.predict();
k.update(180.56, 241.27);
k.observe(180.56, 241.27);
k.predict();
k.update(201.42, 222.98);
k.observe(201.42, 222.98);
k.predict();
k.update(222.62, 203.73);
k.observe(222.62, 203.73);
k.predict();
k.update(239.4, 184.1);
k.observe(239.4, 184.1);
k.predict();
k.update(252.51, 166.12);
k.observe(252.51, 166.12);
k.predict();
k.update(266.26, 138.71);
k.observe(266.26, 138.71);
k.predict();
k.update(271.75, 119.71);
k.observe(271.75, 119.71);
k.predict();
k.update(277.4, 100.41);
k.observe(277.4, 100.41);
k.predict();
k.update(294.12, 79.76);
k.observe(294.12, 79.76);
k.predict();
k.update(301.23, 50.62);
k.observe(301.23, 50.62);
k.predict();
k.update(291.8, 32.99);
k.observe(291.8, 32.99);
k.predict();
k.update(299.89, 2.14);
k.observe(299.89, 2.14);

assert(5 - 0.0001 < k.estimate_uncertainty_p(0, 0) &&
k.estimate_uncertainty_p(0, 0) < 5 + 1.84 &&
Expand Down