FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
math/FixedFrequencyInterpolator.h
View File

@@ -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");

219
math/Floatingpoint.h Normal file
View File

@@ -0,0 +1,219 @@
#ifndef FLOATINGPOINT_H
#define FLOATINGPOINT_H
#include "../Assertions.h"
template <typename T> 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<float>& o) : val(o.val) {;}
FP(const FP<double>& o) : val(o.val) {;}
FP(const volatile FP<float>& o) : val(o.val) {;}
FP(const volatile FP<double>& o) : val(o.val) {;}
//template <typename U> explicit FP(const FP<U>& o) : val(o.val) {;}
//operator FP<float>() const {return FP<float>(val);}
operator FP<double>() const {return FP<double>(val);}
operator float() const {return static_cast<float>(val);}
operator double() const {return static_cast<double>(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<T>& 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<double> operator + (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val + b.val);}
inline FP<double> operator + (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val + b.val);}
inline FP<float> operator + (const FP<float>& a, const float b) {return a + FP<float>(b);}
inline FP<float> operator + (const float a, const FP<float>& b) {return FP<float>(a) + b;}
inline FP<double> operator + (const FP<float>& a, const double b) {return a + FP<double>(b);}
inline FP<double> operator + (const double a, const FP<float>& b) {return FP<double>(a) + b;}
inline FP<double> operator + (const FP<double>& a, const float b) {return a + FP<double>(b);}
inline FP<double> operator + (const float a, const FP<double>& b) {return FP<double>(a) + b;}
inline FP<double> operator + (const FP<double>& a, const double b) {return a + FP<double>(b);}
inline FP<double> operator + (const double a, const FP<double>& b) {return FP<double>(a) + b;}
inline FP<double> operator - (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val - b.val);}
inline FP<double> operator - (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val - b.val);}
inline FP<float> operator - (const FP<float>& a, const float b) {return a - FP<float>(b);}
inline FP<float> operator - (const float a, const FP<float>& b) {return FP<float>(a) - b;}
inline FP<double> operator - (const FP<float>& a, const double b) {return a - FP<double>(b);}
inline FP<double> operator - (const double a, const FP<float>& b) {return FP<double>(a) - b;}
inline FP<double> operator - (const FP<double>& a, const float b) {return a - FP<double>(b);}
inline FP<double> operator - (const float a, const FP<double>& b) {return FP<double>(a) - b;}
inline FP<double> operator - (const FP<double>& a, const double b) {return a - FP<double>(b);}
inline FP<double> operator - (const double a, const FP<double>& b) {return FP<double>(a) - b;}
inline FP<double> operator / (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val / b.val);}
inline FP<double> operator / (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val / b.val);}
inline FP<float> operator / (const FP<float>& a, const float b) {return a / FP<float>(b);}
inline FP<float> operator / (const float a, const FP<float>& b) {return FP<float>(a) / b;}
inline FP<double> operator / (const FP<float>& a, const double b) {return a / FP<double>(b);}
inline FP<double> operator / (const double a, const FP<float>& b) {return FP<double>(a) / b;}
inline FP<double> operator / (const FP<double>& a, const float b) {return a / FP<double>(b);}
inline FP<double> operator / (const float a, const FP<double>& b) {return FP<double>(a) / b;}
inline FP<double> operator / (const FP<double>& a, const double b) {return a / FP<double>(b);}
inline FP<double> operator / (const double a, const FP<double>& b) {return FP<double>(a) / b;}
inline FP<double> operator * (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val * b.val);}
inline FP<double> operator * (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val * b.val);}
inline FP<float> operator * (const FP<float>& a, const float b) {return a * FP<float>(b);}
inline FP<float> operator * (const float a, const FP<float>& b) {return FP<float>(a) * b;}
inline FP<double> operator * (const FP<float>& a, const double b) {return a * FP<double>(b);}
inline FP<double> operator * (const double a, const FP<float>& b) {return FP<double>(a) * b;}
inline FP<double> operator * (const FP<double>& a, const float b) {return a * FP<double>(b);}
inline FP<double> operator * (const float a, const FP<double>& b) {return FP<double>(a) * b;}
inline FP<double> operator * (const FP<double>& a, const double b) {return a * FP<double>(b);}
inline FP<double> operator * (const double a, const FP<double>& b) {return FP<double>(a) * b;}
inline FP<float> operator / (const FP<float>& a, const int b) {return a / FP<float>(b);}
inline FP<float> operator / (const FP<float>& a, const size_t b) {return a / FP<float>(b);}
inline FP<double> operator / (const FP<double>& a, const int b) {return a / FP<double>(b);}
inline FP<double> operator / (const FP<double>& a, const size_t b) {return a / FP<double>(b);}
//template <typename T, typename U> FP<T> operator + (const FP<T>& a, const U b) {return a + FP<T>(b);}
//template <typename T, typename U> FP<T> operator + (const U a, const FP<T>& b) {return FP<T>(a) + b;}
//template <typename T, typename U> FP<T> operator - (const FP<T>& a, const U b) {return a - FP<T>(b);}
//template <typename T, typename U> FP<T> operator - (const U a, const FP<T>& b) {return FP<T>(a) - b;}
//template <typename T, typename U> FP<T> operator * (const FP<T>& a, const U b) {return a * FP<T>(b);}
//template <typename T, typename U> FP<T> operator * (const U a, const FP<T>& b) {return FP<T>(a) * b;}
//template <typename T, typename U> FP<T> operator / (const FP<T>& a, const U b) {return a / FP<T>(b);}
//template <typename T, typename U> FP<T> operator / (const U a, const FP<T>& b) {return FP<T>(a) / b;}
namespace std {
template<class T> FP<T> pow (const FP<T>& x, const FP<T>& y) {
return FP<T>(std::pow(x.val, y.val));
}
template<class T> FP<T> pow (const FP<T>& x, const T& y) {
return FP<T>(std::pow(x.val, y));
}
template<class T> FP<T> sqrt (const FP<T> x) {
return FP<T>(std::sqrt(x.val));
}
template<class T> bool isnan(FP<T> x) {
return std::isnan(x.val);
}
}
#ifdef WITH_FP
using FPFloat = FP<float>;
using FPDouble = FP<double>;
using FPDefault = FPFloat;
#else
using FPFloat = float;
using FPDouble = double;
using FPDefault = double;
#endif
#endif // FLOATINGPOINT_H

