(C++) StateObserver
February 24, 2017 · View on GitHub
(C++) StateObserver
StateObserver is a class for state observer.
Technical facts
./CppStateObserver/CppStateObserver.pri
INCLUDEPATH += \ ../../Classes/CppStateObserver SOURCES += \ ../../Classes/CppStateObserver/alphabetafilter.cpp \ ../../Classes/CppStateObserver/alphabetagammafilter.cpp \ ../../Classes/CppStateObserver/alphafilter.cpp \ ../../Classes/CppStateObserver/floatingpointstateobserver.cpp \ ../../Classes/CppStateObserver/integeralphafilter.cpp \ ../../Classes/CppStateObserver/integerstateobserver.cpp \ ../../Classes/CppStateObserver/integersymmetricalphafilter.cpp \ ../../Classes/CppStateObserver/multialphafilter.cpp \ ../../Classes/CppStateObserver/multiintegerstateobserver.cpp HEADERS += \ ../../Classes/CppStateObserver/alphabetafilter.h \ ../../Classes/CppStateObserver/alphabetagammafilter.h \ ../../Classes/CppStateObserver/alphafilter.h \ ../../Classes/CppStateObserver/floatingpointstateobserver.h \ ../../Classes/CppStateObserver/integeralphafilter.h \ ../../Classes/CppStateObserver/integerstateobserver.h \ ../../Classes/CppStateObserver/integersymmetricalphafilter.h \ ../../Classes/CppStateObserver/multialphafilter.h \ ../../Classes/CppStateObserver/multiintegerstateobserver.h \ ../../Classes/CppStateObserver/stateobserverfwd.h OTHER_FILES += \ ../../Classes/CppStateObserver/Licence.txt
./CppStateObserver/alphabetafilter.h
#ifndef ALPHABETAFILTER_H #define ALPHABETAFILTER_H #include <string> #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "floatingpointstateobserver.h" #pragma GCC diagnostic pop namespace ribi { ///An alpha beta filter struct AlphaBetaFilter : public FloatingPointStateObserver { AlphaBetaFilter( const double alpha = 0.1 , const double beta = 0.01, const double dt = 1.0 ); ///Get the current state estimate double GetEstimate() const { return m_output; } ///Supply a measurement, which will update the state estimate void Update(const double measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: const double m_alpha; const double m_beta; const double m_dt; double m_slope; double m_output; }; } //~namespace ribi #endif // ALPHABETAFILTER_H
./CppStateObserver/alphabetafilter.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "alphabetafilter.h" #include <cassert> #pragma GCC diagnostic pop ribi::AlphaBetaFilter::AlphaBetaFilter( const double alpha, const double beta, const double dt) : m_alpha(alpha), m_beta(beta), m_dt(dt), m_slope(0.0), m_output(0.0) { assert(m_alpha >= 0.0 && m_alpha < 1.0); assert(m_beta >= 0.0 && m_beta <= 2.0); //beta > 1.0 amplifies noise assert(4.0 - (2.0 * m_alpha - m_beta) > 0.0); assert(m_dt != 0.0); } void ribi::AlphaBetaFilter::Update(const double measurement) { const double output_predicted = m_output + ( m_slope * m_dt ); const double prediction_error = measurement - output_predicted; const double output_corrected = output_predicted + (m_alpha * prediction_error); m_output = output_corrected; m_slope += (m_beta * prediction_error / m_dt); } std::string ribi::AlphaBetaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::AlphaBetaFilter::GetVersionHistory() noexcept { std::vector<std::string> v; v.push_back("2013-05-25: version 1.0: initial version"); v.push_back("2013-06-18: version 1.1: derive from FloatingPointStateObserver"); return v; }
./CppStateObserver/alphabetagammafilter.h
#ifndef ALPHABETAGAMMAFILTER_H #define ALPHABETAGAMMAFILTER_H #include <string> #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "floatingpointstateobserver.h" #pragma GCC diagnostic pop namespace ribi { ///An alpha beta gamma filter struct AlphaBetaGammaFilter : public FloatingPointStateObserver { AlphaBetaGammaFilter( const double alpha = 0.1 , const double beta = 0.01, const double gamma = 0.001, const double dt = 1.0 ); ///Get the current state estimate double GetEstimate() const { return m_position; } ///Supply a measurement, which will update the state estimate void Update(const double measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: double m_acceleration; const double m_alpha; const double m_beta; const double m_dt; const double m_gamma; double m_position; double m_velocity; }; } //~namespace ribi #endif // ALPHABETAGAMMAFILTER_H
./CppStateObserver/alphabetagammafilter.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "alphabetagammafilter.h" #include <cassert> #pragma GCC diagnostic pop ribi::AlphaBetaGammaFilter::AlphaBetaGammaFilter( const double alpha, const double beta, const double gamma, const double dt) : m_acceleration(0.0), m_alpha(alpha), m_beta(beta), m_dt(dt), m_gamma(gamma), m_position(0.0), m_velocity(0.0) { assert(m_alpha >= 0.0 && m_alpha < 1.0); assert(m_beta >= 0.0 && m_beta <= 2.0); //beta > 1.0 amplifies noise assert(4.0 - (2.0 * m_alpha - m_beta) > 0.0); assert(m_dt != 0.0); } void ribi::AlphaBetaGammaFilter::Update(const double measurement) { const double output_predicted = m_position + ( m_velocity * m_dt ); const double prediction_error = measurement - output_predicted; m_position = output_predicted + (m_alpha * prediction_error); m_velocity += (m_beta * prediction_error / m_dt); m_acceleration += (m_gamma * 2.0 * prediction_error / (m_dt * m_dt) ); } std::string ribi::AlphaBetaGammaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::AlphaBetaGammaFilter::GetVersionHistory() noexcept { std::vector<std::string> v; v.push_back("2013-05-25: version 1.0: initial version"); v.push_back("2013-06-18: version 1.1: derive from FloatingPointStateObserver"); return v; }
./CppStateObserver/alphafilter.h
#ifndef ALPHAFILTER_H #define ALPHAFILTER_H #include <string> #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "floatingpointstateobserver.h" #pragma GCC diagnostic pop namespace ribi { ///An alpha filter as described on [1] ///An alpha filter is equivalant to a low-pass filter (also called high-cut filter, or treble cut filter) [2] /// [1] http://en.wikipedia.org/wiki/Alpha_beta_filter#The_alpha_filter /// [2] http://en.wikipedia.org/wiki/Low-pass_filter struct AlphaFilter : public FloatingPointStateObserver { AlphaFilter( const double alpha = 0.1, const double dt = 1.0 ); ///Get the current state estimate double GetEstimate() const { return m_output; } ///Supply a measurement, which will update the state estimate void Update(const double measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: ///The response to noise measurements /// * m_alpha = 1.0: follow measurements /// * m_alpha = 0.9: low inertia: noise in measurements in decreased only slightly /// * m_alpha = 0.1: high inertia: noise in measurements in decreased considerably /// * m_alpha = 0.0: ignore measurements /// ///For a low-pass filter: /// /// m_alpha = m_dt / (RC + m_dt) /// ///Where /// * m_dt: time step (sec) /// * R: resistance (ohm) /// * C: capacitance (Farad) const double m_alpha; ///The time constant const double m_dt; ///The current estimate of the state observed double m_output; }; ///An alpha filter can easily be converted to the following state transition matrix: /// ///[x_new] = [x_current] * [1.0 - alpha] + [ alpha ] [ input ] + [ noise ] /// #1 #2 #3 #4 #5 #6 /// ///#1: The new state vector ///#2: The current state vector ///#3: The state transition matrix ///#4: The control matrix ///#5: The input vector ///#6: The process noise vector /// ///The alpha filter is supplied as an example in the tool KalmanFilterer } //~namespace ribi #endif // ALPHAFILTER_H
./CppStateObserver/alphafilter.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "alphafilter.h" #include <cassert> #pragma GCC diagnostic pop ribi::AlphaFilter::AlphaFilter( const double alpha, const double dt) : m_alpha(alpha), m_dt(dt), m_output(0.0) { //assert(m_alpha >= 0.0 && m_alpha < 1.0); //assert(m_beta >= 0.0 && m_beta <= 2.0); //beta > 1.0 amplifies noise //assert(4.0 - (2.0 * m_alpha - m_beta) > 0.0); assert(m_dt != 0.0); } void ribi::AlphaFilter::Update(const double measurement) { const double difference = measurement - m_output; m_output += m_alpha * difference; } std::string ribi::AlphaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::AlphaFilter::GetVersionHistory() noexcept { std::vector<std::string> v; v.push_back("2013-05-25: version 1.0: initial version"); v.push_back("2013-06-18: version 1.1: derive from FloatingPointStateObserver"); return v; }
./CppStateObserver/floatingpointstateobserver.h
#ifndef FLOATINGPOINTSTATEOBSERVER_H #define FLOATINGPOINTSTATEOBSERVER_H #include <string> #include <vector> namespace ribi { ///An observer for floating point values: ///The measurements it works must be floating point, ///The estimates it gives will be floating point struct FloatingPointStateObserver { ///ABC must have public virtual destructor // * Herb Sutter, Andrei Alexandrescu. C++ coding standards: 101 rules, guidelines, and best practices. // ISBN: 0-32-111358-6. Item 50: 'Make base class destructors public and virtual, or protected and nonvirtual' virtual ~FloatingPointStateObserver() {} ///Get the current state estimate virtual double GetEstimate() const = 0; ///Supply a measurement, which will update the state estimate virtual void Update(const double measurement) = 0; ///Obtain the version of this class static std::string GetVersion(); ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory(); }; } //~namespace ribi #endif // FLOATINGPOINTSTATEOBSERVER_H
./CppStateObserver/floatingpointstateobserver.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "floatingpointstateobserver.h" #pragma GCC diagnostic pop std::string ribi::FloatingPointStateObserver::GetVersion() { return "1.0"; } std::vector<std::string> ribi::FloatingPointStateObserver::GetVersionHistory() { std::vector<std::string> v; v.push_back("2013-06-18: version 1.0: initial version"); return v; }
./CppStateObserver/integeralphafilter.h
#ifndef INTEGERALPHAFILTER_H #define INTEGERALPHAFILTER_H #include <string> #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "integerstateobserver.h" #pragma GCC diagnostic pop namespace ribi { struct IntegerAlphaFilter : public IntegerStateObserver { IntegerAlphaFilter( const int alpha, const int64_t value_active = 0); ///Get the current state estimate int64_t GetEstimate() const { return m_value_active; } ///Supply a measurement, which will update the state estimate void Update(const int64_t measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: ///The bitshift used for division int m_alpha; int64_t m_value_active; }; } //~namespace ribi #endif // INTEGERALPHAFILTER_H
./CppStateObserver/integeralphafilter.cpp
#include "integeralphafilter.h" #include <cassert> #include <cstdlib> ribi::IntegerAlphaFilter::IntegerAlphaFilter( const int alpha, const int64_t value_active) : m_alpha(alpha), m_value_active(value_active) { assert(m_alpha >= 0 && "A bitshift should not be done with negative values"); assert(m_alpha <= 63 && "An int64_t can maximally be shifted 63 bits to the right"); } void ribi::IntegerAlphaFilter::Update(const int64_t measurement) { m_value_active += ((measurement - m_value_active) >> m_alpha); } std::string ribi::IntegerAlphaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::IntegerAlphaFilter::GetVersionHistory() noexcept { std::vector<std::string> v; v.push_back("2013-06-04: version 1.0: initial version"); v.push_back("2013-06-18: version 1.1: refactoring of IntegerStateObserver"); return v; }
./CppStateObserver/integerstateobserver.h
#ifndef INTEGERSTATEOBSERVER_H #define INTEGERSTATEOBSERVER_H #include <cinttypes> #include <string> #include <vector> namespace ribi { ///An observer for integer values: ///The measurements it works must be int, ///The estimates it gives will be int struct IntegerStateObserver { virtual ~IntegerStateObserver() noexcept {} ///Get the current state estimate virtual int64_t GetEstimate() const = 0; ///Supply a measurement, which will update the state estimate virtual void Update(const int64_t measurement) = 0; ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; }; } //~namespace ribi #endif // INTEGERSTATEOBSERVER_H
./CppStateObserver/integerstateobserver.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "integerstateobserver.h" #include <boost/numeric/conversion/cast.hpp> #pragma GCC diagnostic pop std::string ribi::IntegerStateObserver::GetVersion() noexcept { return "1.0"; } std::vector<std::string> ribi::IntegerStateObserver::GetVersionHistory() noexcept { return { "2013-06-04: version 1.0: initial version" }; }
./CppStateObserver/integersymmetricalphafilter.h
#ifndef INTEGERSYMMETRICALPHAFILTER_H #define INTEGERSYMMETRICALPHAFILTER_H #include <string> #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "integerstateobserver.h" #pragma GCC diagnostic pop namespace ribi { struct IntegerSymmetricalAlphaFilter : public IntegerStateObserver { IntegerSymmetricalAlphaFilter( const int alpha, const int64_t value_active = 0); ///Get the current state estimate int64_t GetEstimate() const { return m_value_active; } ///Supply a measurement, which will update the state estimate void Update(const int64_t measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: ///The bitshift used for division int m_alpha; int64_t m_value_active; }; } //~namespace ribi #endif // INTEGERSYMMETRICALPHAFILTER_H
./CppStateObserver/integersymmetricalphafilter.cpp
#include "integersymmetricalphafilter.h" #include <cassert> #include <cstdlib> ribi::IntegerSymmetricalAlphaFilter::IntegerSymmetricalAlphaFilter( const int alpha, const int64_t value_active) : m_alpha(alpha), m_value_active(value_active) { assert(m_alpha >= 0 && "A bitshift should not be done with negative values"); assert(m_alpha <= 63 && "An int64_t can maximally be shifted 63 bits to the right"); } void ribi::IntegerSymmetricalAlphaFilter::Update(const int64_t measurement) { const int64_t delta = ((measurement - m_value_active) >> m_alpha); m_value_active += delta + (delta == 0 && measurement - m_value_active > 0 ? 1 : 0); } std::string ribi::IntegerSymmetricalAlphaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::IntegerSymmetricalAlphaFilter::GetVersionHistory() noexcept { std::vector<std::string> v; v.push_back("2013-06-04: version 1.0: initial version"); v.push_back("2013-06-18: version 1.1: refactoring of IntegerStateObserver"); return v; }
./CppStateObserver/multialphafilter.h
#ifndef MULTIALPHAFILTER_H #define MULTIALPHAFILTER_H #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include <boost/shared_ptr.hpp> #include "alphafilter.h" #include "floatingpointstateobserver.h" #pragma GCC diagnostic pop namespace ribi { struct MultiAlphaFilter : public FloatingPointStateObserver { MultiAlphaFilter( const std::vector<double> alphas, const double dt = 1.0 ); ///Get the current state estimate double GetEstimate() const; ///Supply a measurement, which will update the state estimate void Update(const double measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: std::vector<boost::shared_ptr<AlphaFilter> > m_filters; static const std::vector<boost::shared_ptr<AlphaFilter> > CreateFilters( const std::vector<double> alphas, const double dt); }; } //~namespace ribi #endif // MULTIALPHAFILTER_H
./CppStateObserver/multialphafilter.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "multialphafilter.h" #include <cassert> #pragma GCC diagnostic pop ribi::MultiAlphaFilter::MultiAlphaFilter( const std::vector<double> alphas, const double dt) : m_filters(CreateFilters(alphas,dt)) { } const std::vector<boost::shared_ptr<ribi::AlphaFilter> > ribi::MultiAlphaFilter::CreateFilters( const std::vector<double> alphas, const double dt) { std::vector<boost::shared_ptr<AlphaFilter> > v; for(const double alpha: alphas) { boost::shared_ptr<AlphaFilter> filter(new AlphaFilter(alpha,dt)); assert(filter); v.push_back(filter); } return v; } double ribi::MultiAlphaFilter::GetEstimate() const { return m_filters.back()->GetEstimate(); } void ribi::MultiAlphaFilter::Update(double measurement) { const std::size_t sz = m_filters.size(); for (std::size_t i = 0; i!=sz; ++i) { assert(i < m_filters.size() ); assert(m_filters[i]); m_filters[i]->Update(measurement); //One's output is the next one's output measurement = m_filters[i]->GetEstimate(); } } std::string ribi::MultiAlphaFilter::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::MultiAlphaFilter::GetVersionHistory() noexcept { return { "2013-05-25: version 1.0: initial version", "2013-06-18: version 1.1: derive from FloatingPointStateObserver" }; }
./CppStateObserver/multiintegerstateobserver.h
#ifndef MULTIINTEGERSTATEOBSERVER_H #define MULTIINTEGERSTATEOBSERVER_H #include <vector> #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include <boost/shared_ptr.hpp> #include "integeralphafilter.h" #include "integerstateobserver.h" #pragma GCC diagnostic pop namespace ribi { struct MultiIntegerStateObserver : public IntegerStateObserver { MultiIntegerStateObserver( std::vector<boost::shared_ptr<IntegerStateObserver> >& filters ); ///Get the current state estimate int64_t GetEstimate() const; ///Supply a measurement, which will update the state estimate void Update(const int64_t measurement); ///Obtain the version of this class static std::string GetVersion() noexcept; ///Obtain the version history of this class static std::vector<std::string> GetVersionHistory() noexcept; private: std::vector<boost::shared_ptr<IntegerStateObserver> > m_filters; }; } //~namespace ribi #endif // MULTIINTEGERSTATEOBSERVER_H
./CppStateObserver/multiintegerstateobserver.cpp
#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Weffc++" #include "multiintegerstateobserver.h" #include <cassert> #pragma GCC diagnostic pop ribi::MultiIntegerStateObserver::MultiIntegerStateObserver( std::vector<boost::shared_ptr<IntegerStateObserver> >& filters) : m_filters(filters) { } int64_t ribi::MultiIntegerStateObserver::GetEstimate() const { return m_filters.back()->GetEstimate(); } void ribi::MultiIntegerStateObserver::Update(int64_t measurement) { const std::size_t sz = m_filters.size(); for (std::size_t i = 0; i!=sz; ++i) { assert(i < m_filters.size() ); assert(m_filters[i]); m_filters[i]->Update(measurement); //One's output is the next one's output measurement = m_filters[i]->GetEstimate(); //One's output is the next one's output } } std::string ribi::MultiIntegerStateObserver::GetVersion() noexcept { return "1.1"; } std::vector<std::string> ribi::MultiIntegerStateObserver::GetVersionHistory() noexcept { return { "2013-06-04: version 1.0: initial version", "2013-06-18: version 1.1: derive from IntegerStateObserver" }; }
./CppStateObserver/stateobserverfwd.h
#ifndef STATEOBSERVERFWD_H #define STATEOBSERVERFWD_H namespace ribi { struct AlphaFilter; struct AlphaBetaFilter; struct AlphaBetaGammaFilter; struct FloatingPointStateObserver; struct IntegerStateObserver; struct IntegerAlphaFilter; struct IntegerSmartAlphaFilter; struct IntegerSymmetricalAlphaFilter; struct MultiIntegerStateObserver; struct MultiAlphaFilter; } //~namespace ribi #endif // STATEOBSERVERFWD_H