164 lines
4.7 KiB
C++
164 lines
4.7 KiB
C++
#ifndef WALKMODULERELATIVEPRESSURE_H
|
|
#define WALKMODULERELATIVEPRESSURE_H
|
|
|
|
|
|
#include "WalkModule.h"
|
|
#include "WalkStateHeading.h"
|
|
|
|
#include "../../../../geo/Heading.h"
|
|
#include "../../../../math/distribution/Normal.h"
|
|
#include "../../../../Assertions.h"
|
|
|
|
|
|
/**
|
|
* a walk-state the contains the pressure relative to time t0
|
|
*/
|
|
struct WalkStateRelativePressure {
|
|
|
|
/** the pressure level [relative to time t0] one SHOULD messure at this state */
|
|
float pressureRelToT0;
|
|
|
|
int dirLock = 0;
|
|
|
|
/** ctor */
|
|
WalkStateRelativePressure(const float pressureRelToT0) : pressureRelToT0(pressureRelToT0) {;}
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* uses the pressure relative to t=0 within the control-data
|
|
* to estimate the likelyhood for any z-changes during the transition
|
|
*/
|
|
template <typename Node, typename WalkState, typename Control> class WalkModuleRelativePressureControl : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
Control* ctrl;
|
|
|
|
/** pressure-change (hPa) per meter */
|
|
const float hPaPerMeter = 0.126f; // given an average hPa of 938
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WalkModuleRelativePressureControl(Control* ctrl, const float hPaPerMeter) : ctrl(ctrl), hPaPerMeter(hPaPerMeter) {
|
|
;
|
|
}
|
|
|
|
virtual void updateBefore(WalkState& state, const Node& startNode) override {
|
|
(void) state;
|
|
(void) startNode;
|
|
}
|
|
|
|
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
|
|
|
// // e.g. walking down from the 3rd to the second floor
|
|
// // startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
|
|
// // deltaPressure is POSITIVE (pressure increases) as we walk downstairs
|
|
// const int deltaZ_cm = startNode.z_cm - endNode.z_cm;
|
|
// const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
|
|
|
|
// // update the states pressure
|
|
// state.pressureRelToT0 += deltaPressure;
|
|
|
|
// // sanity checks
|
|
// Assert::isNotNaN(deltaPressure, "detected NaN!");
|
|
// Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
|
|
|
|
// static int xx = 0;
|
|
// if (++xx % 1024 == 0) {
|
|
// ++xx;
|
|
// }
|
|
|
|
}
|
|
|
|
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
|
|
|
// e.g. walking down from the 3rd to the second floor
|
|
// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
|
|
// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
|
|
const int deltaZ_cm = curNode.z_cm - nextNode.z_cm;
|
|
const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
|
|
const float expectedPressure = state.pressureRelToT0 + deltaPressure;
|
|
|
|
// update the states pressure
|
|
state.pressureRelToT0 = expectedPressure;
|
|
|
|
// sanity checks
|
|
Assert::isNotNaN(deltaPressure, "detected NaN!");
|
|
Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
|
|
|
|
if(std::abs(state.dirLock) > 0) {
|
|
if (state.dirLock > 0) {--state.dirLock;}
|
|
if (state.dirLock < 0) {++state.dirLock;}
|
|
} else {
|
|
if (deltaZ_cm > 0) {state.dirLock = +25;}
|
|
if (deltaZ_cm < 0) {state.dirLock = -25;}
|
|
}
|
|
|
|
}
|
|
|
|
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) startNode;
|
|
|
|
// get the values from control-data
|
|
const float curRelPressure = ctrl->barometer.hPaRelativeToT0;
|
|
const float pressureSigma = ctrl->barometer.estimatedSigma * 1.50f;
|
|
|
|
// not yet available/calibrated/possible -> skip evaluation!
|
|
if (pressureSigma == 0) {return 1;}
|
|
|
|
// e.g. walking down from the 3rd to the second floor
|
|
// startZ = 10 meter, endZ = 7 meter -> deltaZ = +3 meter;
|
|
// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
|
|
const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
|
|
const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
|
|
const float expectedPressure = state.pressureRelToT0 + deltaPressure;
|
|
|
|
const float oldErr = std::abs(state.pressureRelToT0 - curRelPressure);
|
|
const float newErr = std::abs(expectedPressure - curRelPressure);
|
|
|
|
if (std::abs(state.dirLock) > 0) {
|
|
if (state.dirLock > 0) {
|
|
if (deltaZ_cm < 0) {return 0;}
|
|
if (deltaZ_cm == 0) {return 0.1;}
|
|
return 0.9;
|
|
} else {
|
|
if (deltaZ_cm > 0) {return 0;}
|
|
if (deltaZ_cm == 0) {return 0.1;}
|
|
return 0.9;
|
|
}
|
|
}
|
|
|
|
|
|
if (oldErr > pressureSigma) {
|
|
if (deltaZ_cm == 0) {return 0.001;}
|
|
}
|
|
|
|
if (newErr > oldErr) {return 0.002;}
|
|
if (newErr == oldErr) {return 0.005;}
|
|
if (newErr < oldErr) {return 0.99;}
|
|
throw 1;
|
|
|
|
|
|
|
|
// // compare control-data with potential transition
|
|
// double prob = Distribution::Normal<double>::getProbability(curRelPressure, pressureSigma, expectedPressure);
|
|
//// prob = std::pow(prob, 20);
|
|
|
|
// // sanity checks
|
|
// Assert::isNotNaN(prob, "detected NaN!");
|
|
// Assert::isNotNaN(deltaPressure, "detected NaN!");
|
|
// Assert::isNotNaN(expectedPressure, "detected NaN!");
|
|
|
|
// return prob;
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULERELATIVEPRESSURE_H
|