84
math/KahanSum.h Normal file
View File

@@ -0,0 +1,84 @@
#ifndef KAHANSUM_H
#define KAHANSUM_H
#include <vector>
template <typename T> 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<T>& 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

106
math/Matrix3.h
View File

@@ -4,6 +4,7 @@
#include <initializer_list>
#include <cmath>
#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);
}

34
math/MovingAverageTS.h
View File

@@ -4,6 +4,7 @@
#include <vector>
#include "../data/Timestamp.h"
#include <algorithm>
#include "KahanSum.h"
template <typename T> class MovingAverageTS {
@@ -22,14 +23,19 @@ private:
/** the value history for the window-size */
std::vector<Entry> history;
const T zero;
/** current sum */
T sum;
/** more exact running summation of many values */
KahanSum<T> 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();
// }
};

73
math/MovingMinMaxTS.h Normal file
View File

@@ -0,0 +1,73 @@
#ifndef MOVINGMINMAXTS_H
#define MOVINGMINMAXTS_H
#include <vector>
#include "../data/Timestamp.h"
#include <algorithm>
template <typename T> 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<Entry> 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

11
math/MovingStdDevTS.h
View File

@@ -10,8 +10,8 @@ template <typename T> class MovingStdDevTS {
private:
MovingAverageTS<T> avg;
MovingAverageTS<T> avg2;
MovingAverageTS<double> avg;
MovingAverageTS<double> 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);
}

197
math/Quaternion.h Normal file
View File

@@ -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

67
math/distribution/NormalN.h
View File

@@ -3,6 +3,7 @@
#include <cmath>
#include <random>
#include <vector>
#include <eigen3/Eigen/Dense>
@@ -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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<std::vector<double>>& 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<double>& vec : data) {
const Eigen::Map<Eigen::VectorXd> eVec((double*)vec.data(), vec.size());
eSum += eVec;
}
eAvg = eSum / data.size();
// covariance
for (const std::vector<double>& vec : data) {
const Eigen::Map<Eigen::VectorXd> 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<Point2> getContour2(const double percentWithin) {
std::vector<Point2> getContour2(const double percentWithin) const {
const int degreesOfFreedom = 2; // 2D distribution
const ChiSquared<double> chi(degreesOfFreedom);
@@ -114,7 +148,7 @@ namespace Distribution {
std::vector<Point2> res;
std::cout << sigma << std::endl;
//std::cout << sigma << std::endl;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> 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)));
}

2
math/dsp/iir/BiQuad.h
View File

@@ -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);

74
math/filter/Complementary.h Normal file
View File

@@ -0,0 +1,74 @@
#ifndef COMPLEMENTARY_H
#define COMPLEMENTARY_H
#include "../dsp/iir/BiQuad.h"
#include "../../math/stats/SampleRateEstimator.h"
namespace Filter {
template <typename T> class Complementary {
private:
const float fLo;
const float fHi;
IIR::BiQuad<T> lp;
IIR::BiQuad<T> 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

41
math/stats/SampleRateEstimator.h Normal file
View File

@@ -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<double>((ts-tsLast).sec());
tsLast = ts;
if (diffSec != 0) {
curHz = a*curHz + (1-a) * (1.0/diffSec);
}
return static_cast<float>(curHz);
}
float getCurHz() const {
return static_cast<float>(curHz);
}
};
#endif // SAMPLERATEESTIMATOR_H