mpusz/mp-units
1
0
Fork 1
mirror of https://github.com/mpusz/mp-units.git synced 2025-07-31 19:04:27 +02:00
Code Issues Packages Projects Releases Wiki Activity

Kalman filter slightly refactored + clang_format

Browse Source
This commit is contained in:
Mateusz Pusz
2020-03-01 14:40:54 +01:00
parent cd0eb11c14
commit 0c5864cc87
1 changed files with 60 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
example/kalman_filter-alpha_beta_filter_example2.cpp
View File

@@ -1,92 +1,81 @@
#include <vector>
#include <iostream>
#include <iomanip>
#include <units/format.h>
#include <units/physical/si/length.h>
#include <units/physical/si/time.h>
#include <units/physical/si/velocity.h>
#include <iomanip>
#include <iostream>
#include <vector>
/*
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 <Quantity Q>
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<si::length<si::metre> > range;
state_variable<si::velocity<si::metre_per_second> > speed;
void estimate(const state & previous_state, const si::length<si::metre>& 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<Quantity Q>
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<length<metre>> range;
state_variable<velocity<metre_per_second>> speed;
constexpr void estimate(const state& previous_state, const length<metre>& 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<si::length<si::metre> > 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<length<metre>> 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<state> track{num_measurements};
std::vector<state> 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);
}
}