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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
toni
2018-11-24 17:09:55 +01:00
parent 1dac907004 63bc8131ac
commit 7af6277da5
275 changed files with 5107 additions and 697 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
sensors/MACAddress.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef MACADDRESS_H
#define MACADDRESS_H

10
sensors/beacon/Beacon.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACON_H
#define BEACON_H

10
sensors/beacon/BeaconMeasurement.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONMEASUREMENT_H
#define BEACONMEASUREMENT_H

10
sensors/beacon/BeaconMeasurements.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONMEASUREMENTS_H
#define BEACONMEASUREMENTS_H

10
sensors/beacon/BeaconProbability.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONPROBABILITY_H
#define BEACONPROBABILITY_H

10
sensors/beacon/BeaconProbabilityFree.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONPROBABILITYFREE_H
#define BEACONPROBABILITYFREE_H

10
sensors/beacon/model/BeaconModel.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONMODEL_H
#define BEACONMODEL_H

10
sensors/beacon/model/BeaconModelLogDist.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONMODELLOGDIST_H
#define BEACONMODELLOGDIST_H

10
sensors/beacon/model/BeaconModelLogDistCeiling.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef BEACONMODELLOGDISTCEILING_H
#define BEACONMODELLOGDISTCEILING_H

23
sensors/gps/GPSData.h
View File

@@ -1,20 +1,31 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef GPSDATA_H
#define GPSDATA_H
#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 +60,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) );}
};

10
sensors/gps/GPSProbability.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef GPSPROBABILITY_H
#define GPSPROBABILITY_H

39
sensors/imu/AccelerometerData.h
View File

@@ -1,22 +1,35 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef ACCELEROMETERDATA_H
#define ACCELEROMETERDATA_H
#include <cmath>
#include <sstream>
#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 +47,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 +83,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) );}
};

19
sensors/imu/CompassData.h
View File

@@ -1,19 +1,28 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef COMPASSDATA_H
#define COMPASSDATA_H
#include <cmath>
#include <sstream>
#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 +58,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) );}
};

66
sensors/imu/CompassDetection.h
View File

