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

fixed some issues

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

40
sensors/imu/TurnDetection.h
View File

@@ -4,11 +4,11 @@
#include "GyroscopeData.h"
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingStdDevTS.h"
#include "../../math/Matrix3.h"
#include "../../geo/Point3.h"
#include "PoseDetection.h"
#include "PoseProvider.h"
//#include <eigen3/Eigen/Dense>
@@ -19,12 +19,13 @@
#include "../../Assertions.h"
#include "TurnProvider.h"
class TurnDetection {
class TurnDetection : public TurnProvider {
private:
PoseDetection* pose = nullptr;
PoseProvider* pose = nullptr;
//std::vector<GyroscopeData> gyroData;
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -32,6 +33,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 +44,7 @@ private:
public:
/** ctor */
TurnDetection(PoseDetection* pose) : pose(pose) {
TurnDetection(PoseProvider* pose) : pose(pose) {
;
}
@@ -65,6 +70,11 @@ public:
// } driftEst;
/** get the current uncertainty estimation */
float getSigma() const override {
return curSigma;
}
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
// ignore the first reading completely, just remember its timestamp
@@ -90,17 +100,20 @@ public:
const Vector3 curGyro = pose->getMatrix() * vec;
//driftEst.removeDrift(ts, curGyro);
// area
//const Eigen::Vector3f area =
const Vector3 area =
// Trapezoid rule (should be more accurate but does not always help?!)
//(prevGyro * curDiff.sec()) + // squared region
//((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality)
// Trapezoid rule (should be more accurate but does not always help?!)
//(prevGyro * curDiff.sec()) + // squared region
//((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality)
// just the rectangular region
(prevGyro * curDiff.sec()); // BEST?!
// average (is the same as above)
//((curGyro+prevGyro)/2 * curDiff.sec());
// just the rectangular region
(prevGyro * curDiff.sec()); // BEST?!
//}
@@ -112,9 +125,14 @@ public:
const float delta = area.z;
#ifdef WITH_DEBUG_PLOT
plot.add(ts, delta, gyro, curGyro);
plot.addRelative(ts, delta, gyro, curGyro);
#endif
//stdDevForSigma.add(ts, prevGyro.z); // ignore delta T. directly us radians-per-sec as sigma
//curSigma = stdDevForSigma.get();
const float radPerSec = 1.0f / 180.0f * M_PI;
curSigma = radPerSec + std::abs(prevGyro.z * 0.05); // constant of 1deg/sec + 5% of current turn rate
// done
return delta;