Skip to content

Commit

Permalink
Merge 234639b into 7d46693
Browse files Browse the repository at this point in the history
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge authored Apr 29, 2022
2 parents 7d46693 + 234639b commit 20af0d2
Show file tree
Hide file tree
Showing 7 changed files with 116 additions and 95 deletions.
19 changes: 14 additions & 5 deletions README.md
View file
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
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
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
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
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
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
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

0 comments on commit 20af0d2

Please sign in to comment.