@@ -1,19 +1,32 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_COMPASSDETECTION_H
#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 <cmath>
#include <vector>
#include "../../math/dsp/iir/BiQuad.h"
/**
@@ -33,21 +46,37 @@ private:
// Timestamp lastEstimation;
// } orientation;
MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
MovingAverageTS<MagnetometerData> avgIn;
//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
PoseDetection* pose = nullptr;
const PoseProvider* pose = nullptr;
const TurnProvider* turn = nullptr;
int numMagReadings = 0;
bool stable;
MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(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 +102,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 +144,7 @@ public:
}
};
#endif // INDOOR_IMU_COMPASSDETECTION_H

18
sensors/imu/CompassDetectionPlot.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H
#define INDOOR_IMU_COMPASSDETECTIONPLOT_H
@@ -22,7 +32,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 +57,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 +116,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);

24
sensors/imu/GravityData.h
View File

@@ -1,22 +1,32 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef GRAVITYDATA_H
#define GRAVITYDATA_H
#include <cmath>
#include <sstream>
#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 +48,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 +70,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) );}
};

21
sensors/imu/GyroscopeData.h
View File

@@ -1,8 +1,19 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef GYROSCOPEDATA_H
#define GYROSCOPEDATA_H
#include <cmath>
#include <sstream>
#include "../../math/Floatingpoint.h"
/**
* data received from a gyroscope sensor
@@ -10,14 +21,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 +52,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) );}
};

10
sensors/imu/LinearAccelerationData.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef LINEARACCELERATIONDATA_H
#define LINEARACCELERATIONDATA_H

34
sensors/imu/MagnetometerData.h
View File

@@ -1,23 +1,34 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_MAGNETOMETERDATA_H
#define INDOOR_IMU_MAGNETOMETERDATA_H
#include <cmath>
#include <sstream>
#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 +40,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 +64,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) );}
};

10
sensors/imu/MotionDetection.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef MOTIONDETECTION_H
#define MOTIONDETECTION_H

481
sensors/imu/PoseDetection.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_POSEDETECTION_H
#define INDOOR_IMU_POSEDETECTION_H
@@ -5,6 +15,7 @@
#include "../../data/Timestamp.h"
#include "../../math/MovingStdDevTS.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingMedianTS.h"
#include "../../math/Matrix3.h"
@@ -14,67 +25,13 @@
//#include <eigen3/Eigen/Dense>
#include "PoseDetectionPlot.h"
#include "PoseProvider.h"
/**
* estimate the smartphones world-pose,
* based on the accelerometer's data
*/
class PoseDetection {
// struct LongTermTriggerAverage {
// Eigen::Vector3f sum;
// int cnt;
// XYZ() {
// reset();
// }
// /** add the given accelerometer reading */
// void addAcc(const Timestamp ts, const AccelerometerData& acc) {
// // did NOT improve the result for every smartphone (only some)
// //const float deltaMag = std::abs(acc.magnitude() - 9.81);
// //if (deltaMag > 5.0) {return;}
// // adjust sum and count (for average calculation)
// Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
// sum += vec;
// ++cnt;
// }
// AccelerometerData getAvg() const {
// return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
// }
// /** get the current rotation matrix estimation */
// Eigen::Matrix3f get() const {
// // get the current acceleromter average
// const Eigen::Vector3f avg = sum / cnt;
// // rotate average accelerometer into (0,0,1)
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
// 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();
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
// return rotMat;
// }
// /** reset the current sum etc. */
// void reset() {
// cnt = 0;
// sum = Eigen::Vector3f::Zero();
// }
// };
class PoseDetection : public PoseProvider {
/** live-pose-estimation using moving average of the smartphone's accelerometer */
@@ -84,7 +41,7 @@ class PoseDetection {
MovingAverageTS<AccelerometerData> avg;
EstMovingAverage(const Timestamp window) :
avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {
avg(window, AccelerometerData()) {
// start approximately
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
@@ -100,26 +57,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;
@@ -127,70 +140,72 @@ class PoseDetection {
};
/** live-pose-estimation using moving median of the smartphone's accelerometer */
struct EstMovingMedian {
// /** live-pose-estimation using moving median of the smartphone's accelerometer */
// struct EstMovingMedian {
// median the accelerometer
MovingMedianTS<float> medianX;
MovingMedianTS<float> medianY;
MovingMedianTS<float> medianZ;
// // median the accelerometer
// MovingMedianTS<float> medianX;
// MovingMedianTS<float> medianY;
// MovingMedianTS<float> medianZ;
EstMovingMedian(const Timestamp window) :
medianX(window), medianY(window), medianZ(window) {
// EstMovingMedian(const Timestamp window) :
// medianX(window), medianY(window), medianZ(window) {
// start approximately
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
// // start approximately
// addAcc(Timestamp(), AccelerometerData(0,0,9.81));
}
// }
/** add the given accelerometer reading */
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
medianX.add(ts, acc.x);
medianY.add(ts, acc.y);
medianZ.add(ts, acc.z);
}
// /** add the given accelerometer reading */
// void addAcc(const Timestamp ts, const AccelerometerData& acc) {
// medianX.add(ts, acc.x);
// medianY.add(ts, acc.y);
// medianZ.add(ts, acc.z);
// }
AccelerometerData getBase() const {
return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
}
// AccelerometerData getBase() const {
// return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
// }
/** get the current rotation matrix estimation */
//Eigen::Matrix3f get() const {
Matrix3 get() const {
// /** get the current rotation matrix estimation */
// //Eigen::Matrix3f get() const {
// Matrix3 get() const {
const Vector3 base(medianX.get(), medianY.get(), medianZ.get());
// const Vector3 base(medianX.get(), medianY.get(), medianZ.get());
// rotate average-accelerometer into (0,0,1)
const Vector3 zAxis(0,0,1);
const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
// // rotate average-accelerometer into (0,0,1)
// const Vector3 zAxis(0,0,1);
// const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
const Vector3 aligned = (rotMat * base).normalized();
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
// const Vector3 aligned = (rotMat * base).normalized();
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
return rotMat;
// return rotMat;
}
};
// }
// };
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<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
#ifdef WITH_DEBUG_PLOT
int plotLimit = 0;
PoseDetectionPlot plot;
#endif
@@ -198,27 +213,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 */
// Eigen::Matrix3f getMatrix() const {
// return orientation.rotationMatrix;
// }
/** 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);
@@ -229,29 +263,16 @@ 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
}
public:
// /** get a matrix that rotates the vector "from" into the vector "to" */
// static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
// const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
// Eigen::Matrix3f A; A <<
// 0, -x(2), x(1),
// x(2), 0, -x(0),
// -x(1), x(0), 0;
// return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
// }
/** get a matrix that rotates the vector "from" into the vector "to" */
static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {
@@ -271,208 +292,6 @@ public:
}
// /** get a rotation matrix for the given x,y,z rotation (in radians) */
// static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
// const float g = x; const float b = y; const float a = z;
// 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);
// Eigen::Matrix3f m;
// m <<
// a11, a12, a13,
// a21, a22, a23,
// a31, a32, a33;
// ;
// return m;
// }
// /** estimate the smartphones current holding position */
// void estimateHolding2() {
// // z-axis points through the device and is the axis we are interested in
// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
// avgAcc = Eigen::Vector3f::Zero();
// for (const AccelerometerData& acc : accData) {
// //for (const GyroscopeData& acc : gyroData) {
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
// avgAcc += vec;
// }
// //avgAcc /= accData.size();
// avgAcc = avgAcc.normalized();
// Eigen::Vector3f rev; rev << 0,0,1;
// rotMat = getRotationMatrix(avgAcc, rev);
// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
//// Eigen::Vector3f re = rotMat * avgAcc;
//// Eigen::Vector3f diff = rev-re;
//// Assert::isTrue(diff.norm() < 0.001, "rotation error");
// }
// struct RotationMatrixEstimationUsingAccAngle {
// Eigen::Vector3f lastAvg;
// Eigen::Vector3f avg;
// int cnt;
// RotationMatrixEstimationUsingAccAngle() {
// reset();
// }
// void add(const float x, const float y, const float z) {
// Eigen::Vector3f vec; vec << x,y,z;
// avg += vec;
// ++cnt;
// }
// void reset() {
// cnt = 0;
// avg = Eigen::Vector3f::Zero();
// }
// Eigen::Matrix3f get() {
// // http://www.hobbytronics.co.uk/accelerometer-info
// avg /= cnt;
// lastAvg = avg;
// //const float mag = avg.norm();
// const float baseX = 0;
// const float baseY = 0;
// const float baseZ = 0; // mag?
// const float x = avg(0) - baseX;
// const float y = avg(1) - baseY;
// const float z = avg(2) - baseZ;
// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax?
// // sanity check
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
// Eigen::Vector3f aligned = (rotMat * avg).normalized();
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
// // int i = 0; (void) i;
// reset();
// return rotMat;
// }
// } est;
// struct PCA {
// Eigen::Vector3f avg;
// Eigen::Vector3f lastAvg;
// Eigen::Matrix3f covar;
// int cnt = 0;
// PCA() {
// reset();
// }
// void add(const float x, const float y, const float z) {
// Eigen::Vector3f vec; vec << x,y,z;
// avg += vec;
// covar += vec*vec.transpose();
// ++cnt;
// }
// Eigen::Matrix3f get() {
// avg /= cnt;
// covar /= cnt;
// lastAvg = avg;
// std::cout << avg << std::endl;
// Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
// solver.eigenvalues();
// solver.eigenvectors();
// const auto eval = solver.eigenvalues();
// const auto evec = solver.eigenvectors();
// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
// Eigen::Matrix3f rotMat;
// rotMat.col(0) = evec.col(0);
// rotMat.col(1) = evec.col(1);
// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue
// rotMat.transposeInPlace();
// //Eigen::Vector3f gy; gy << 0, 30, 30;
// Eigen::Vector3f avg1 = rotMat * avg;
// int i = 0; (void) i;
// reset();
// return rotMat;
// }
// void reset() {
// cnt = 0;
// avg = Eigen::Vector3f::Zero();
// covar = Eigen::Matrix3f::Zero();
// }
// } pca1;
// /** estimate the smartphones current holding position */
// void estimateHolding() {
// Eigen::Vector3f avg = Eigen::Vector3f::Zero();
// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
// for (const AccelerometerData& acc : accData) {
//// for (const GyroscopeData& acc : gyroData) {
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
// avg += vec;
// covar += vec * vec.transpose();
// }
// avg /= accData.size(); // TODO
// covar /= accData.size(); //TODO
// avgAcc = avg.normalized();
};

311
sensors/imu/PoseDetection2.h Normal file
View File

@@ -0,0 +1,311 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#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<Vector3> interpolAcc;
//FixedFrequencyInterpolator<Vector3> interpolGyro;
Filter::Complementary<Vector3> filter;
//std::vector<Vector3> accBuf;
//std::vector<Vector3> 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

189
sensors/imu/PoseDetection3.h Normal file
View File

@@ -0,0 +1,189 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#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

157
sensors/imu/PoseDetectionPlot.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H
#define INDOOR_IMU_POSEDETECTIONPLOT_H
@@ -17,19 +27,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 +135,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 +147,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 +178,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;
@@ -111,24 +207,35 @@
}
}
// // update un-rotated 3D smartphone model
// for (size_t i = 0; i < pose.size(); ++i) {
// K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear();
// for (const std::vector<float>& pts : pose[i]) {
// const Vector3 vec1(pts[0], pts[1], pts[2]);
// const Vector3 vec2 = vec1 - Vector3(0.5, 0.5, 0.5); // center cube at 0,0,0
// const Vector3 vec3 = vec2 * Vector3(7, 15, 1); // stretch cube
// const Vector3 vec4 = rotation * vec3;
// gp->add(K::GnuplotCoordinate3(vec4.x, vec4.y, vec4.z, K::GnuplotCoordinateSystem::FIRST));
// }
// }
// add coordinate system
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);

