From 857d7a1553d71123c4a77b6f5b222917d858c693 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 4 Sep 2018 10:49:00 +0200 Subject: [PATCH] fixed some issues added new pose/turn detections new helper classes define-flags for libEigen --- Assertions.h | 7 + CMakeLists.txt | 2 +- Exception.h | 8 +- data/Timestamp.h | 74 +++-- floorplan/3D/objects/MTLReader.h | 9 +- floorplan/3D/objects/OBJReader.h | 5 + geo/Point2.h | 3 + grid/factory/GridImportance.h | 2 +- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 2 +- grid/walk/GridWalkWeighted.h | 2 +- grid/walk/v2/modules/WalkModuleHeading.h | 4 +- .../WalkModuleRelativePressureControl.h | 2 +- main.cpp | 2 +- math/FixedFrequencyInterpolator.h | 2 +- math/Floatingpoint.h | 219 +++++++++++++ math/KahanSum.h | 84 +++++ math/Matrix3.h | 106 ++++-- math/MovingAverageTS.h | 34 +- math/MovingMinMaxTS.h | 73 +++++ math/MovingStdDevTS.h | 11 +- math/Quaternion.h | 197 ++++++++++++ math/distribution/NormalN.h | 67 +++- math/dsp/iir/BiQuad.h | 2 +- math/filter/Complementary.h | 74 +++++ math/stats/SampleRateEstimator.h | 41 +++ sensors/gps/GPSData.h | 13 +- sensors/imu/AccelerometerData.h | 29 +- sensors/imu/CompassData.h | 8 +- sensors/imu/CompassDetection.h | 56 +++- sensors/imu/CompassDetectionPlot.h | 8 +- sensors/imu/GravityData.h | 14 +- sensors/imu/GyroscopeData.h | 11 +- sensors/imu/MagnetometerData.h | 24 +- sensors/imu/PoseDetection.h | 127 ++++++-- sensors/imu/PoseDetection2.h | 301 ++++++++++++++++++ sensors/imu/PoseDetection3.h | 179 +++++++++++ sensors/imu/PoseDetectionPlot.h | 134 ++++++-- sensors/imu/PoseProvider.h | 43 +++ sensors/imu/StepDetection2.h | 12 +- sensors/imu/TurnDetection.h | 40 ++- sensors/imu/TurnDetection2.h | 117 +++++++ sensors/imu/TurnDetectionPlot.h | 45 ++- sensors/imu/TurnProvider.h | 12 + sensors/pressure/BarometerData.h | 47 ++- tests/math/TestDistribution.cpp | 10 +- tests/math/TestQuaternion.cpp | 82 +++++ tests/math/distribution/TestNormalN.cpp | 2 + tests/math/divergence/TestJensenShannon.cpp | 2 + tests/math/divergence/TestKullbackLeibler.cpp | 2 + tests/sensors/imu/TestTurnDetection.cpp | 3 +- .../mixing/TestMixingSamplerDivergency.cpp | 3 +- 51 files changed, 2149 insertions(+), 207 deletions(-) create mode 100644 math/Floatingpoint.h create mode 100644 math/KahanSum.h create mode 100644 math/MovingMinMaxTS.h create mode 100644 math/Quaternion.h create mode 100644 math/filter/Complementary.h create mode 100644 math/stats/SampleRateEstimator.h create mode 100644 sensors/imu/PoseDetection2.h create mode 100644 sensors/imu/PoseDetection3.h create mode 100644 sensors/imu/PoseProvider.h create mode 100644 sensors/imu/TurnDetection2.h create mode 100644 sensors/imu/TurnProvider.h create mode 100644 tests/math/TestQuaternion.cpp diff --git a/Assertions.h b/Assertions.h index 4aba559..08649f2 100644 --- a/Assertions.h +++ b/Assertions.h @@ -70,6 +70,13 @@ namespace Assert { } } + template static inline void isNear(const T v1, const U v2, const V delta, const STR err) { + if (std::abs(v1-v2) > delta) { + std::stringstream ss; ss << "\nexpected " << v1 << " +/- " << delta << " but is " << v2 << "\n"; + doThrow(err+ss.str()); + } + } + template static inline void isBetween(const T v, const T min, const T max, const STR err) { if (v < min || v > max) { std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n"; diff --git a/CMakeLists.txt b/CMakeLists.txt index acfd892..18a9b20 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,7 +119,7 @@ ADD_EXECUTABLE( # needed external libraries TARGET_LINK_LIBRARIES( ${PROJECT_NAME} -# gtest + gtest # pthread ${EXTRA_LIBS} ) diff --git a/Exception.h b/Exception.h index 1e72f78..cd14319 100755 --- a/Exception.h +++ b/Exception.h @@ -5,7 +5,8 @@ #include #ifdef ANDROID -#include +//include +#include #endif class Exception : public std::exception { @@ -22,7 +23,10 @@ public: // TODO better solution? #ifdef ANDROID - QMessageBox::question(nullptr, "Exception", str.c_str(), QMessageBox::Ok); + qDebug() << "-------- ERROR --------"; + qDebug() << str.c_str(); + qDebug() << "------------------------"; + //QMessageBox::question(nullptr, "Exception", str.c_str(), QMessageBox::Ok); #endif } diff --git a/data/Timestamp.h b/data/Timestamp.h index 90fcf4b..8b08707 100644 --- a/data/Timestamp.h +++ b/data/Timestamp.h @@ -11,29 +11,51 @@ struct Timestamp { private: /** internal timestamp in milliseconds */ - int64_t _ms; + //int64_t _ms; + int64_t _us; - /** HIDDEN ctor. use factory methods */ - explicit Timestamp(const int64_t ms) : _ms(ms) {;} + /** hidden ctor. for internal methods only */ + Timestamp(int64_t us) : _us(us) {;} public: /** empty ctor */ - explicit Timestamp() : _ms(0) {;} + explicit Timestamp() : _us(0) {;} + /** get timestamp from the given value which represents microseconds */ + static inline Timestamp fromUS(const int64_t us) { + Timestamp ts; + ts._us = us; + return ts; + } /** get timestamp from the given value which represents milliesconds */ - static inline Timestamp fromMS(const int64_t ms) {return Timestamp(ms);} + static inline Timestamp fromMS(const int64_t ms) { + Timestamp ts; + ts._us = ms * 1000; + return ts; + } /** get timestamp from the given value which represents seconds */ - static inline Timestamp fromSec(const float sec) {return Timestamp(sec*1000);} + static inline Timestamp fromSec(const float sec) { + Timestamp ts; + ts._us = static_cast(sec * 1000 * 1000); + return ts; + } + + /** get timestamp from the given value which represents a sample rate in hz */ + static inline Timestamp fromHz(const float hz) { + const float sec = 1.0f / hz; + return Timestamp::fromSec(sec); + } /** get timestamp for the current unix-time */ static inline Timestamp fromUnixTime() { auto now = std::chrono::system_clock::now(); auto duration = now.time_since_epoch(); - auto millis = std::chrono::duration_cast(duration).count(); - return Timestamp(millis); + //auto millis = std::chrono::duration_cast(duration).count(); + auto micros = std::chrono::duration_cast(duration).count(); + return Timestamp::fromUS(micros); } /** get timestamp for the current system-time */ @@ -46,44 +68,50 @@ public: public: + /** get finest available value */ + inline int64_t finest() const {return _us;} + + /** get timestamp in microseconds */ + inline int64_t us() const {return _us;} + /** get timestamp in milliseconds */ - inline int64_t ms() const {return _ms;} + inline int64_t ms() const {return _us/1000;} /** get timestamp in seconds */ - inline float sec() const {return _ms/1000.0f;} + inline float sec() const {return _us/1000.0f/1000.0f;} public: /** is this timestamp zero? */ - bool isZero() const {return _ms == 0;} + bool isZero() const {return _us == 0;} /** equal? */ - bool operator == (const Timestamp& o) const {return _ms == o._ms;} + bool operator == (const Timestamp& o) const {return _us == o._us;} /** not equal? */ - bool operator != (const Timestamp& o) const {return _ms != o._ms;} + bool operator != (const Timestamp& o) const {return _us != o._us;} /** smaller than the given one? */ - bool operator < (const Timestamp& o) const {return _ms < o._ms;} - bool operator <= (const Timestamp& o) const {return _ms <= o._ms;} + bool operator < (const Timestamp& o) const {return _us < o._us;} + bool operator <= (const Timestamp& o) const {return _us <= o._us;} /** greater than the given one? */ - bool operator > (const Timestamp& o) const {return _ms > o._ms;} - bool operator >= (const Timestamp& o) const {return _ms >= o._ms;} + bool operator > (const Timestamp& o) const {return _us > o._us;} + bool operator >= (const Timestamp& o) const {return _us >= o._us;} - Timestamp operator - (const Timestamp& o) const {return Timestamp(_ms - o._ms);} - Timestamp& operator -= (const Timestamp& o) {_ms += o._ms; return *this;} + Timestamp operator - (const Timestamp& o) const {return Timestamp(_us - o._us);} + Timestamp& operator -= (const Timestamp& o) {_us += o._us; return *this;} - Timestamp operator + (const Timestamp& o) const {return Timestamp(_ms + o._ms);} - Timestamp& operator += (const Timestamp& o) {_ms += o._ms; return *this;} + Timestamp operator + (const Timestamp& o) const {return Timestamp(_us + o._us);} + Timestamp& operator += (const Timestamp& o) {_us += o._us; return *this;} - template Timestamp operator * (const T val) const {return Timestamp(_ms * val);} + template Timestamp operator * (const T val) const {return Timestamp(_us * val);} - template Timestamp operator / (const T val) const {return Timestamp(_ms / val);} + template Timestamp operator / (const T val) const {return Timestamp(_us / val);} // /** cast to float */ // operator float () const {return sec();} diff --git a/floorplan/3D/objects/MTLReader.h b/floorplan/3D/objects/MTLReader.h index 0007b74..21584b8 100644 --- a/floorplan/3D/objects/MTLReader.h +++ b/floorplan/3D/objects/MTLReader.h @@ -29,7 +29,7 @@ public: ; } - /** read .obj from the given file */ + /** read .mtl from the given file */ void readFile(const std::string& file) { std::ifstream is(file); std::string line; @@ -37,7 +37,12 @@ public: is.close(); } - /** read obj from the given data string (.obj file contents) */ + /** read mtl from the given data string (.mtl file contents) */ + void readData(const char* data) { + readData(std::string(data)); + } + + /** read mtl from the given data string (.mtl file contents) */ void readData(const std::string& data) { std::stringstream is(data); std::string line; diff --git a/floorplan/3D/objects/OBJReader.h b/floorplan/3D/objects/OBJReader.h index f9b6d27..dd4476c 100644 --- a/floorplan/3D/objects/OBJReader.h +++ b/floorplan/3D/objects/OBJReader.h @@ -65,6 +65,11 @@ public: is.close(); } + /** read obj from the given data string (.obj file contents) */ + void readData(const char* data) { + readData(std::string(data)); + } + /** read obj from the given data string (.obj file contents) */ void readData(const std::string& data) { std::stringstream is(data); diff --git a/geo/Point2.h b/geo/Point2.h index 16f79e8..cc36d29 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -78,6 +78,9 @@ inline float dot(const Point2 p1, const Point2 p2) { return (p1.x*p2.x) + (p1.y*p2.y); } +inline float determinant(const Point2 p1, const Point2 p2) { + return (p1.x*p2.y) - (p1.y*p2.x); +} inline void swap(Point2& p1, Point2& p2) { std::swap(p1.x, p2.x); diff --git a/grid/factory/GridImportance.h b/grid/factory/GridImportance.h index d374b6e..7ceeb90 100644 --- a/grid/factory/GridImportance.h +++ b/grid/factory/GridImportance.h @@ -11,7 +11,7 @@ #include "../../nav/dijkstra/Dijkstra.h" #include "../../nav/dijkstra/DijkstraPath.h" -#include "../../math/Distributions.h" +#include "../../math/distribution/Normal.h" diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index d59aa97..6920b1c 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -5,7 +5,7 @@ #include "../Grid.h" #include "../../math/DrawList.h" -#include "../../math/Distributions.h" +#include "../../math/distribution/Normal.h" #include "../../math/DrawList.h" #include "../../nav/dijkstra/Dijkstra.h" diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h index 5159398..97459ed 100644 --- a/grid/walk/GridWalkWeighted.h +++ b/grid/walk/GridWalkWeighted.h @@ -5,7 +5,7 @@ #include "../Grid.h" #include "../../math/DrawList.h" -#include "../../math/Distributions.h" +#include "../../math/distribution/Normal.h" #include "../../math/DrawList.h" /** diff --git a/grid/walk/v2/modules/WalkModuleHeading.h b/grid/walk/v2/modules/WalkModuleHeading.h index beb3687..d992499 100644 --- a/grid/walk/v2/modules/WalkModuleHeading.h +++ b/grid/walk/v2/modules/WalkModuleHeading.h @@ -7,7 +7,9 @@ #include "../../../../Assertions.h" #include "../../../../geo/Heading.h" -#include "../../../../math/Distributions.h" +#include "../../../../math/distribution/Normal.h" +#include "../../../../math/distribution/LUT.h" +#include "../../../../math/distribution/VonMises.h" #include "../../../../geo/Heading.h" diff --git a/grid/walk/v2/modules/WalkModuleRelativePressureControl.h b/grid/walk/v2/modules/WalkModuleRelativePressureControl.h index 6e7853d..c3a5aed 100644 --- a/grid/walk/v2/modules/WalkModuleRelativePressureControl.h +++ b/grid/walk/v2/modules/WalkModuleRelativePressureControl.h @@ -6,7 +6,7 @@ #include "WalkStateHeading.h" #include "../../../../geo/Heading.h" -#include "../../../../math/Distributions.h" +#include "../../../../math/distribution/Normal.h" #include "../../../../Assertions.h" diff --git a/main.cpp b/main.cpp index 05b1969..376e399 100755 --- a/main.cpp +++ b/main.cpp @@ -111,7 +111,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "*FIRComplex*"; + ::testing::GTEST_FLAG(filter) = "*Quaternion*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/math/FixedFrequencyInterpolator.h b/math/FixedFrequencyInterpolator.h index 3e386e8..a6d7a13 100644 --- a/math/FixedFrequencyInterpolator.h +++ b/math/FixedFrequencyInterpolator.h @@ -70,7 +70,7 @@ public: while(nextOutput < cur.ts) { // interpolation rate - const float percent = (nextOutput.ms() - last.ts.ms()) / (float) (diff.ms()); + const float percent = (nextOutput.finest() - last.ts.finest()) / (float) (diff.finest()); // sanity checks Assert::isNotNaN(percent, "detected NaN for interpolation"); diff --git a/math/Floatingpoint.h b/math/Floatingpoint.h new file mode 100644 index 0000000..4158e94 --- /dev/null +++ b/math/Floatingpoint.h @@ -0,0 +1,219 @@ +#ifndef FLOATINGPOINT_H +#define FLOATINGPOINT_H + +#include "../Assertions.h" + +template struct FP { + + T val; + +public: + + FP(int val) : val(val) {;} + FP(size_t val) : val(val) {;} + FP(float val) : val(val) {;} + FP(double val) : val(val) {;} + + FP(const FP& o) : val(o.val) {;} + FP(const FP& o) : val(o.val) {;} + + FP(const volatile FP& o) : val(o.val) {;} + FP(const volatile FP& o) : val(o.val) {;} + + //template explicit FP(const FP& o) : val(o.val) {;} + + //operator FP() const {return FP(val);} + operator FP() const {return FP(val);} + + operator float() const {return static_cast(val);} + operator double() const {return static_cast(val);} + + + FP& operator += (const FP o) { + checkAddSub(val, o.val); + val += o.val; + return *this; + } + + FP& operator -= (const FP o) { + checkAddSub(val, o.val); + val -= o.val; + return *this; + } + + FP& operator *= (const FP o) { + val *= o.val; + return *this; + } + + FP& operator /= (const FP o) { + val /= o.val; + return *this; + } + + /** -------- X -------- */ + + FP operator - () const { + return FP(-val); + } + + FP operator + (const FP o) const { + checkAddSub(val, o.val); + return FP(val+o.val); + } + + FP operator - (const FP o) const { + checkAddSub(val, o.val); + return FP(val-o.val); + } + + FP operator * (const FP o) const { + return FP(val*o.val); + } + + FP operator / (const FP o) const { + return FP(val/o.val); + } + + FP& operator = (const FP o) { + this->val = o; + return *this; + } + + + volatile FP& operator = (const FP& o) volatile { + this->val = o; + return *this; + } + + + + bool operator == (const FP o) const { + return val == o.val; + } + + bool operator != (const FP o) const { + return val != o.val; + } + + + + + friend std::ostream& operator << (std::ostream& stream, const FP& fp) { + stream << fp.val; + return stream; + } + +private: + + static inline void checkAddSub(const T a, const T b) { + const int x = expDiff(a,b); + Assert::isTrue(x <= 8, "invalid precision for +/-"); + } + + static inline int expDiff(const T a, const T b) { + int x, y; + frexp(a, &x); + frexp(b, &y); + const int d = (std::max(x,y) - std::min(x,y)) / 3; // 3 exponents in binary per 10 in decimal? + return d; + } + + + +}; + +inline FP operator + (const FP& a, const FP& b) {return FP(a.val + b.val);} +inline FP operator + (const FP& a, const FP& b) {return FP(a.val + b.val);} +inline FP operator + (const FP& a, const float b) {return a + FP(b);} +inline FP operator + (const float a, const FP& b) {return FP(a) + b;} +inline FP operator + (const FP& a, const double b) {return a + FP(b);} +inline FP operator + (const double a, const FP& b) {return FP(a) + b;} +inline FP operator + (const FP& a, const float b) {return a + FP(b);} +inline FP operator + (const float a, const FP& b) {return FP(a) + b;} +inline FP operator + (const FP& a, const double b) {return a + FP(b);} +inline FP operator + (const double a, const FP& b) {return FP(a) + b;} + +inline FP operator - (const FP& a, const FP& b) {return FP(a.val - b.val);} +inline FP operator - (const FP& a, const FP& b) {return FP(a.val - b.val);} +inline FP operator - (const FP& a, const float b) {return a - FP(b);} +inline FP operator - (const float a, const FP& b) {return FP(a) - b;} +inline FP operator - (const FP& a, const double b) {return a - FP(b);} +inline FP operator - (const double a, const FP& b) {return FP(a) - b;} +inline FP operator - (const FP& a, const float b) {return a - FP(b);} +inline FP operator - (const float a, const FP& b) {return FP(a) - b;} +inline FP operator - (const FP& a, const double b) {return a - FP(b);} +inline FP operator - (const double a, const FP& b) {return FP(a) - b;} + +inline FP operator / (const FP& a, const FP& b) {return FP(a.val / b.val);} +inline FP operator / (const FP& a, const FP& b) {return FP(a.val / b.val);} +inline FP operator / (const FP& a, const float b) {return a / FP(b);} +inline FP operator / (const float a, const FP& b) {return FP(a) / b;} +inline FP operator / (const FP& a, const double b) {return a / FP(b);} +inline FP operator / (const double a, const FP& b) {return FP(a) / b;} +inline FP operator / (const FP& a, const float b) {return a / FP(b);} +inline FP operator / (const float a, const FP& b) {return FP(a) / b;} +inline FP operator / (const FP& a, const double b) {return a / FP(b);} +inline FP operator / (const double a, const FP& b) {return FP(a) / b;} + +inline FP operator * (const FP& a, const FP& b) {return FP(a.val * b.val);} +inline FP operator * (const FP& a, const FP& b) {return FP(a.val * b.val);} +inline FP operator * (const FP& a, const float b) {return a * FP(b);} +inline FP operator * (const float a, const FP& b) {return FP(a) * b;} +inline FP operator * (const FP& a, const double b) {return a * FP(b);} +inline FP operator * (const double a, const FP& b) {return FP(a) * b;} +inline FP operator * (const FP& a, const float b) {return a * FP(b);} +inline FP operator * (const float a, const FP& b) {return FP(a) * b;} +inline FP operator * (const FP& a, const double b) {return a * FP(b);} +inline FP operator * (const double a, const FP& b) {return FP(a) * b;} + +inline FP operator / (const FP& a, const int b) {return a / FP(b);} +inline FP operator / (const FP& a, const size_t b) {return a / FP(b);} + +inline FP operator / (const FP& a, const int b) {return a / FP(b);} +inline FP operator / (const FP& a, const size_t b) {return a / FP(b);} + + +//template FP operator + (const FP& a, const U b) {return a + FP(b);} +//template FP operator + (const U a, const FP& b) {return FP(a) + b;} + +//template FP operator - (const FP& a, const U b) {return a - FP(b);} +//template FP operator - (const U a, const FP& b) {return FP(a) - b;} + +//template FP operator * (const FP& a, const U b) {return a * FP(b);} +//template FP operator * (const U a, const FP& b) {return FP(a) * b;} + +//template FP operator / (const FP& a, const U b) {return a / FP(b);} +//template FP operator / (const U a, const FP& b) {return FP(a) / b;} + + + +namespace std { + template FP pow (const FP& x, const FP& y) { + return FP(std::pow(x.val, y.val)); + } + template FP pow (const FP& x, const T& y) { + return FP(std::pow(x.val, y)); + } + template FP sqrt (const FP x) { + return FP(std::sqrt(x.val)); + } + template bool isnan(FP x) { + return std::isnan(x.val); + } +} + +#ifdef WITH_FP + using FPFloat = FP; + using FPDouble = FP; + using FPDefault = FPFloat; +#else + using FPFloat = float; + using FPDouble = double; + using FPDefault = double; +#endif + + + + +#endif // FLOATINGPOINT_H diff --git a/math/KahanSum.h b/math/KahanSum.h new file mode 100644 index 0000000..4ae2587 --- /dev/null +++ b/math/KahanSum.h @@ -0,0 +1,84 @@ +#ifndef KAHANSUM_H +#define KAHANSUM_H + +#include + +template class KahanSum { + +private: + + const T zero; + + /** current sum */ + T sum; + + /** current compensation */ + T comp; + + +public: + + /** ctor with zero element */ + KahanSum(const T zero) : zero(zero), sum(zero), comp(zero) { + + } + + KahanSum& operator += (const T val) { + add(val); + return *this; + } + + KahanSum& operator -= (const T val) { + sub(val); + return *this; + } + + T get() const { + return sum; + } + +private: + +#define NO_OPT __attribute__((optimize("-O0"))) + + /** adjust the sum */ + void add(const T val) NO_OPT { + const T y = val - comp; + const T t = sum + y; + comp = (t-sum) - y; // very important line + sum = t; + } + + /** adjust the sum */ + void sub(const T val) NO_OPT { + const T y = val - comp; + const T t = sum - y; + comp = (t-sum) + y; // very important line + sum = t; + } + +public: + + static T get(const std::vector& values, const T zero = T()) { + return get(values.data(), values.size(), zero); + } + + static T get(const T* values, const size_t cnt, const T zero = T()) { + + T sum = zero; + volatile T comp = zero; + for (size_t i = 0; i < cnt; ++i) { + const T inp = values[i]; + const volatile T y = inp - comp; + const volatile T t = sum + y; + comp = (t-sum) - y; + sum = t; + } + + return sum; + + } + +}; + +#endif // KAHANSUM_H diff --git a/math/Matrix3.h b/math/Matrix3.h index 2ca0b7f..e43eebc 100644 --- a/math/Matrix3.h +++ b/math/Matrix3.h @@ -4,6 +4,7 @@ #include #include +#include "../Assertions.h" class Matrix3 { @@ -24,30 +25,72 @@ public: return Matrix3( {1,0,0, 0,1,0, 0,0,1} ); } -// static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) { -// return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI); -// } + static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) { + return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI); + } -// static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) { + static Matrix3 getRotationRad(const float radX, const float radY, const float radZ) { -// const float g = radX; const float b = radY; const float a = radZ; -// const float a11 = std::cos(a)*std::cos(b); -// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); -// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); -// const float a21 = std::sin(a)*std::cos(b); -// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); -// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); -// const float a31 = -std::sin(b); -// const float a32 = std::cos(b)*std::sin(g); -// const float a33 = std::cos(b)*std::cos(g); -// return Matrix4({ -// a11, a12, a13, 0, -// a21, a22, a23, 0, -// a31, a32, a33, 0, -// 0, 0, 0, 1 -// }); + const float g = radX; const float b = radY; const float a = radZ; + const float a11 = std::cos(a)*std::cos(b); + const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); + const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); + const float a21 = std::sin(a)*std::cos(b); + const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); + const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); + const float a31 = -std::sin(b); + const float a32 = std::cos(b)*std::sin(g); + const float a33 = std::cos(b)*std::cos(g); + return Matrix3({ + a11, a12, a13, + a21, a22, a23, + a31, a32, a33, + }); -// } + } + static Matrix3 getRotationVec(const float nx, const float ny, const float nz, const float mag) { + Assert::isNotNaN(nx, "detected NaN"); + Assert::isNotNaN(ny, "detected NaN"); + Assert::isNotNaN(nz, "detected NaN"); + Assert::isNotNaN(mag, "detected NaN"); + const float c = std::cos(mag); + const float s = std::sin(mag); + return Matrix3({ + c+nx*nx*(1-c), nx*ny*(1-c)+nz*s, nx*nz*(1-c)-ny*s, + ny*nx*(1-c)-nz*s, c+ny*ny*(1-c), ny*nz*(1-c)+nx*s, + nz*nx*(1-c)+ny*s, nz*ny*(1-c)-nx*s, c+nz*nz*(1-c) + }); + } + static Matrix3 getRotationRadX(const float x) { + return Matrix3({ + 1, 0, 0, + 0, cos(x), -sin(x), + 0, sin(x), cos(x) + }); + } + static Matrix3 getRotationRadY(const float y) { + return Matrix3({ + cos(y), 0, sin(y), + 0, 1, 0, + -sin(y),0, cos(y) + }); + } + static Matrix3 getRotationRadZ(const float z) { + return Matrix3({ + cos(z), -sin(z), 0, + sin(z), cos(z), 0, + 0, 0, 1 + }); + } + + + Matrix3 transposed() const { + return Matrix3({ + data[0], data[3], data[6], + data[1], data[4], data[7], + data[2], data[5], data[8] + }); + } static Matrix3 getTranslation(const float x, const float y) { return Matrix3({ @@ -126,6 +169,27 @@ struct Vector3 { return Vector3(x/v, y/v, z/v); } + Vector3& operator += (const Vector3 o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + Vector3& operator -= (const Vector3 o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + +// Vector& operator = (const float val) { +// this->x = val; +// this->y = val; +// this->z = val; +// return *this; +// } + bool operator == (const Vector3 o) const { return (x==o.x) && (y==o.y) && (z==o.z); } diff --git a/math/MovingAverageTS.h b/math/MovingAverageTS.h index dd8b7bd..20200a4 100644 --- a/math/MovingAverageTS.h +++ b/math/MovingAverageTS.h @@ -4,6 +4,7 @@ #include #include "../data/Timestamp.h" #include +#include "KahanSum.h" template class MovingAverageTS { @@ -22,14 +23,19 @@ private: /** the value history for the window-size */ std::vector history; + const T zero; + /** current sum */ T sum; + /** more exact running summation of many values */ + KahanSum kSum; + public: /** ctor with the window-size to use */ - MovingAverageTS(const Timestamp window, const T zeroElement) : window(window), sum(zeroElement) { + MovingAverageTS(const Timestamp window, const T zeroElement) : window(window), zero(zeroElement), sum(zeroElement), kSum(zeroElement) { } @@ -41,6 +47,7 @@ public: // adjust sum sum += data; + kSum += data; // remove too-old history entries const Timestamp oldest = ts - window; @@ -48,6 +55,7 @@ public: // adjust sum sum -= history.front().value; + kSum -= history.front().value; // remove from history history.erase(history.begin()); @@ -56,11 +64,31 @@ public: } - /** get the current average */ - T get() const { + /** get the current average (with increasing error due to float sum!) */ + T getOldAPX() const { return sum / history.size(); } + /** get the current average */ + T get() const { + return kSum.get() / history.size(); + } + +// T get() const { + +// T sum = zero; +// volatile T comp = zero; +// for (const auto e : history) { +// T inp = e.value; +// T y = inp - comp; +// T t = sum + y; +// comp = (t-sum) - y; +// sum = t; +// } + +// return sum / history.size(); + +// } }; diff --git a/math/MovingMinMaxTS.h b/math/MovingMinMaxTS.h new file mode 100644 index 0000000..a99aa19 --- /dev/null +++ b/math/MovingMinMaxTS.h @@ -0,0 +1,73 @@ +#ifndef MOVINGMINMAXTS_H +#define MOVINGMINMAXTS_H + +#include +#include "../data/Timestamp.h" +#include + +template class MovingMinMaxTS { + +private: + + /** timestamp -> value combination */ + struct Entry { + Timestamp ts; + T value; + Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;} + }; + + /** the regional window to use */ + Timestamp window; + + /** the value history for the window-size */ + std::vector history; + + /** current sum */ + T sum; + +public: + + + /** ctor with the window-size to use */ + MovingMinMaxTS(const Timestamp window, const T zeroElement) : window(window), sum(zeroElement) { + + } + + /** add a new entry */ + void add(const Timestamp ts, const T& data) { + + // append to history + history.push_back(Entry(ts, data)); + + // remove too-old history entries + const Timestamp oldest = ts - window; + while(history.front().ts < oldest) { + + // remove from history + history.erase(history.begin()); + + } + + } + + /** get the current min element */ + T getMin() const { + auto comp = [] (const Entry& e1, const Entry& e2) {return e1.value < e2.value;}; + auto it = std::min_element(history.begin(), history.end(), comp); + return it->value; + } + + /** get the current max element */ + T getMax() const { + auto comp = [] (const Entry& e1, const Entry& e2) {return e1.value < e2.value;}; + auto it = std::max_element(history.begin(), history.end(), comp); + return it->value; + } + + T getRange() const { + return getMax() - getMin(); + } + +}; + +#endif // MOVINGMINMAXTS_H diff --git a/math/MovingStdDevTS.h b/math/MovingStdDevTS.h index 4cfd439..f565611 100644 --- a/math/MovingStdDevTS.h +++ b/math/MovingStdDevTS.h @@ -10,8 +10,8 @@ template class MovingStdDevTS { private: - MovingAverageTS avg; - MovingAverageTS avg2; + MovingAverageTS avg; + MovingAverageTS avg2; public: @@ -36,9 +36,10 @@ public: /** get the current std-dev */ T get() const { - const T e = avg.get(); - const T e2 = avg2.get(); - const T var = e2 - e*e; + const double e = avg.get(); + const double e2 = avg2.get(); + const double var = e2 - e*e; + //if (var < 0) {return 0;} return std::sqrt(var); } diff --git a/math/Quaternion.h b/math/Quaternion.h new file mode 100644 index 0000000..c04bea8 --- /dev/null +++ b/math/Quaternion.h @@ -0,0 +1,197 @@ +#ifndef QUATERNION_H +#define QUATERNION_H + +#include "Matrix3.h" + +class Quaternion { + +public: + + float w; + float x, y, z; + + /** empty ctor */ + Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) { + ; + } + + /** valued ctor */ + Quaternion(float w, float x, float y, float z) : w(w), x(x), y(y), z(z) { + ; + } + + /** construct from Euler angles in radians */ + static Quaternion fromEuler(float x, float y, float z) { + Quaternion q; + q.setEuler(x,y,z); + return q; + } + + static Quaternion fromAxisAngle(float angle, float x, float y, float z) { + const float qx = x * std::sin(angle/2); + const float qy = y * std::sin(angle/2); + const float qz = z * std::sin(angle/2); + const float qw = std::cos(angle/2); + return Quaternion(qw, qx,qy,qz); + } + + + void normalize() { + *this *= 1.0 / std::sqrt( x*x + y*y + z*z + w*w ); + } + + inline float dotProduct(const Quaternion& q2) const { + return (x * q2.x) + (y * q2.y) + (z * q2.z) + (w * q2.w); + } + + + /** linear interpolation between q1 and q2 */ + static Quaternion lerp( Quaternion q1, Quaternion q2, float time) { + const float scale = 1.0f - time; + return (q1*scale) + (q2*time); + } + + /** spherical interpolation between q1 and q2 */ + static Quaternion slerp( Quaternion q1, Quaternion q2, float time, float threshold = 0.05) { + + float angle = q1.dotProduct(q2); + + // make sure we use the short rotation + if (angle < 0.0f) { + q1 *= -1.0f; + angle *= -1.0f; + } + + if (angle <= (1-threshold)) { + // spherical interpolation + const float theta = acosf(angle); + const float invsintheta = 1.0/(sinf(theta)); + const float scale = sinf(theta * (1.0f-time)) * invsintheta; + const float invscale = sinf(theta * time) * invsintheta; + return (q1*scale) + (q2*invscale); + } else { + // linear interpolation + return Quaternion::lerp(q1,q2,time); + } + + } + + + /** multiply by scalar */ + Quaternion& operator *= (float val) { + x *= val; + y *= val; + z *= val; + w *= val; + return *this; + } + + /** multiply by scalar */ + Quaternion operator * (float val) const { + return Quaternion(w*val, x*val, y*val, z*val); + } + + /** multiply by other quaterion */ + Quaternion& operator *= (const Quaternion& other) { + return (*this = other * (*this)); + } + + /** multiply by other quaterion */ + Quaternion operator * (const Quaternion& other) const { + Quaternion tmp; + tmp.w = (other.w * w) - (other.x * x) - (other.y * y) - (other.z * z); + tmp.x = (other.w * x) + (other.x * w) + (other.y * z) - (other.z * y); + tmp.y = (other.w * y) + (other.y * w) + (other.z * x) - (other.x * z); + tmp.z = (other.w * z) + (other.z * w) + (other.x * y) - (other.y * x); + return tmp; + } + + /** add two quaternions */ + Quaternion operator + (const Quaternion& b) const { + return Quaternion(w+b.w, x+b.x, y+b.y, z+b.z); + } + + + /** set from euler angles (in radians) */ + void setEuler(float x, float y, float z) { + + double angle; + + angle = x * 0.5; + const double sr = std::sin(angle); + const double cr = std::cos(angle); + + angle = y * 0.5; + const double sp = std::sin(angle); + const double cp = std::cos(angle); + + angle = z * 0.5; + const double sy = std::sin(angle); + const double cy = std::cos(angle); + + const double cpcy = cp * cy; + const double spcy = sp * cy; + const double cpsy = cp * sy; + const double spsy = sp * sy; + + this->x = (float)(sr * cpcy - cr * spsy); + this->y = (float)(cr * spcy + sr * cpsy); + this->z = (float)(cr * cpsy - sr * spcy); + this->w = (float)(cr * cpcy + sr * spsy); + normalize(); + + } + + /** convert to euler angles */ + Vector3 toEuler() const { + + Vector3 res; + + const double sqw = w*w; + const double sqx = x*x; + const double sqy = y*y; + const double sqz = z*z; + const double test = 2.0 * (y*w - x*z); + + if (near(test, 1.0)) { + // heading = rotation about z-axis + res.z = (float) (-2.0*atan2(x, w)); + // bank = rotation about x-axis + res.x = 0; + // attitude = rotation about y-axis + res.y = (float) (M_PI/2.0); + } else if (near(test, -1.0)) { + // heading = rotation about z-axis + res.z = (float) (2.0*std::atan2(x, w)); + // bank = rotation about x-axis + res.x = 0; + // attitude = rotation about y-axis + res.y = (float) (M_PI/-2.0); + } else { + // heading = rotation about z-axis + res.z = (float) std::atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw)); + // bank = rotation about x-axis + res.x = (float) std::atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw)); + // attitude = rotation about y-axis + res.y = (float) std::asin( clamp(test, -1.0, 1.0) ); + } + + return res; + + } + +private: + + static inline bool near(double d1, double d2) { + return std::abs(d1-d2) < 0.00001; + } + + static inline double clamp(double val, double min, double max) { + if (val < min) {return min;} + if (val > max) {return max;} + return val; + } + +}; + +#endif // QUATERNION_H diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index 011f6ae..080922c 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -3,6 +3,7 @@ #include #include +#include #include @@ -20,23 +21,28 @@ namespace Distribution { Eigen::VectorXd mu; Eigen::MatrixXd sigma; - const double _a; - const Eigen::MatrixXd _sigmaInv; + double _a; + Eigen::MatrixXd _sigmaInv; - const Eigen::SelfAdjointEigenSolver eigenSolver; - Eigen::MatrixXd transform; //can i make this const? + Eigen::MatrixXd transform; - Random::RandomGenerator gen; - std::normal_distribution<> dist; + Random::RandomGenerator gen; + std::normal_distribution<> dist; public: - /** ctor */ - NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : - mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()), eigenSolver(sigma) { + /** empty ctor */ + NormalDistributionN() { + ; + } - transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal(); - } + /** ctor */ + NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + + const Eigen::SelfAdjointEigenSolver eigenSolver(sigma); + transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal(); + } /** get probability for the given value */ double getProbability(const Eigen::VectorXd val) const { @@ -50,12 +56,12 @@ namespace Distribution { } /** get the mean vector */ - const Eigen::VectorXd getMu(){ + const Eigen::VectorXd getMu() const { return this->mu; } /** get covariance matrix */ - const Eigen::MatrixXd getSigma(){ + const Eigen::MatrixXd getSigma() const { return this->sigma; } @@ -71,6 +77,34 @@ namespace Distribution { this->mu = mu; } + /** return a NormalN based on given data */ + static NormalDistributionN getNormalNFromSamples(const std::vector>& data) { + + int numAttrs = data[0].size(); + Eigen::MatrixXd eCovar = Eigen::MatrixXd::Zero(numAttrs, numAttrs); + Eigen::VectorXd eSum = Eigen::VectorXd::Zero(numAttrs); + Eigen::VectorXd eAvg = Eigen::VectorXd::Zero(numAttrs); + + // mean + for (const std::vector& vec : data) { + const Eigen::Map eVec((double*)vec.data(), vec.size()); + eSum += eVec; + } + eAvg = eSum / data.size(); + + // covariance + for (const std::vector& vec : data) { + const Eigen::Map eVec((double*)vec.data(), vec.size()); + const Eigen::VectorXd eTmp = (eVec - eAvg); + eCovar += eTmp * eTmp.transpose(); + } + eCovar /= data.size(); + + return NormalDistributionN(eAvg, eCovar); + + } + + /** return a NormalN based on given data */ static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) { @@ -98,7 +132,7 @@ namespace Distribution { return NormalDistributionN(mean, cov); } - std::vector getContour2(const double percentWithin) { + std::vector getContour2(const double percentWithin) const { const int degreesOfFreedom = 2; // 2D distribution const ChiSquared chi(degreesOfFreedom); @@ -114,7 +148,7 @@ namespace Distribution { std::vector res; - std::cout << sigma << std::endl; + //std::cout << sigma << std::endl; Eigen::SelfAdjointEigenSolver solver(this->sigma); Eigen::Vector2d evec1 = solver.eigenvectors().col(0); @@ -127,7 +161,8 @@ namespace Distribution { const float rad = deg / 180.0f * M_PI; Eigen::Vector2d pos = std::cos(rad) * std::sqrt(mul * eval1) * evec1 + - std::sin(rad) * std::sqrt(mul * eval2) * evec2; + std::sin(rad) * std::sqrt(mul * eval2) * evec2 + + this->mu; res.push_back(Point2(pos(0), pos(1))); } diff --git a/math/dsp/iir/BiQuad.h b/math/dsp/iir/BiQuad.h index ad15eb8..896edcd 100644 --- a/math/dsp/iir/BiQuad.h +++ b/math/dsp/iir/BiQuad.h @@ -29,7 +29,7 @@ namespace IIR { /** filter the given amplitude of the given channel (history) */ Scalar filter( const Scalar aIn ) { - Scalar aOut = 0; + Scalar aOut = Scalar(); // zero aOut += aIn *(b0a0); aOut += in[0] *(b1a0); aOut += in[1] *(b2a0); diff --git a/math/filter/Complementary.h b/math/filter/Complementary.h new file mode 100644 index 0000000..3670c15 --- /dev/null +++ b/math/filter/Complementary.h @@ -0,0 +1,74 @@ +#ifndef COMPLEMENTARY_H +#define COMPLEMENTARY_H + +#include "../dsp/iir/BiQuad.h" +#include "../../math/stats/SampleRateEstimator.h" + +namespace Filter { + + template class Complementary { + + private: + + const float fLo; + const float fHi; + + IIR::BiQuad lp; + IIR::BiQuad hp; + + SampleRateEstimator slp; + SampleRateEstimator shp; + + T slow; + T fast; + + public: + + /** ctor with transition frequency and sample-rate */ + Complementary(const float fLo, const float fHi) : fLo(fLo), fHi(fHi), slow(), fast() { + //adjust(); + } + + void addSlow(const Timestamp ts, const T& slow) { + slp.update(ts); + this->slow = lp.filter(slow); + adjustLP(); + } + + void addFast(const Timestamp ts, const T& fast) { + shp.update(ts); + this->fast = hp.filter(fast); + adjustHP(); + } + + T get() const { + return slow+fast; + } + + T getLo() const { + return slow; + } + + T getHi() const { + return fast; + } + + private: + + void adjustLP() { + const float freqLP = slp.getCurHz(); + //std::cout << freqLP << ":" << freqHP << std::endl; + if (freqLP > fLo*2) {lp.setLowPass(fLo, 0.707, freqLP);} + } + + void adjustHP() { + const float freqHP = slp.getCurHz(); + //std::cout << freqLP << ":" << freqHP << std::endl; + if (freqHP > fHi*2) {hp.setHighPass(fHi, 0.707, freqHP);} + } + + }; + +} + +#endif // COMPLEMENTARY_H diff --git a/math/stats/SampleRateEstimator.h b/math/stats/SampleRateEstimator.h new file mode 100644 index 0000000..e5e1164 --- /dev/null +++ b/math/stats/SampleRateEstimator.h @@ -0,0 +1,41 @@ +#ifndef SAMPLERATEESTIMATOR_H +#define SAMPLERATEESTIMATOR_H + +#include "../../data/Timestamp.h" + +class SampleRateEstimator { + +private: + + const double a = 0.99; + double curHz = 0; + Timestamp tsLast; + +public: + + float update(const Timestamp ts) { + + // first + if (tsLast.isZero()) { + tsLast = ts; + return 0; + } + + const double diffSec = static_cast((ts-tsLast).sec()); + tsLast = ts; + + if (diffSec != 0) { + curHz = a*curHz + (1-a) * (1.0/diffSec); + } + + return static_cast(curHz); + + } + + float getCurHz() const { + return static_cast(curHz); + } + +}; + +#endif // SAMPLERATEESTIMATOR_H diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index 80f4360..c3fa77e 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -3,18 +3,19 @@ #include "../../data/Timestamp.h" #include "../../geo/EarthPos.h" +#include "../../math/Floatingpoint.h" struct GPSData { /** time this measurement was received (NOT the GPS-time) */ Timestamp tsReceived; - float lat; // deg - float lon; // deg - float alt; // m above sea-level + FPDefault lat; // deg + FPDefault lon; // deg + FPDefault alt; // m above sea-level - float accuracy; // m [might be NAN] - float speed; // m/s [might be NAN] + FPDefault accuracy; // m [might be NAN] + FPDefault speed; // m/s [might be NAN] /** ctor for invalid/unknown data */ GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;} @@ -49,7 +50,7 @@ struct GPSData { private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index 55e69d4..b71626d 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -3,20 +3,23 @@ #include #include +#include "../../math/Floatingpoint.h" /** data received from an accelerometer sensor */ struct AccelerometerData { - float x; - float y; - float z; + FPDefault x; + FPDefault y; + FPDefault z; AccelerometerData() : x(0), y(0), z(0) {;} - AccelerometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + AccelerometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;} - float magnitude() const { + //AccelerometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;} + + FPDefault magnitude() const { return std::sqrt( x*x + y*y + z*z ); } @@ -34,22 +37,24 @@ struct AccelerometerData { return *this; } - AccelerometerData operator - (const AccelerometerData& o) const { - return AccelerometerData(x-o.x, y-o.y, z-o.z); + + AccelerometerData operator + (const AccelerometerData o) const { + return AccelerometerData(x+o.x, y+o.y, z+o.z); } - AccelerometerData operator / (const float val) const { - return AccelerometerData(x/val, y/val, z/val); + AccelerometerData operator - (const AccelerometerData& o) const { + return AccelerometerData(x-o.x, y-o.y, z-o.z); } AccelerometerData operator * (const float val) const { return AccelerometerData(x*val, y*val, z*val); } - AccelerometerData operator + (const AccelerometerData o) const { - return AccelerometerData(x+o.x, y+o.y, z+o.z); + AccelerometerData operator / (const float val) const { + return AccelerometerData(x/val, y/val, z/val); } + std::string asString() const { std::stringstream ss; ss << "(" << x << "," << y << "," << z << ")"; @@ -68,7 +73,7 @@ struct AccelerometerData { private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/CompassData.h b/sensors/imu/CompassData.h index c336091..551f63d 100644 --- a/sensors/imu/CompassData.h +++ b/sensors/imu/CompassData.h @@ -4,16 +4,16 @@ #include #include - +#include "../../math/Floatingpoint.h" /** data received from a compass sensor */ struct CompassData { /** azimuth angle. NAN if not available */ - float azimuth = NAN; + FPDefault azimuth = NAN; /** describes the sensor's quality */ - float quality01 = 0; + FPDefault quality01 = 0; /** empty ctor */ @@ -49,7 +49,7 @@ struct CompassData { private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/CompassDetection.h b/sensors/imu/CompassDetection.h index 0af0398..2ec5635 100644 --- a/sensors/imu/CompassDetection.h +++ b/sensors/imu/CompassDetection.h @@ -2,18 +2,21 @@ #define INDOOR_IMU_COMPASSDETECTION_H #include "MagnetometerData.h" -#include "PoseDetection.h" +#include "PoseProvider.h" +#include "TurnProvider.h" #include "../../data/Timestamp.h" #include "../../math/MovingAverageTS.h" #include "../../math/MovingStdDevTS.h" #include "../../geo/Point3.h" #include "../../Assertions.h" +#include "../../geo/Angle.h" #include "CompassDetectionPlot.h" #include #include +#include "../../math/dsp/iir/BiQuad.h" /** @@ -33,21 +36,37 @@ private: // Timestamp lastEstimation; // } orientation; - MovingAverageTS avgIn = MovingAverageTS(Timestamp::fromMS(150), MagnetometerData(0,0,0)); + MovingAverageTS avgIn; //MovingStdDevTS stdDev = MovingStdDevTS(Timestamp::fromMS(2000), MagnetometerData(0,0,0)); MovingStdDevTS stdDev = MovingStdDevTS(Timestamp::fromMS(1500), 0); - PoseDetection* pose = nullptr; + + + const PoseProvider* pose = nullptr; + const TurnProvider* turn = nullptr; + int numMagReadings = 0; + bool stable; + + MovingStdDevTS stdDevForSigma = MovingStdDevTS(Timestamp::fromMS(500), 0); + float lastHeading = 0; + float curSigma = 0; + public: /** ctor */ - CompassDetection(PoseDetection* pose) : pose(pose) { + CompassDetection(const PoseProvider* pose, const TurnProvider* turn, const Timestamp avgFrame = Timestamp::fromMS(500)) : + pose(pose), turn(turn), avgIn(avgFrame, MagnetometerData(0,0,0)) { ; } + /** get the current uncertainty estimation */ + float getSigma() { + return curSigma + pose->getSigma() + turn->getSigma(); + } + /** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */ float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) { @@ -73,30 +92,38 @@ public: // calculate angle // https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf - const float mx = curMag.x; - const float my = curMag.y; + const float mx = (curMag.x); + const float my = (curMag.y); const float tmp = std::atan2(my, mx); //const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my)); // http://www.magnetic-declination.com/ // http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html const float declination = 3.0f / 180.0f * M_PI; // GERMANY! - const float curHeading = - tmp + declination; + //const float declination = 0; // GERMANY! + float curHeading = Angle::makeSafe_2PI(- tmp + declination); + float resHeading; - bool stable = true; + stable = true; // calculate standard-deviation within a certain timeframe - stdDev.add(ts, curHeading); + const float curDiff = Angle::getSignedDiffRAD_2PI(curHeading, lastHeading); + stdDev.add(ts, curDiff); + stdDevForSigma.add(ts, curDiff); + curSigma = (5.0f/180.0f*(float)M_PI) + stdDevForSigma.get()*10; + lastHeading = curHeading; + + // if the standard-deviation is too high, // the compass is considered unstable - if (numMagReadings < 250 || stdDev.get() > 0.30) { - resHeading = NAN; - stable = false; - } else { +// if (numMagReadings < 250 || stdDev.get() > 0.30) { +// resHeading = NAN; +// stable = false; +// } else { resHeading = curHeading; stable = true; - } +// } #ifdef WITH_DEBUG_PLOT plot.add(ts, curHeading, stable, mag, curMag); @@ -107,6 +134,7 @@ public: } + }; #endif // INDOOR_IMU_COMPASSDETECTION_H diff --git a/sensors/imu/CompassDetectionPlot.h b/sensors/imu/CompassDetectionPlot.h index 2b4db3b..5c1cbe1 100644 --- a/sensors/imu/CompassDetectionPlot.h +++ b/sensors/imu/CompassDetectionPlot.h @@ -22,7 +22,7 @@ K::Gnuplot gp1; K::Gnuplot gp2; - K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2); + //K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2); K::GnuplotPlot plotMagRaw; K::GnuplotPlotElementLines lineMagRawX; @@ -47,8 +47,8 @@ gp1 << "set autoscale xfix\n"; gp2 << "set size ratio -1\n"; - multiplot.add(&plotMagRaw); - multiplot.add(&plotMagFix); + //multiplot.add(&plotMagRaw); + //multiplot.add(&plotMagFix); plotMagRaw.setTitle("Magnetometer (raw)"); plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x"); @@ -106,7 +106,7 @@ gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; - gp1.draw(multiplot); + gp1.draw(plotMagFix); gp1.flush(); gp2.draw(plotMagScatter); diff --git a/sensors/imu/GravityData.h b/sensors/imu/GravityData.h index 3d87b0b..24fee76 100644 --- a/sensors/imu/GravityData.h +++ b/sensors/imu/GravityData.h @@ -3,20 +3,20 @@ #include #include - +#include "../../math/Floatingpoint.h" /** data received from an accelerometer sensor */ struct GravityData { - float x; - float y; - float z; + FPDefault x; + FPDefault y; + FPDefault z; GravityData() : x(0), y(0), z(0) {;} GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;} - float magnitude() const { + FPDefault magnitude() const { return std::sqrt( x*x + y*y + z*z ); } @@ -38,7 +38,7 @@ struct GravityData { return GravityData(x-o.x, y-o.y, z-o.z); } - GravityData operator / (const float val) const { + GravityData operator / (const FPDefault val) const { return GravityData(x/val, y/val, z/val); } @@ -60,7 +60,7 @@ struct GravityData { private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/GyroscopeData.h b/sensors/imu/GyroscopeData.h index d4f0ee3..5205a72 100644 --- a/sensors/imu/GyroscopeData.h +++ b/sensors/imu/GyroscopeData.h @@ -3,6 +3,7 @@ #include #include +#include "../../math/Floatingpoint.h" /** * data received from a gyroscope sensor @@ -10,14 +11,14 @@ */ struct GyroscopeData { - float x; - float y; - float z; + FPDefault x; + FPDefault y; + FPDefault z; GyroscopeData() : x(0), y(0), z(0) {;} /** ctor from RADIANS */ - GyroscopeData(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + GyroscopeData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;} float magnitude() const { return std::sqrt( x*x + y*y + z*z ); @@ -41,7 +42,7 @@ struct GyroscopeData { private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h index 847ee49..613fa37 100644 --- a/sensors/imu/MagnetometerData.h +++ b/sensors/imu/MagnetometerData.h @@ -4,20 +4,21 @@ #include #include +#include "../../math/Floatingpoint.h" /** * data received from a magnetometer sensor */ struct MagnetometerData { - float x; - float y; - float z; + FPDefault x; + FPDefault y; + FPDefault z; MagnetometerData() : x(0), y(0), z(0) {;} /** ctor from RADIANS */ - MagnetometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + MagnetometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;} std::string asString() const { std::stringstream ss; @@ -29,7 +30,7 @@ struct MagnetometerData { return (x == x) && (y == y) && (z == z); } - bool operator == (const GyroscopeData& o ) const { + bool operator == (const MagnetometerData& o ) const { return EQ_OR_NAN(x, o.x) && EQ_OR_NAN(y, o.y) && EQ_OR_NAN(z, o.z); @@ -53,22 +54,27 @@ struct MagnetometerData { return *this; } - MagnetometerData operator * (const MagnetometerData& o) const { - return MagnetometerData(x*o.x, y*o.y, z*o.z); + + MagnetometerData operator + (const MagnetometerData& o) const { + return MagnetometerData(x+o.x, y+o.y, z+o.z); } MagnetometerData operator - (const MagnetometerData& o) const { return MagnetometerData(x-o.x, y-o.y, z-o.z); } - MagnetometerData operator / (const float val) const { + MagnetometerData operator * (const MagnetometerData& o) const { + return MagnetometerData(x*o.x, y*o.y, z*o.z); + } + + MagnetometerData operator / (const FPDefault val) const { return MagnetometerData(x/val, y/val, z/val); } private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/PoseDetection.h b/sensors/imu/PoseDetection.h index 968c3f1..a990f43 100644 --- a/sensors/imu/PoseDetection.h +++ b/sensors/imu/PoseDetection.h @@ -5,6 +5,7 @@ #include "../../data/Timestamp.h" +#include "../../math/MovingStdDevTS.h" #include "../../math/MovingAverageTS.h" #include "../../math/MovingMedianTS.h" #include "../../math/Matrix3.h" @@ -14,12 +15,13 @@ //#include #include "PoseDetectionPlot.h" +#include "PoseProvider.h" /** * estimate the smartphones world-pose, * based on the accelerometer's data */ -class PoseDetection { +class PoseDetection : public PoseProvider { /** live-pose-estimation using moving average of the smartphone's accelerometer */ @@ -29,7 +31,7 @@ class PoseDetection { MovingAverageTS avg; EstMovingAverage(const Timestamp window) : - avg(MovingAverageTS(window, AccelerometerData())) { + avg(window, AccelerometerData()) { // start approximately addAcc(Timestamp(), AccelerometerData(0,0,9.81)); @@ -45,26 +47,82 @@ class PoseDetection { return avg.get(); } +// /** get the current rotation matrix estimation */ +// //Eigen::Matrix3f get() const { +// Matrix3 get() const { + +// // get the current acceleromter average +// const AccelerometerData avgAcc = avg.get(); +// //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); +// const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z); + +// // rotate average-accelerometer into (0,0,1) +// //Eigen::Vector3f zAxis; zAxis << 0, 0, 1; +// const Vector3 zAxis(0,0,1); +// const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis); +// //const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE +// //const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + +// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) +// //Eigen::Vector3f aligned = (rotMat * avg).normalized(); +// const Vector3 aligned = (rotMat * avg).normalized(); +// Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted"); +// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + +// return rotMat; + +// } + + // FOR TESTING /** get the current rotation matrix estimation */ //Eigen::Matrix3f get() const { Matrix3 get() const { + // https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix // get the current acceleromter average const AccelerometerData avgAcc = avg.get(); //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); - const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z); + const Point3 vZ = Point3(avgAcc.x, avgAcc.y, avgAcc.z).normalized(); + const Point3 vX = cross(Point3(0,1,0), vZ).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x); + //const Point3 v2 = cross(v3, vx).normalized(); + const Point3 vY = cross(vZ, vX).normalized(); - // rotate average-accelerometer into (0,0,1) - //Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - const Vector3 zAxis(0,0,1); - const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis); - //const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE - //const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + Matrix3 rotMat({ + vX.x, vY.x, vZ.x, + vX.y, vY.y, vZ.y, + vX.z, vY.z, vZ.z, + }); + + // above transposed = inverse matrix = undo rotation + rotMat = rotMat.transposed(); + +// // https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix +// // get the current acceleromter average +// const AccelerometerData avgAcc = avg.get(); +// //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); +// const Point3 vZ = Point3(-avgAcc.x, -avgAcc.y, -avgAcc.z).normalized(); +// const Point3 vX = cross(vZ, Point3(0,1,0)).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x); +// //const Point3 v2 = cross(v3, vx).normalized(); +// const Point3 vY = cross(vX, vZ).normalized(); + +// Matrix3 rotMat({ +// vX.x, vY.x, vZ.x, +// vX.y, vY.y, vZ.y, +// vX.z, vY.z, vZ.z, +// }); + +// //rotMat = Matrix3::getRotationDeg(180, 0, 0) * rotMat; +// //rotMat = Matrix3::getRotationDeg(0, 0, 180) * rotMat; + +// // above transposed = inverse matrix = undo rotation +// //rotMat = rotMat.transposed(); // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) - //Eigen::Vector3f aligned = (rotMat * avg).normalized(); - const Vector3 aligned = (rotMat * avg).normalized(); - Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + const Vector3 zAxis(0,0,1); + const Vector3 inp(avgAcc.x, avgAcc.y, avgAcc.z); + const Vector3 aligned = (rotMat * inp).normalized(); + Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted"); + //Assert::isTrue((aligned-zAxis).norm() < 0.10f, "deviation too high"); return rotMat; @@ -120,22 +178,24 @@ class PoseDetection { // }; - private: struct { - //Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); Matrix3 rotationMatrix = Matrix3::identity(); + float curSigma = 0; bool isKnown = false; Timestamp lastEstimation; } orientation; /** how the pose is estimated */ //LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250)); - EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450)); + EstMovingAverage est; //EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300)); + MovingStdDevTS stdDevForSigma = MovingStdDevTS(Timestamp::fromMS(500), 0); + #ifdef WITH_DEBUG_PLOT + int plotLimit = 0; PoseDetectionPlot plot; #endif @@ -143,22 +203,46 @@ private: public: /** ctor */ - PoseDetection() { - ; + PoseDetection(const Timestamp delay = Timestamp::fromMS(450)) : est(delay) { + #ifdef WITH_DEBUG_PLOT + plot.setName("PoseDetection1"); + #endif } /** get the smartphone's rotation matrix */ - const Matrix3& getMatrix() const { + const Matrix3& getMatrix() const override { return orientation.rotationMatrix; } /** is the pose known and stable? */ - bool isKnown() const { + bool isKnown() const override { return orientation.isKnown; } + Matrix3 getMatrixGyro() const { + return Matrix3::identity(); + } + + Matrix3 getMatrixAcc() const { + return orientation.rotationMatrix; + } + + /** current uncertainty */ + float getSigma() const { + return orientation.curSigma; + } + + void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) { + // uncertainty + const Vector3 curAcc = Vector3(acc.x, acc.y, acc.z); + float angleDiff = std::acos(curAcc.normalized().dot(Vector3(0,0,1))); + if (!std::isnan(angleDiff)) { + stdDevForSigma.add(ts, angleDiff); + orientation.curSigma = stdDevForSigma.get()*1; + } + // add accelerometer data est.addAcc(ts, acc); @@ -169,7 +253,10 @@ public: // debug-plot (if configured) #ifdef WITH_DEBUG_PLOT - plot.add(ts, est.getBase(), orientation.rotationMatrix); + if (++plotLimit > 5) { + plot.add(ts, est.getBase(), orientation.rotationMatrix); + plotLimit = 0; + } #endif } diff --git a/sensors/imu/PoseDetection2.h b/sensors/imu/PoseDetection2.h new file mode 100644 index 0000000..e3b6ef3 --- /dev/null +++ b/sensors/imu/PoseDetection2.h @@ -0,0 +1,301 @@ +#ifndef POSEDETECTION2_H +#define POSEDETECTION2_H + + +#include "AccelerometerData.h" +#include "GyroscopeData.h" + +#include "../../data/Timestamp.h" + +//#include "../../math/MovingAverageTS.h" +//#include "../../math/MovingMedianTS.h" +#include "../../math/Matrix3.h" + +#include "../../math/filter/Complementary.h" + +#include "../../geo/Point3.h" + +//#include "../../math/FixedFrequencyInterpolator.h" + +#include "PoseDetectionPlot.h" +#include "PoseProvider.h" + +/** + * estimate the smartphones world-pose, + * based on the accelerometer's data + * + * https://robotics.stackexchange.com/questions/6953/how-to-calculate-euler-angles-from-gyroscope-output + */ +class PoseDetection2 : public PoseProvider { + +private: + + struct { + //Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); + Matrix3 rotationMatrix = Matrix3::identity(); + bool isKnown = false; + Timestamp lastEstimation; + } orientation; + + /** how the pose is estimated */ + //LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250)); + //EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450)); + //EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300)); + + #ifdef WITH_DEBUG_PLOT + PoseDetectionPlot plot; + PoseDetectionPlotAngles plotAngles; + #endif + + Vector3 lastGyroReading; + Timestamp lastGyroReadingTS; + + //FixedFrequencyInterpolator interpolAcc; + //FixedFrequencyInterpolator interpolGyro; + + Filter::Complementary filter; + + //std::vector accBuf; + //std::vector gyroBuf; + + + + Vector3 curAccel; + + bool firstAccel = true; + Vector3 curAccel_rad; + Vector3 curGyroSum_rad; + + //Matrix3 curGyroMatrix = Matrix3::identity(); + + static constexpr float splitFreq = 2.5; + static constexpr float sRate = 50; + static constexpr int sRateTS = 1000/sRate; + + +public: + + /** ctor */ + //PoseDetection2() : interpolAcc(Timestamp::fromMS(sRateTS)), interpolGyro(Timestamp::fromMS(sRateTS)), filter(splitFreq, sRate) { + PoseDetection2() : filter(1, 2.5) { + #ifdef WITH_DEBUG_PLOT + plot.setName("PoseDetection2"); + #endif + } + + /** get the smartphone's rotation matrix */ + const Matrix3& getMatrix() const override { + return orientation.rotationMatrix; + } + + /** is the pose known and stable? */ + bool isKnown() const override { + return orientation.isKnown; + } + + + float angleX_old_rad = 0; + float angleY_old_rad = 0; + + + void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) { + + // https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/#docs + + float angleX_rad = std::atan2( acc.y, acc.z); + float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z)); + float angleZ_rad = 0; + + if (std::abs(angleX_rad-angleX_old_rad) > M_PI) {angleX_rad += 2*M_PI;} + if (std::abs(angleY_rad-angleY_old_rad) > M_PI) {angleY_rad += 2*M_PI;} + + angleX_old_rad = angleX_rad; + angleY_old_rad = angleY_rad; + +// float angleX_rad = std::atan2(acc.y, std::sqrt(acc.x*acc.x + acc.z*acc.z)); +// float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z)); +// float angleZ_rad = 0; + +// Point2 ref(0,1); +// Point2 xz( acc.x, acc.z); +// Point2 yz(-acc.y, acc.z); +// float angleY_rad = std::atan2( determinant(ref,xz), dot(ref,xz) ); +// float angleX_rad = std::atan2( determinant(ref,yz), dot(ref,yz) ); +// float angleZ_rad = 0.0; + +// float angleX_rad = std::atan2(acc.y, acc.z); +// float angleY_rad = -std::atan2(acc.x, acc.z); +// float angleZ_rad = 0;//-std::atan2(acc.y, acc.x); + + + // float angleX2_rad = (angleX1_rad > 0) ? (angleX1_rad - 2*M_PI) : (angleX1_rad + 2*M_PI); +// float angleY2_rad = (angleY1_rad > 0) ? (angleY1_rad - 2*M_PI) : (angleY1_rad + 2*M_PI); +// float angleX_rad = (std::abs(curAngle_rad.x-angleX1_rad) < std::abs(curAccel_rad.x-angleX2_rad)) ? (angleX1_rad) : (angleX2_rad); +// float angleY_rad = (std::abs(curAngle_rad.y-angleY1_rad) < std::abs(curAccel_rad.y-angleY2_rad)) ? (angleY1_rad) : (angleY2_rad); + + curAccel = Vector3(acc.x, acc.y, acc.z); + curAccel_rad = Vector3(angleX_rad, angleY_rad, angleZ_rad); + + //curAccel_rad = Matrix3::getRotationRad(0,0,-angleZ_rad) * Vector3(angleX_rad, angleY_rad, angleZ_rad); + + + if (firstAccel) { + curGyroSum_rad = curAccel_rad; + firstAccel = false; + } + + + + } + + + + + void addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { + + Vector3 vec(gyro.x, gyro.y, gyro.z); + + // ignore the first reading completely, just remember its timestamp + if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return;} + + // time-difference between previous and current reading + const Timestamp curDiff = ts - lastGyroReadingTS; + lastGyroReadingTS = ts; + + // fast sensors might lead to delay = 0 ms. filter those values + if (curDiff.isZero()) {return;} + + // current area + const Vector3 curArea = (lastGyroReading * curDiff.sec()); + +// // update sum +// curAngle_rad += curArea; +// curGyroSum_rad += curArea; + +// // PAPER + const float dx = 1 * lastGyroReading.x + std::sin(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.y + std::cos(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.z; + const float dy = std::cos(curGyroSum_rad.x)*lastGyroReading.y + -std::sin(curGyroSum_rad.x)*lastGyroReading.z; + const float dz = std::sin(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.y + std::cos(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.z; + curGyroSum_rad.x += dx*curDiff.sec(); + curGyroSum_rad.y += dy*curDiff.sec(); + curGyroSum_rad.z += dz*curDiff.sec(); + + +// // PAPER +// const Vector3 n = lastGyroReading / lastGyroReading.norm(); +// const float mag = lastGyroReading.norm() * curDiff.sec(); +// if (mag != 0) { +// curGyroMatrix = Matrix3::getRotationVec(n.x, n.y, n.z, mag).transposed() * curGyroMatrix; +// } + + // DEBUG PLOT + + + // update old reading + lastGyroReading = vec; + + update(ts); + + + + } + + Matrix3 getMatrixGyro() const { + return Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); + } + + Matrix3 getMatrixAcc() const { + return Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z); + } + +private: + + Vector3 curAngle_rad; + int cnt = 0; + + void update(const Timestamp ts) { + + + // complementary filter x = alpha * x + (1-alpha) * y; + const float alpha = 0.98f; + //curAngle_rad = curAngle_rad*alpha + curAccel_rad*(1-alpha); + //curAngle_rad = curAccel_rad; + //curAngle_rad = curGyroSum_rad; + + //std::cout << curGyroSum_rad.x <<" " << curGyroSum_rad.y << " " << curGyroSum_rad.z << std::endl; + + //curAngle_rad = filter.get(); + + filter.addSlow(ts, curAccel_rad); + filter.addFast(ts, curGyroSum_rad); + + static Vector3 fused; + static Vector3 pref; + Vector3 diff = curGyroSum_rad - pref; + fused = (fused+diff) * alpha + curAccel_rad * (1-alpha); + pref = curGyroSum_rad; + + // update + //orientation.rotationMatrix = Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z); //getMatrixFor(cur); + //orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); //getMatrixFor(cur); + //orientation.rotationMatrix = curGyroMatrix; + //orientation.rotationMatrix = Matrix3::getRotationRadZ(curGyroSum_rad.z) * Matrix3::getRotationRadX(curGyroSum_rad.x) * Matrix3::getRotationRadY(curGyroSum_rad.y); + //orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); + orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, 0); //getMatrixFor(cur); + + + orientation.isKnown = true; + orientation.lastEstimation = ts; + + // debug-plot (if configured) + #ifdef WITH_DEBUG_PLOT + plot.add(ts, curAccel, orientation.rotationMatrix); + if (++cnt % 2 == 0) { + plotAngles.addAcc(ts, curAccel_rad.x, curAccel_rad.y); + plotAngles.addGyro(ts, curGyroSum_rad.x, curGyroSum_rad.y); + plotAngles.addFused(ts, fused.x, fused.y); + } + #endif + + } + +// /** get the current rotation matrix estimation */ +// Matrix3 getMatrixFor(const Vector3 cur) const { + +// // rotate average-accelerometer into (0,0,1) +// //Eigen::Vector3f zAxis; zAxis << 0, 0, 1; +// const Vector3 zAxis(0,0,1); +// const Matrix3 rotMat = getRotationMatrix(cur.normalized(), zAxis); +// //const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE +// //const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + +// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) +// //Eigen::Vector3f aligned = (rotMat * avg).normalized(); +// const Vector3 aligned = (rotMat * cur).normalized(); +// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + +// return rotMat; + +// } + +// /** get a matrix that rotates the vector "from" into the vector "to" */ +// static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) { + +// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another + +// const Vector3 v = from.cross(to) / from.cross(to).norm(); +// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() ); + +// Matrix3 A({ +// 0.0f, -v.z, v.y, +// v.z, 0.0f, -v.x, +// -v.y, v.x, 0.0f +// }); + +// return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle))); + +// } + +}; + +#endif // POSEDETECTION2_H diff --git a/sensors/imu/PoseDetection3.h b/sensors/imu/PoseDetection3.h new file mode 100644 index 0000000..b22d8da --- /dev/null +++ b/sensors/imu/PoseDetection3.h @@ -0,0 +1,179 @@ +#ifndef POSEDETECTION3_H +#define POSEDETECTION3_H + +#include "AccelerometerData.h" +#include "GyroscopeData.h" + +#include "../../data/Timestamp.h" +#include "../../math/Matrix3.h" +#include "../../math/Quaternion.h" + +#include "../../geo/Point3.h" + +#include "PoseDetectionPlot.h" +#include "PoseProvider.h" + + +class PoseDetection3 : public PoseProvider { + +private: + + struct { + //Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); + Matrix3 rotationMatrix = Matrix3::identity(); + bool isKnown = false; + Timestamp lastEstimation; + } orientation; + + #ifdef WITH_DEBUG_PLOT + PoseDetectionPlot plot; + PoseDetectionPlotAngles plotAngles; + #endif + + Vector3 lastGyroReading; + Timestamp lastGyroReadingTS; + +public: + + /** ctor */ + PoseDetection3() { + #ifdef WITH_DEBUG_PLOT + plot.setName("PoseDetection3"); + #endif + } + + /** get the smartphone's rotation matrix */ + const Matrix3& getMatrix() const override { + return orientation.rotationMatrix; + } + + /** is the pose known and stable? */ + bool isKnown() const override { + return orientation.isKnown; + } + + + bool first = true; + + Quaternion qGyro = Quaternion(1, 0,0,0); + Quaternion qGyroUpdate; + Quaternion qAccel; + Quaternion qFiltered; + + void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) { + + const Vector3 vec(-acc.x, -acc.y, acc.z); + const Vector3 v = vec.normalized(); + + + const float mag = -std::acos(v.dot(Vector3(0,0,1)));//-std::acos(v.z); << same + const Vector3 n = Vector3(v.y, -v.x, 0).normalized(); + qAccel = Quaternion::fromAxisAngle(mag, n.x, n.y, n.z); + + //const float magY = std::atan2(acc.x, acc.z); + //const float magX = std::atan2(acc.y, acc.z); + //qAccel = Quaternion::fromAxisAngle(magX, 1, 0, 0) * Quaternion::fromAxisAngle(-magY, 0, 1, 0); + + + + //Vector3 dv = qAccel.toEuler(); + //dv.z = 0; + //std::cout << dv.x << " " << dv.y << " " << dv.z << std::endl; + + // for plotting + if (first) { + qGyro = qAccel; + first = false; + } + + int i = 0; (void) i; + + } + + + + + void addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { + + Vector3 vec(gyro.x, gyro.y, gyro.z); + + // ignore the first reading completely, just remember its timestamp + if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return;} + + // time-difference between previous and current reading + const Timestamp curDiff = ts - lastGyroReadingTS; + lastGyroReadingTS = ts; + + // fast sensors might lead to delay = 0 ms. filter those values + if (curDiff.isZero()) {return;} + + // current area + //const Vector3 curArea = (lastGyroReading * curDiff.sec()); + + + // length of the rotation vector + const float theta = curDiff.sec() * lastGyroReading.norm(); + const Vector3 v = lastGyroReading.normalized(); + lastGyroReading = vec; + + if (theta == 0) {return;} + qGyroUpdate = Quaternion::fromAxisAngle(theta, v.x, v.y, v.z);// qUpdate(cos(theta/2), v.x * sin(theta/2), v.y * sin(theta/2), v.z * sin(theta/2)); + + // update + qGyro = qGyroUpdate * qGyro; + + + + update(ts); + + } + + Matrix3 getMatrixAcc() const { + Vector3 r = qAccel.toEuler(); + return Matrix3::getRotationRad(r.x, r.y, r.z); + } + Matrix3 getMatrixGyro() const { + Vector3 r = qGyro.toEuler(); + return Matrix3::getRotationRad(r.x, r.y, r.z); + } + +private: + + Vector3 curAngle_rad; + int cnt = 0; + + void update(const Timestamp ts) { + + + //Quaternion qFused = qGyro; + //qFiltered = qAccel; + //Quaternion qFused = (qGyro * alpha) + (qAccel * (1-alpha)); + qFiltered = Quaternion::lerp(qFiltered*qGyroUpdate, qAccel, 0.05); + //qFiltered = Quaternion::fromAxisAngle(qAccel.w*0.02, qAccel.x, qAccel.y, qAccel.z) * (qFiltered * qGyroUpdate) * 0.98; + + Vector3 fused = qFiltered.toEuler(); + //std::cout << fused.x <<" " << fused.y << " " << fused.z << std::endl; + + // update + orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, fused.z); //getMatrixFor(cur); + + + orientation.isKnown = true; + orientation.lastEstimation = ts; + + // debug-plot (if configured) + #ifdef WITH_DEBUG_PLOT + plot.add(ts, Vector3(0,0,0), orientation.rotationMatrix); + if (++cnt % 2 == 0) { + plotAngles.addAcc(ts, qAccel.toEuler().x, qAccel.toEuler().y); + plotAngles.addGyro(ts, qGyro.toEuler().x, qGyro.toEuler().y); + plotAngles.addFused(ts, fused.x, fused.y); + } + #endif + + } + + +}; + +#endif // POSEDETECTION3_H diff --git a/sensors/imu/PoseDetectionPlot.h b/sensors/imu/PoseDetectionPlot.h index 46b0d8f..8d86d71 100644 --- a/sensors/imu/PoseDetectionPlot.h +++ b/sensors/imu/PoseDetectionPlot.h @@ -17,19 +17,103 @@ #include "../../math/Matrix3.h" #include "AccelerometerData.h" + class PoseDetectionPlotAngles { + + Timestamp plotRef; + Timestamp lastPlot; + + K::Gnuplot gp; + + K::GnuplotPlot plotAcc; + + K::GnuplotPlotElementLines lineAccX; + K::GnuplotPlotElementLines lineAccY; + + K::GnuplotPlotElementLines lineGyroX; + K::GnuplotPlotElementLines lineGyroY; + + K::GnuplotPlotElementLines lineFusedX; + K::GnuplotPlotElementLines lineFusedY; + + public: + + PoseDetectionPlotAngles() { + + gp << "set autoscale xfix\n"; + + plotAcc.setTitle("Accelerometer"); + + plotAcc.add(&lineFusedX); lineFusedX.getStroke().getColor().setHexStr("#990000"); lineFusedX.setTitle("FusedX");lineFusedX.getStroke().setWidth(3); + plotAcc.add(&lineFusedY); lineFusedY.getStroke().getColor().setHexStr("#09900"); lineFusedY.setTitle("FusedY"); lineFusedY.getStroke().setWidth(3); + + plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff8888"); lineAccX.setTitle("AccX"); lineAccX.getStroke().setType(K::GnuplotDashtype::DOTTED); lineAccX.getStroke().setWidth(2); + plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#88ff88"); lineAccY.setTitle("AccY"); lineAccY.getStroke().setType(K::GnuplotDashtype::DOTTED); lineAccY.getStroke().setWidth(2); + + plotAcc.add(&lineGyroX); lineGyroX.getStroke().getColor().setHexStr("#ff8888"); lineGyroX.setTitle("GyroX"); lineGyroX.getStroke().setType(K::GnuplotDashtype::DASHED); lineGyroX.getStroke().setWidth(2); + plotAcc.add(&lineGyroY); lineGyroY.getStroke().getColor().setHexStr("#88ff88"); lineGyroY.setTitle("GyroY"); lineGyroY.getStroke().setType(K::GnuplotDashtype::DASHED); lineGyroY.getStroke().setWidth(2); + + plotAcc.getKey().setVisible(true); + + } + + void addAcc(Timestamp ts, float x, float y) { + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), x) ); + lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), y) ); + } + + void addGyro(Timestamp ts, float x, float y) { + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + lineGyroX.add( K::GnuplotPoint2(tsPlot.ms(), x) ); + lineGyroY.add( K::GnuplotPoint2(tsPlot.ms(), y) ); + } + + void addFused(Timestamp ts, float x, float y) { + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + lineFusedX.add( K::GnuplotPoint2(tsPlot.ms(), x) ); + lineFusedY.add( K::GnuplotPoint2(tsPlot.ms(), y) ); + if (++cnt % 40 == 0) {flush(ts);} + } + + + + private: + + int cnt = 0; + + void flush(Timestamp ts) { + cleanup(ts); + gp.draw(plotAcc); + gp.flush(); + } + + // remove old entries + void cleanup(Timestamp ts) { + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(3000); + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineAccX.removeIf(remove); + lineAccY.removeIf(remove); + lineGyroX.removeIf(remove); + lineGyroY.removeIf(remove); + lineFusedX.removeIf(remove); + lineFusedY.removeIf(remove); + } + + + + }; + class PoseDetectionPlot { Timestamp plotRef; Timestamp lastPlot; - K::Gnuplot gp1; K::Gnuplot gp2; - K::GnuplotPlot plotAcc; - K::GnuplotPlotElementLines lineAccX; - K::GnuplotPlotElementLines lineAccY; - K::GnuplotPlotElementLines lineAccZ; - K::GnuplotSplot plotPose; K::GnuplotSplotElementLines linePose; //K::GnuplotSplotElementEmpty emptyPose; @@ -41,14 +125,9 @@ /** ctor */ PoseDetectionPlot() { - gp1 << "set autoscale xfix\n"; + //gp1 << "set autoscale xfix\n"; gp2 << "set view equal xyz\n"; - plotAcc.setTitle("Accelerometer"); - plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX"); - plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY"); - plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ"); - plotPose.setTitle("Pose"); plotPose.getView().setEnabled(false); plotPose.add(&linePose); @@ -58,6 +137,10 @@ plotPose.getAxisY().setRange(-8,+8); plotPose.getAxisZ().setRange(-8,+8); + plotPose.getAxisX().setLabel("x"); + plotPose.getAxisY().setLabel("y"); + plotPose.getAxisZ().setLabel("z"); + const float a = 0.05; const float b = 0.95; pose = { {{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden @@ -85,17 +168,20 @@ } + void setName(const std::string& name) { + plotPose.setTitle(name); + } + + void add(Timestamp ts, const Vector3& avg, const Matrix3& rotation) { + add(ts, AccelerometerData(avg.x, avg.y, avg.z), rotation); + } + void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) { if (plotRef.isZero()) {plotRef = ts;} const Timestamp tsPlot = (ts-plotRef); const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); - // acc - lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) ); - lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) ); - lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) ); - if (lastPlot + Timestamp::fromMS(50) < tsPlot) { lastPlot = tsPlot; @@ -128,20 +214,18 @@ const Vector3 vx = rotation * Vector3(2,0,0); const Vector3 vy = rotation * Vector3(0,3,0); const Vector3 vz = rotation * Vector3(0,0,5); + const Vector3 vA = Vector3(avg.x, avg.y, -avg.z) * 1; linePose.clear(); linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z)); linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z)); linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z)); + linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vA.x, vA.y, vA.z)); - // remove old accelerometer entries - auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; - lineAccX.removeIf(remove); - lineAccY.removeIf(remove); - lineAccZ.removeIf(remove); + gp2 << "set label 91 at " << vx.x << "," << vx.y << "," << vx.z << " 'x' \n"; + gp2 << "set label 92 at " << vy.x << "," << vy.y << "," << vy.z << " 'y' \n"; + gp2 << "set label 93 at " << vz.x << "," << vz.y << "," << vz.z << " 'z' \n"; + gp2 << "set label 99 at " << vA.x << "," << vA.y << "," << vA.z << " 'accel' \n"; - // raw accelerometer - gp1.draw(plotAcc); - gp1.flush(); // 3D pose gp2.draw(plotPose); diff --git a/sensors/imu/PoseProvider.h b/sensors/imu/PoseProvider.h new file mode 100644 index 0000000..37cc8b5 --- /dev/null +++ b/sensors/imu/PoseProvider.h @@ -0,0 +1,43 @@ +#ifndef POSEPROVIDER_H +#define POSEPROVIDER_H + +#include "../../math/Matrix3.h" + +class PoseProvider { + +public: + + virtual const Matrix3& getMatrix() const = 0; + + virtual float getSigma() const = 0; + + virtual bool isKnown() const = 0; + +}; + + +class PoseProviderDummy : public PoseProvider { + + Matrix3 mat = Matrix3::identity(); + +public: + + virtual const Matrix3& getMatrix() const override { + return mat; + } + + Matrix3 getMatrixGyro() const { + return mat; + } + + Matrix3 getMatrixAcc() const { + return mat; + } + + virtual bool isKnown() const override { + return true; + } + +}; + +#endif // POSEPROVIDER_H diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h index 0bb354b..9df73df 100644 --- a/sensors/imu/StepDetection2.h +++ b/sensors/imu/StepDetection2.h @@ -1,7 +1,6 @@ #ifndef STEPDETECTION2_H #define STEPDETECTION2_H - #include "AccelerometerData.h" #include "../../data/Timestamp.h" @@ -52,6 +51,8 @@ private: float threshold = 0.50; + float curFiltered = 0; + #ifdef WITH_DEBUG_PLOT K::Gnuplot gp; K::GnuplotPlot plot; @@ -109,6 +110,10 @@ public: } + float getCurFiltered() const { + return curFiltered; + } + /** does the given data indicate a step? */ bool add(const Timestamp ts, const AccelerometerData& acc) { @@ -118,6 +123,7 @@ public: auto onResample = [&] (const Timestamp ts, const AccelerometerData data) { const float mag = data.magnitude(); + Assert::isNotNaN(mag, "detected NaN magnitude"); // use long-term average to center around zero avg.add(ts, mag); @@ -128,6 +134,8 @@ public: // if (f != f) {return;} const float f = biquad.filter(mag0); const float fMag = f; + curFiltered = fMag; + Assert::isNotNaN(fMag, "detected NaN filtered magnitude"); const bool isMax = locMax.add(fMag); @@ -173,6 +181,8 @@ public: }; + //qDebug() << ts.ms() << " ---" << acc.x << " " << acc.y << " " << acc.z; + // ensure fixed sampling rate for FIR freq filters to work! interpol.add(ts, acc, onResample); diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 7e932d6..794f1f9 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.h @@ -4,11 +4,11 @@ #include "GyroscopeData.h" #include "AccelerometerData.h" #include "../../data/Timestamp.h" -#include "../../math/MovingAverageTS.h" +#include "../../math/MovingStdDevTS.h" #include "../../math/Matrix3.h" #include "../../geo/Point3.h" -#include "PoseDetection.h" +#include "PoseProvider.h" //#include @@ -19,12 +19,13 @@ #include "../../Assertions.h" +#include "TurnProvider.h" -class TurnDetection { +class TurnDetection : public TurnProvider { private: - PoseDetection* pose = nullptr; + PoseProvider* pose = nullptr; //std::vector gyroData; //Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); @@ -32,6 +33,10 @@ private: Timestamp lastGyroReading; + float curSigma = 0; + + MovingStdDevTS stdDevForSigma = MovingStdDevTS(Timestamp::fromMS(500), 0); + #ifdef WITH_DEBUG_PLOT TurnDetectionPlot plot; #endif @@ -39,7 +44,7 @@ private: public: /** ctor */ - TurnDetection(PoseDetection* pose) : pose(pose) { + TurnDetection(PoseProvider* pose) : pose(pose) { ; } @@ -65,6 +70,11 @@ public: // } driftEst; + /** get the current uncertainty estimation */ + float getSigma() const override { + return curSigma; + } + float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { // ignore the first reading completely, just remember its timestamp @@ -90,17 +100,20 @@ public: const Vector3 curGyro = pose->getMatrix() * vec; //driftEst.removeDrift(ts, curGyro); + // area //const Eigen::Vector3f area = const Vector3 area = - // Trapezoid rule (should be more accurate but does not always help?!) - //(prevGyro * curDiff.sec()) + // squared region - //((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality) + // Trapezoid rule (should be more accurate but does not always help?!) + //(prevGyro * curDiff.sec()) + // squared region + //((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality) - // just the rectangular region - (prevGyro * curDiff.sec()); // BEST?! + // average (is the same as above) + //((curGyro+prevGyro)/2 * curDiff.sec()); + // just the rectangular region + (prevGyro * curDiff.sec()); // BEST?! //} @@ -112,9 +125,14 @@ public: const float delta = area.z; #ifdef WITH_DEBUG_PLOT - plot.add(ts, delta, gyro, curGyro); + plot.addRelative(ts, delta, gyro, curGyro); #endif + //stdDevForSigma.add(ts, prevGyro.z); // ignore delta T. directly us radians-per-sec as sigma + //curSigma = stdDevForSigma.get(); + const float radPerSec = 1.0f / 180.0f * M_PI; + curSigma = radPerSec + std::abs(prevGyro.z * 0.05); // constant of 1deg/sec + 5% of current turn rate + // done return delta; diff --git a/sensors/imu/TurnDetection2.h b/sensors/imu/TurnDetection2.h new file mode 100644 index 0000000..e15d451 --- /dev/null +++ b/sensors/imu/TurnDetection2.h @@ -0,0 +1,117 @@ +#ifndef TURNDETECTION2_H +#define TURNDETECTION2_H + + +#include "GyroscopeData.h" +#include "AccelerometerData.h" +#include "../../data/Timestamp.h" +#include "../../math/MovingAverageTS.h" +#include "../../math/Matrix3.h" + +#include "../../geo/Point3.h" +#include "PoseProvider.h" + +//#include + +#include +#include + +#include "TurnDetectionPlot.h" + + +#include "../../Assertions.h" + +/** THIS WILL NOT WORK AS EXPECTED! */ +class TurnDetection2 { + +private: + + PoseProvider* pose = nullptr; + + Vector3 sumSinceStart = Vector3(0,0,0); + + Timestamp lastGyroReadingTS; + Vector3 lastGyroReading = Vector3(0,0,0); + + bool first = true; + Matrix3 poseMat = Matrix3::identity(); + + float curRes = 0; + + #ifdef WITH_DEBUG_PLOT + TurnDetectionPlot plot; + #endif + +public: + + /** ctor */ + TurnDetection2(PoseProvider* pose) : pose(pose) { + ; + } + + + float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { + + + + // ignore the first reading completely, just remember its timestamp + if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return curRes;} + + // time-difference between previous and current reading + const Timestamp curDiff = ts - lastGyroReadingTS; + lastGyroReadingTS = ts; + + // fast sensors might lead to delay = 0 ms. filter those values + if (curDiff.isZero()) {return curRes;} + + // current area + //const Eigen::Vector3f area = + const Vector3 curArea = + + // Trapezoid rule (should be more accurate but does not always help?!) + //(prevGyro * curDiff.sec()) + // squared region + //((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality) + + // just the rectangular region + (lastGyroReading * curDiff.sec()); // BEST?! + + //} + + // adjust sum + sumSinceStart = sumSinceStart + curArea; + + // update the previous value + lastGyroReading = Vector3(gyro.x, gyro.y, gyro.z); + + + // TESTING + const float sign = (curArea.z < 0) ? (-1) : (+1); + const float mag = curArea.norm(); + return (mag * sign); + + +// // ignore readings until the first orientation-estimation is available +// // otherwise we would use a wrong rotation matrix which yields wrong results! +// if (!pose->isKnown()) {return curRes;} +// if (first) {poseMat = pose->getMatrix(); first = false;} + +// // rotate the sum since start into our desired coordinate system, where the smartphone lies flat on the ground +// const Vector3 angles = pose->getMatrix() * sumSinceStart; +// //const Vector3 angles = poseMat * sumSinceStart; + +// // rotation = z-axis only! +// //const float delta = area(2); +// curRes = angles.z; + +// #ifdef WITH_DEBUG_PLOT +// plot.addAbsolute(ts, curRes, gyro, sumSinceStart); +// #endif + +// // done +// return curRes; + + } + +}; + +#endif // TURNDETECTION2_H diff --git a/sensors/imu/TurnDetectionPlot.h b/sensors/imu/TurnDetectionPlot.h index 4f76849..0f78847 100644 --- a/sensors/imu/TurnDetectionPlot.h +++ b/sensors/imu/TurnDetectionPlot.h @@ -69,7 +69,7 @@ } - void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) { + void addRelative(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) { plotCurHead += delta; @@ -114,6 +114,49 @@ } + void addAbsolute(Timestamp ts, const float curHead, const GyroscopeData& gyro, const Vector3& gyroFixed) { + + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); + + // raw gyro + lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) ); + lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) ); + lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) ); + + // adjusted gyro + lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) ); + lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) ); + lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) ); + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + + lastPlot = tsPlot; + + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineGyroRawX.removeIf(remove); + lineGyroRawY.removeIf(remove); + lineGyroRawZ.removeIf(remove); + lineGyroFixX.removeIf(remove); + lineGyroFixY.removeIf(remove); + lineGyroFixZ.removeIf(remove); + + const float ax = 0.85 + std::cos(curHead)*0.1; + const float ay = 0.85 + std::sin(curHead)*0.1; + gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; + gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; + + //gp1.draw(plotGyroRaw); // raw only + gp1.draw(plotGyroFix); // fixed only + //gp1.draw(multiplot); // both + + gp1.flush(); + + } + + } + }; #endif diff --git a/sensors/imu/TurnProvider.h b/sensors/imu/TurnProvider.h new file mode 100644 index 0000000..7db1cdc --- /dev/null +++ b/sensors/imu/TurnProvider.h @@ -0,0 +1,12 @@ +#ifndef TURN_PROVIDER_H +#define TURN_PROVIDER_H + +class TurnProvider { + +public: + + virtual float getSigma() const = 0; + +}; + +#endif // TURN_PROVIDER_H diff --git a/sensors/pressure/BarometerData.h b/sensors/pressure/BarometerData.h index 23d17c6..931418d 100644 --- a/sensors/pressure/BarometerData.h +++ b/sensors/pressure/BarometerData.h @@ -3,28 +3,67 @@ #include +#include "../../math/Floatingpoint.h" /** data received from a barometer sensor */ struct BarometerData { - float hPa; + FPDefault hPa; explicit BarometerData() : hPa(0) {;} - explicit BarometerData(const float hPa) : hPa(hPa) {;} + explicit BarometerData(const FPDefault hPa) : hPa(hPa) {;} /** valid data? */ bool isValid() const { - return hPa == hPa; + return !std::isnan(hPa); } bool operator == (const BarometerData& o ) const { return EQ_OR_NAN(hPa, o.hPa); } +public: + + static constexpr double R = 8.31447; // universal gas constant for air, newton meter / mol kelvin + static constexpr double L = 0.0065; // standard temperature lapse rate, degree kelven per meter + static constexpr double g = 9.80665; // gravity constant, meter per square second + static constexpr double M = 0.0289644; // molar mass for dry air, kg / mol + static constexpr double P0 = 1013.25; // pressure at mean sea level, hPa + static constexpr double T0 = 288.15; // temperature at mean sea level, kelvin + + /** hPa->meter assuming a constant 1013.25 at 0m as reference */ + static FPDefault hpaToMeter(const FPDefault pressure) { + return static_cast( + (T0/L) * (std::pow(static_cast(pressure)/P0, -(R*L)/(g*M))-1.0) + ); + } + + /** hPa->meter by using the given pressure pRef at hRef as reference */ + static FPDefault hpaToMeter(const FPDefault pressure, const FPDefault PRef, const FPDefault hRef) { + return static_cast( + hRef + (T0/L) * (std::pow(static_cast(pressure)/PRef, -(R*L)/(g*M))-1.0) + ); + } + + /** meter->hPa assuming a constant 1013.25 at 0m as reference */ + static FPDefault meterTohPa(const FPDefault altitude) { + return static_cast( + P0 * std::pow(T0 / (T0+L*altitude), (g*M)/(R*L)) + ); + } + + /** meter->hPa by using the given pressure pRef at hRef as reference */ + static FPDefault meterTohPa(const FPDefault altitude, const FPDefault PRef, const FPDefault hRef) { + return static_cast( + PRef * std::pow(T0 / (T0+L*(altitude-hRef)), (g*M)/(R*L)) + ); + } + + private: - static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp index 68d5655..f12cd42 100644 --- a/tests/math/TestDistribution.cpp +++ b/tests/math/TestDistribution.cpp @@ -1,7 +1,15 @@ #ifdef WITH_TESTS #include "../Tests.h" -#include "../../math/Distributions.h" +#include "../../math/distribution/Normal.h" +#include "../../math/distribution/Bessel.h" +#include "../../math/distribution/LUT.h" +#include "../../math/distribution/VonMises.h" +#include "../../math/distribution/Exponential.h" +#include "../../math/distribution/NormalCDF.h" +#include "../../math/distribution/Region.h" +#include "../../math/distribution/Triangle.h" + #include "../../misc/Time.h" template void benchDist(Dist dist) { diff --git a/tests/math/TestQuaternion.cpp b/tests/math/TestQuaternion.cpp new file mode 100644 index 0000000..50a82fa --- /dev/null +++ b/tests/math/TestQuaternion.cpp @@ -0,0 +1,82 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/Quaternion.h" + +const float d = 0.00001; + +TEST(Quaternion, fromEuler) { + + Quaternion q1 = Quaternion::fromEuler(0,0,0); + + +} + +TEST(Quaternion, mul1) { + + Quaternion q1 = Quaternion::fromEuler(0.5,0,0); + Quaternion q2 = Quaternion::fromEuler(0.3,0,0); + Quaternion q3 = q1*q2; + ASSERT_NEAR(0.8, q3.toEuler().x, d); + ASSERT_NEAR(0.0, q3.toEuler().y, d); + ASSERT_NEAR(0.0, q3.toEuler().z, d); + +} + +TEST(Quaternion, mul2) { + + Quaternion q1 = Quaternion::fromEuler(0.5, 0.0, 0.0); + Quaternion q2 = Quaternion::fromEuler(0.0, 0.3, 0.0); + Quaternion q3 = Quaternion::fromEuler(0.0, 0.0, 0.2); + Quaternion q4 = q1*q2*q3; + ASSERT_NEAR(0.5, q4.toEuler().x, d); + ASSERT_NEAR(0.3, q4.toEuler().y, d); + ASSERT_NEAR(0.2, q4.toEuler().z, d); + +} + +TEST(Quaternion, toEuler) { + + + + Quaternion q1 = Quaternion::fromEuler(0,0,0); + ASSERT_NEAR(0, q1.toEuler().x, d); + ASSERT_NEAR(0, q1.toEuler().y, d); + ASSERT_NEAR(0, q1.toEuler().z, d); + + { + Quaternion q2 = Quaternion::fromEuler(0.2, 1.0, 1.2); + ASSERT_NEAR(0.2, q2.toEuler().x, d); + ASSERT_NEAR(1.0, q2.toEuler().y, d); + ASSERT_NEAR(1.2, q2.toEuler().z, d); + } + + { + Quaternion q = Quaternion::fromEuler(0.5, 0, 0); + ASSERT_NEAR(0.5, q.toEuler().x, d); + ASSERT_NEAR(0.0, q.toEuler().y, d); + ASSERT_NEAR(0.0, q.toEuler().z, d); + }{ + Quaternion q = Quaternion::fromEuler(0, 0.5, 0); + ASSERT_NEAR(0.0, q.toEuler().x, d); + ASSERT_NEAR(0.5, q.toEuler().y, d); + ASSERT_NEAR(0.0, q.toEuler().z, d); + }{ + Quaternion q = Quaternion::fromEuler(0, 0, 0.5); + ASSERT_NEAR(0.0, q.toEuler().x, d); + ASSERT_NEAR(0.0, q.toEuler().y, d); + ASSERT_NEAR(0.5, q.toEuler().z, d); + }{ + Quaternion q = Quaternion::fromEuler(-0.5, 0, 0.5); + ASSERT_NEAR(-0.5, q.toEuler().x, d); + ASSERT_NEAR(0.0, q.toEuler().y, d); + ASSERT_NEAR(0.5, q.toEuler().z, d); + } + +} + + + + + +#endif diff --git a/tests/math/distribution/TestNormalN.cpp b/tests/math/distribution/TestNormalN.cpp index 39f79e6..9899ce2 100644 --- a/tests/math/distribution/TestNormalN.cpp +++ b/tests/math/distribution/TestNormalN.cpp @@ -1,4 +1,5 @@ #ifdef WITH_TESTS +#ifdef WITH_EIGEN #include "../../Tests.h" #include "../../../math/divergence/KullbackLeibler.h" @@ -74,4 +75,5 @@ TEST(NormalN, multivariateGaussSampleData) { } #endif +#endif diff --git a/tests/math/divergence/TestJensenShannon.cpp b/tests/math/divergence/TestJensenShannon.cpp index 4bf8cf3..0841e54 100644 --- a/tests/math/divergence/TestJensenShannon.cpp +++ b/tests/math/divergence/TestJensenShannon.cpp @@ -1,4 +1,5 @@ #ifdef WITH_TESTS +#ifdef WITH_EIGEN #include "../../Tests.h" #include "../../../math/divergence/JensenShannon.h" @@ -92,3 +93,4 @@ TEST(JensenShannon, generalFromSamples) { } #endif +#endif diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp index 6f066cb..f8585b6 100644 --- a/tests/math/divergence/TestKullbackLeibler.cpp +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -1,4 +1,5 @@ #ifdef WITH_TESTS +#ifdef WITH_EIGEN #include "../../Tests.h" #include "../../../math/divergence/KullbackLeibler.h" @@ -301,3 +302,4 @@ TEST(KullbackLeibler, generalFromSamples) { } #endif +#endif diff --git a/tests/sensors/imu/TestTurnDetection.cpp b/tests/sensors/imu/TestTurnDetection.cpp index 3f79dad..049ed4a 100644 --- a/tests/sensors/imu/TestTurnDetection.cpp +++ b/tests/sensors/imu/TestTurnDetection.cpp @@ -1,5 +1,6 @@ #ifdef WITH_TESTS +#ifdef TODO #include "../../Tests.h" #include "../../../sensors/imu/TurnDetection.h" @@ -67,5 +68,5 @@ TEST(TurnDetection, xx) { } - +#endif #endif diff --git a/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp index 159161d..ef6beba 100644 --- a/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp +++ b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp @@ -1,4 +1,5 @@ #ifdef WITH_TESTS +#ifdef WITH_EIGEN #include "../../../../smc/merging/mixing/MixingSamplerDivergency.h" #include "../../../../smc/filtering/ParticleFilterMixing.h" @@ -142,5 +143,5 @@ namespace K { } - +#endif #endif