151 lines
4.3 KiB
C++
151 lines
4.3 KiB
C++
/*
|
||
* © 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 "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"
|
||
|
||
|
||
/**
|
||
* custom compass implementation, similar to turn-detection
|
||
*/
|
||
class CompassDetection {
|
||
|
||
private:
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
CompassDetectionPlot plot;
|
||
#endif
|
||
|
||
// struct {
|
||
// Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||
// bool isKnown = false;
|
||
// Timestamp lastEstimation;
|
||
// } orientation;
|
||
|
||
MovingAverageTS<MagnetometerData> avgIn;
|
||
|
||
//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
|
||
MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
|
||
|
||
|
||
|
||
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(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) {
|
||
|
||
++numMagReadings;
|
||
|
||
// get the current magnetometer-reading as vector
|
||
//Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z;
|
||
const Vector3 vec(mag.x, mag.y, mag.z);
|
||
|
||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||
//Eigen::Vector3f _curMag = pose->getMatrix() * vec;
|
||
const Vector3 _curMag = pose->getMatrix() * vec;
|
||
|
||
//avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2)));
|
||
avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z));
|
||
const MagnetometerData curMag = avgIn.get();
|
||
//const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z);
|
||
|
||
// calculate standard-deviation
|
||
//stdDev.add(ts, curMag);
|
||
//const float dev = std::max(stdDev.get().x, stdDev.get().y);
|
||
|
||
|
||
// 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 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 declination = 0; // GERMANY!
|
||
float curHeading = Angle::makeSafe_2PI(- tmp + declination);
|
||
|
||
float resHeading;
|
||
stable = true;
|
||
|
||
// calculate standard-deviation within a certain timeframe
|
||
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 {
|
||
resHeading = curHeading;
|
||
stable = true;
|
||
// }
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
plot.add(ts, curHeading, stable, mag, curMag);
|
||
#endif
|
||
|
||
// done
|
||
return resHeading;
|
||
|
||
}
|
||
|
||
|
||
};
|
||
|
||
#endif // INDOOR_IMU_COMPASSDETECTION_H
|