53
sensors/imu/PoseProvider.h Normal file
View File

@@ -0,0 +1,53 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#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

10
sensors/imu/StepDetection.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef STEPDETECTION_H
#define STEPDETECTION_H

69
sensors/imu/StepDetection2.h
View File

@@ -1,7 +1,16 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef STEPDETECTION2_H
#define STEPDETECTION2_H
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
@@ -22,8 +31,9 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/fir/Real.h"
#include "../../math/dsp/fir/RealFactory.h"
#include "../../math/dsp/iir/BiQuadStack.h"
//#include "../../math/dsp/fir/Real.h"
//#include "../../math/dsp/fir/RealFactory.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/LocalMaxima.h"
#include "../../math/MovingAverageTS.h"
@@ -36,19 +46,22 @@
*/
class StepDetection2 {
static constexpr int sRate_hz = 75;
static constexpr int sRate_hz = 100;
static constexpr int every_ms = 1000 / sRate_hz;
private:
FixedFrequencyInterpolator<AccelerometerData> interpol;
FIR::Real::Filter fir;
//FIR::Real::Filter fir;
IIR::BiQuadStack<float> biquad;
LocalMaxima locMax;
// longterm average to center around zero
MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
const float threshold = 0.50;
float threshold = 0.50;
float curFiltered = 0;
#ifdef WITH_DEBUG_PLOT
K::Gnuplot gp;
@@ -69,16 +82,28 @@ private:
public:
/** ctor */
StepDetection2() : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
StepDetection2(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
//fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz
//fir.shiftBy(2.00); // typical step freq ~2Hz
//fir.lowPass(3.5, 25); // everything up to 3 HZ
FIR::Real::Factory fac(sRate_hz);
fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
//FIR::Real::Factory fac(sRate_hz);
//fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";
@@ -95,31 +120,37 @@ public:
}
float getCurFiltered() const {
return curFiltered;
}
/** does the given data indicate a step? */
bool add(const Timestamp ts, const AccelerometerData& acc) {
bool step = false;
bool gotStep = false;
// accel-data incoming on a fixed sampling rate (needed for FIR to work)
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);
const float mag0 = mag - avg.get();
//const std::complex<float> c = fir.append(mag0);
//const float real = c.real();
//if (real != real) {return;}
//const float fMag = real;
const float f = fir.append(mag0);
if (f != f) {return;}
// const float f = fir.append(mag0);
// 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);
step = (isMax) && (locMax.get() > threshold);
const bool step = (isMax) && (locMax.get() > threshold);
if (step) {gotStep = true;}
#ifdef WITH_DEBUG_OUTPUT
if (step) {
@@ -160,10 +191,12 @@ 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);
return step;
return gotStep;
}

