From 0c5864cc878a901b66dd9a9ce608f10c04403f2b Mon Sep 17 00:00:00 2001 From: Mateusz Pusz Date: Sun, 1 Mar 2020 14:40:54 +0100 Subject: [PATCH] Kalman filter slightly refactored + clang_format --- ...lman_filter-alpha_beta_filter_example2.cpp | 131 ++++++++---------- 1 file changed, 60 insertions(+), 71 deletions(-) diff --git a/example/kalman_filter-alpha_beta_filter_example2.cpp b/example/kalman_filter-alpha_beta_filter_example2.cpp index 2a9f894d..315ab30a 100644 --- a/example/kalman_filter-alpha_beta_filter_example2.cpp +++ b/example/kalman_filter-alpha_beta_filter_example2.cpp @@ -1,92 +1,81 @@ - - -#include -#include -#include - +#include #include #include #include +#include +#include +#include /* - kalman filter tutorial - 1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2 + kalman filter tutorial + 1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2 */ -using namespace units; -using namespace units::si::literals; - -template -struct state_variable{ - Q estimated_current_state; - Q predicted_next_state; -}; - namespace { - constexpr auto radar_transmit_interval = 5.0q_s; - constexpr double kalman_range_gain = 0.2; - constexpr double kalman_speed_gain = 0.1; -} -struct state{ +using namespace units; - state_variable > range; - state_variable > speed; - - void estimate(const state & previous_state, const si::length& measurement) - { - auto const innovation = measurement - previous_state.range.predicted_next_state; - range.estimated_current_state = previous_state.range.predicted_next_state + kalman_range_gain * innovation; - speed.estimated_current_state = previous_state.speed.predicted_next_state + kalman_speed_gain * innovation / radar_transmit_interval; - } - - void predict() - { - range.predicted_next_state = range.estimated_current_state + speed.estimated_current_state * radar_transmit_interval; - speed.predicted_next_state = speed.estimated_current_state; - } +template +struct state_variable { + Q estimated_current_state; + Q predicted_next_state; }; +using namespace units::si; + +constexpr auto radar_transmit_interval = 5.0q_s; +constexpr double kalman_range_gain = 0.2; +constexpr double kalman_speed_gain = 0.1; + +struct state { + state_variable> range; + state_variable> speed; + + constexpr void estimate(const state& previous_state, const length& measurement) + { + auto const innovation = measurement - previous_state.range.predicted_next_state; + range.estimated_current_state = previous_state.range.predicted_next_state + kalman_range_gain * innovation; + speed.estimated_current_state = + previous_state.speed.predicted_next_state + kalman_speed_gain * innovation / radar_transmit_interval; + } + + constexpr void predict() + { + range.predicted_next_state = + range.estimated_current_state + speed.estimated_current_state * radar_transmit_interval; + speed.predicted_next_state = speed.estimated_current_state; + } +}; + +} // namespace + int main() { - std::cout << "\n\n1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2"; - std::cout << "\n\n"; + std::cout << "\n\n1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2"; + std::cout << "\n\n"; - std::vector > measurements { - 0.0q_m, // N.B measurement[0] is unknown and unused - 30110.0q_m, - 30265.0q_m, - 30740.0q_m, - 30750.0q_m, - 31135.0q_m, - 31015.0q_m, - 31180.0q_m, - 31610.0q_m, - 31960.0q_m, - 31865.0q_m - }; + std::vector> measurements{0.0q_m, // N.B measurement[0] is unknown and unused + 30110.0q_m, 30265.0q_m, 30740.0q_m, 30750.0q_m, 31135.0q_m, + 31015.0q_m, 31180.0q_m, 31610.0q_m, 31960.0q_m, 31865.0q_m}; - const auto num_measurements = measurements.size(); + const auto num_measurements = measurements.size(); - std::vector track{num_measurements}; + std::vector track(num_measurements); - //We need an initial estimate of track[0] as there is no previous state to get a prediction from - track[0].range.estimated_current_state = 30'000q_m; - track[0].speed.estimated_current_state = 40.0q_mps; - - for ( auto n = 0U; n < num_measurements;++n){ + // We need an initial estimate of track[0] as there is no previous state to get a prediction from + track[0].range.estimated_current_state = 30'000q_m; + track[0].speed.estimated_current_state = 40.0q_mps; - if ( n > 0){ - track[n].estimate(track[n-1],measurements[n]); - } - track[n].predict(); - - std::cout << std::fixed; - std::cout << "measurement[" << n << "] = " << std::setprecision(0) << measurements[n] <<'\n'; - std::cout << "range.estimated_current_state[" << n << "] = " << std::setprecision(1) << track[n].range.estimated_current_state<<'\n'; - std::cout << "speed.estimated_current_state[" << n << "] = " << track[n].speed.estimated_current_state <<'\n'; - std::cout << "range.predicted_next_state[" << n << "] = " << track[n].range.predicted_next_state << '\n'; - std::cout << "speed.predicted_next_state[" << n << "] = " << track[n].speed.predicted_next_state << "\n\n"; - } + for (auto n = 0U; n < num_measurements; ++n) { + if (n > 0) { + track[n].estimate(track[n - 1], measurements[n]); + } + track[n].predict(); + std::cout << fmt::format("measurement[{}] = {:.0}\n", n, measurements[n]); + std::cout << fmt::format("range.estimated_current_state[{}] = {:.1}\n", n, track[n].range.estimated_current_state); + std::cout << fmt::format("speed.estimated_current_state[{}] = {:.1}\n", n, track[n].speed.estimated_current_state); + std::cout << fmt::format("range.predicted_next_state[{}] = {:.1}\n", n, track[n].range.predicted_next_state); + std::cout << fmt::format("speed.predicted_next_state[{}] = {:.1}\n\n", n, track[n].speed.predicted_next_state); + } }