54
sensors/imu/StepDetection3.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef STEPDETECTION3_H
#define STEPDETECTION3_H
@@ -22,7 +32,7 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/iir/BiQuad.h"
#include "../../math/dsp/iir/BiQuadStack.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/DelayBuffer.h"
@@ -38,9 +48,12 @@ class StepDetection3 {
static constexpr float gravity = 9.81;
static constexpr float stepRate_hz = 2.0;
static constexpr float cutOff_hz = 3.0;
static constexpr float iirQ = 0.70;
static constexpr int sRate_hz = 100;
static constexpr int every_ms = 1000 / sRate_hz;
static constexpr float threshold = 1.0;
float threshold = 0.8;
float max = 0;
Timestamp maxTS;
@@ -48,8 +61,8 @@ class StepDetection3 {
private:
FixedFrequencyInterpolator<AccelerometerData> interpol;
IIR::BiQuad<float> biquad;
DelayBuffer<float> delay;
IIR::BiQuadStack<float> biquad;
DelayBuffer<float> lookBehind;
@@ -72,10 +85,23 @@ private:
public:
/** ctor */
StepDetection3() : interpol(Timestamp::fromMS(every_ms)), delay(10) {
StepDetection3(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), lookBehind(5) {
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
biquad.preFill(gravity);
//biquad.setBandPass(stepRate_hz, 1.5, sRate_hz);
//biquad.preFill(gravity);
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";
@@ -105,10 +131,10 @@ public:
const float mag = data.magnitude();
// apply filter
const float fMag = biquad.filter(mag);
const float fMag = biquad.filter(mag - 9.81); // remove gravity
// history buffer
float fMagOld = delay.add(fMag);
float fMagOld = lookBehind.add(fMag);
// zero crossing?
float tmp = max;
@@ -117,16 +143,22 @@ public:
step = true;
gotStep = true;
}
delay.setAll(0);
lookBehind.setAll(0);
max = 0;
}
// track maximum value
if (fMag > max) {max = fMag; maxTS = ts;}
#ifdef WITH_DEBUG_OUTPUT
if (step) {
std::cout << ts.ms() << std::endl;
// // track delay due to zero crossing
// const float tsDelay = ts.ms() - maxTS.ms();
// std::cout << "step at " << ts.ms() << ". delay due to zero crossing: " << tsDelay << std::endl;
outSteps << maxTS.ms() << " " << tmp << "\n";
outSteps.flush();
}

44
sensors/imu/StepDetection4.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef STEPDETECTION4_H
#define STEPDETECTION4_H
@@ -24,7 +34,7 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/iir/BiQuad.h"
#include "../../math/dsp/iir/BiQuadStack.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/DelayBuffer.h"
@@ -43,7 +53,7 @@ class StepDetection4 {
static constexpr float stepRate_hz = 2.0;
static constexpr int sRate_hz = 100;
static constexpr int every_ms = 1000 / sRate_hz;
static constexpr float threshold = 1.0;
float threshold = 0.8;
float max = 0;
Timestamp maxTS;
@@ -52,7 +62,7 @@ private:
PoseDetection* pose;
FixedFrequencyInterpolator<Vector3> interpol;
IIR::BiQuad<float> biquad;
IIR::BiQuadStack<float> biquad;
DelayBuffer<float> delay;
@@ -75,16 +85,30 @@ private:
public:
/** ctor */
StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
StepDetection4(PoseDetection* pose, bool useBandPass) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
plot.getKey().setVisible(true);
lineRaw.setTitle("unrotated Z");
lineFiltered.setTitle("IIR filtered");
//biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
//biquad.preFill(gravity);
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
biquad.preFill(gravity);
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
plot.getKey().setVisible(true);
lineRaw.setTitle("unrotated Z");
lineFiltered.setTitle("IIR filtered");
gp << "set autoscale xfix\n";
plot.setTitle("Step Detection");
plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
@@ -123,7 +147,7 @@ public:
const float mag = data.z;
// apply filter
const float fMag = biquad.filter(mag);
const float fMag = biquad.filter(mag - gravity);
// history buffer
float fMagOld = delay.add(fMag);

50
sensors/imu/TurnDetection.h
View File

@@ -1,14 +1,24 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef TURNDETECTION_H
#define TURNDETECTION_H
#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 <eigen3/Eigen/Dense>
@@ -19,12 +29,13 @@
#include "../../Assertions.h"
#include "TurnProvider.h"
class TurnDetection {
class TurnDetection : public TurnProvider {
private:
PoseDetection* pose = nullptr;
PoseProvider* pose = nullptr;
//std::vector<GyroscopeData> gyroData;
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -32,6 +43,10 @@ private:
Timestamp lastGyroReading;
float curSigma = 0;
MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
#ifdef WITH_DEBUG_PLOT
TurnDetectionPlot plot;
#endif
@@ -39,7 +54,7 @@ private:
public:
/** ctor */
TurnDetection(PoseDetection* pose) : pose(pose) {
TurnDetection(PoseProvider* pose) : pose(pose) {
;
}
@@ -65,6 +80,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 +110,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 +135,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;

127
sensors/imu/TurnDetection2.h Normal file
View File

@@ -0,0 +1,127 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#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 <eigen3/Eigen/Dense>
#include <cmath>
#include <vector>
#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

55
sensors/imu/TurnDetectionPlot.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H
#define INDOOR_IMU_TURNDETECTIONPLOT_H
@@ -69,7 +79,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 +124,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

22
sensors/imu/TurnProvider.h Normal file
View File

@@ -0,0 +1,22 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef TURN_PROVIDER_H
#define TURN_PROVIDER_H
class TurnProvider {
public:
virtual float getSigma() const = 0;
};
#endif // TURN_PROVIDER_H

10
sensors/offline/FilePlayer.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef FILEPLAYER_H
#define FILEPLAYER_H

10
sensors/offline/FileReader.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef FILEREADER_H
#define FILEREADER_H

10
sensors/offline/FileWriter.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef FILEWRITER_H
#define FILEWRITER_H

10
sensors/offline/Listener.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef OFFLINE_LISTENER_H
#define OFFLINE_LISTENER_H

10
sensors/offline/OfflineAndroid.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef OFFLINEANDROID_H
#define OFFLINEANDROID_H

10
sensors/offline/Sensors.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef OFFLINE_SENSORS_H
#define OFFLINE_SENSORS_H

10
sensors/offline/Splitter.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef DATA_SPLITTER_H
#define DATA_SPLITTER_H

47
sensors/pressure/BarometerData.h
View File

@@ -3,28 +3,67 @@
#include <cmath>
#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<FPDefault>(
(T0/L) * (std::pow(static_cast<FPDouble>(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<FPDefault>(
hRef + (T0/L) * (std::pow(static_cast<FPDouble>(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<FPDefault>(
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<FPDefault>(
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) );}
};

10
sensors/radio/AccessPoint.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef ACCESSPOINT_H
#define ACCESSPOINT_H

10
sensors/radio/VAPGrouper.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef VAPGROUPER_H
#define VAPGROUPER_H

10
sensors/radio/WiFiGridEstimator.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIGRIDESTIMATOR_H
#define WIFIGRIDESTIMATOR_H

10
sensors/radio/WiFiGridNode.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIGRIDNODE_H
#define WIFIGRIDNODE_H

10
sensors/radio/WiFiMeasurement.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMEASUREMENT_H
#define WIFIMEASUREMENT_H

10
sensors/radio/WiFiMeasurements.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMEASUREMENTS_H
#define WIFIMEASUREMENTS_H

10
sensors/radio/WiFiProbability.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFPROBABILITY_H
#define WIFPROBABILITY_H

10
sensors/radio/WiFiProbabilityFree.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIPROBABILITYFREE_H
#define WIFIPROBABILITYFREE_H

10
sensors/radio/WiFiProbabilityGrid.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIPROBABILITYGRID_H
#define WIFIPROBABILITYGRID_H

10
sensors/radio/WiFiQualityAnalyzer.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIQUALITYANALYZER_H
#define WIFIQUALITYANALYZER_H

10
sensors/radio/model/LogDistanceModel.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef LOGDISTANCEMODEL_H
#define LOGDISTANCEMODEL_H

10
sensors/radio/model/WiFiModel.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODEL_H
#define WIFIMODEL_H

11
sensors/radio/model/WiFiModelFactory.h
View File

@@ -1,7 +1,16 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELFACTORY_H
#define WIFIMODELFACTORY_H
#include "WiFiModel.h"
#include "../../../floorplan/v2/Floorplan.h"

10
sensors/radio/model/WiFiModelFactoryImpl.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELFACTORYIMPL_H
#define WIFIMODELFACTORYIMPL_H

10
sensors/radio/model/WiFiModelLogDist.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELLOGDIST_H
#define WIFIMODELLOGDIST_H

10
sensors/radio/model/WiFiModelLogDistCeiling.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELLOGDISTCEILING_H
#define WIFIMODELLOGDISTCEILING_H

11
sensors/radio/model/WiFiModelPerBBox.h
View File

@@ -1,7 +1,16 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELPERBBOX_H
#define WIFIMODELPERBBOX_H
#include "../AccessPoint.h"
#include "../../../geo/Point3.h"
#include "../../../geo/BBoxes3.h"

10
sensors/radio/model/WiFiModelPerFloor.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELPERFLOOR_H
#define WIFIMODELPERFLOOR_H

10
sensors/radio/model/WiFiModels.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIMODELS_H
#define WIFIMODELS_H

10
sensors/radio/scan/WiFiChannels.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_WIFICHANNELS_H
#define INDOOR_WIFICHANNELS_H

10
sensors/radio/scan/WiFiRAW.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_WIFIRAW_H
#define INDOOR_WIFIRAW_H

10
sensors/radio/scan/WiFiScan.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef INDOOR_WIFI_SCAN_H
#define INDOOR_WIFI_SCAN_H

10
sensors/radio/setup/WiFiFingerprint.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIFINGERPRINT_H
#define WIFIFINGERPRINT_H

10
sensors/radio/setup/WiFiFingerprints.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIFINGERPRINTS_H
#define WIFIFINGERPRINTS_H

10
sensors/radio/setup/WiFiOptimizer.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIOPTIMIZER_H
#define WIFIOPTIMIZER_H

10
sensors/radio/setup/WiFiOptimizerLogDistCeiling.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFI_OPTIMIZER_LOG_DIST_CEILING_H
#define WIFI_OPTIMIZER_LOG_DIST_CEILING_H

10
sensors/radio/setup/WiFiOptimizerPerFloor.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIOPTIMIZERPERFLOOR_H
#define WIFIOPTIMIZERPERFLOOR_H

10
sensors/radio/setup/WiFiOptimizerStructs.h
View File

@@ -1,3 +1,13 @@
/*
* © Copyright 2014 – Urheberrechtshinweis
* Alle Rechte vorbehalten / All Rights Reserved
*
* Programmcode ist urheberrechtlich geschuetzt.
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
* Keine Verwendung ohne explizite Genehmigung.
* (vgl. § 106 ff UrhG / § 97 UrhG)
*/
#ifndef WIFIOPTIMIZERSTRUCTS_H
#define WIFIOPTIMIZERSTRUCTS_H