3f8d21e146
1) when barometer produces false measurements, we needed some kind of upper boundary 2) coding error static.. while initializing this object over multiple test iterations isnt the best idea
126 lines
4.5 KiB
C++
126 lines
4.5 KiB
C++
#ifndef WALKMODULEHEADINGVONMISES_H
|
|
#define WALKMODULEHEADINGVONMISES_H
|
|
|
|
#include "WalkModule.h"
|
|
#include "WalkStateHeading.h"
|
|
|
|
#include "../../../../geo/Heading.h"
|
|
#include "../../../../math/Distributions.h"
|
|
|
|
|
|
/** keep the state's heading */
|
|
template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingVonMises : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
/** van-Mises distribution */
|
|
Distribution::VonMises<float> dist;
|
|
|
|
/** random noise */
|
|
Distribution::Normal<float> distNoise;
|
|
|
|
const Control* ctrl;
|
|
|
|
public:
|
|
|
|
/** ctor 3.0 should be OK! */
|
|
WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) :
|
|
dist(Distribution::VonMises<float>(0.0f, 2.0f)),
|
|
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
|
|
ctrl(ctrl) {
|
|
|
|
// ensure the template WalkState inherits from 'WalkStateHeading'!
|
|
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
|
|
|
|
}
|
|
|
|
|
|
virtual void updateBefore(WalkState& state, const Node& startNode) override {
|
|
|
|
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
|
|
// reset this one after every transition!
|
|
Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
|
|
|
|
// sensor noise
|
|
float var = distNoise.draw();
|
|
|
|
// stair? -> increase variance
|
|
if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
|
|
|
|
// adjust the state's heading using the control-data
|
|
//TODO: if motionaxis detects landscaping, do not do this!
|
|
state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
|
|
|
|
//set kappa of mises
|
|
float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
|
|
dist.setKappa(kappa);
|
|
|
|
}
|
|
|
|
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
|
(void) state;
|
|
(void) startNode;
|
|
(void) endNode;
|
|
}
|
|
|
|
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
|
|
|
(void) state;
|
|
|
|
// ignore for stairs?
|
|
//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
|
|
|
|
// for elevator edges [same (x,y) but different z] do not adjust anything
|
|
if (nextNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
|
|
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
|
|
//if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
|
|
|
|
// get the heading denoted by the way from curNode to nextNode
|
|
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
|
|
|
|
// get the heading requested by the state
|
|
const Heading stateHead = state.heading.direction;
|
|
|
|
// get the error (signed difference) between both
|
|
//const float angularDiff = stateHead.getSignedDiff(head);
|
|
const float angularDiff = Heading::getSignedDiff(head, stateHead);
|
|
|
|
// adjust the error.
|
|
// note: the error may get > +/- 2PI but this is not an issue!
|
|
// when the error is added to the current heading within getProbability(),
|
|
// it is ensured their sum is within [0:2pi]
|
|
state.heading.error += angularDiff;
|
|
|
|
}
|
|
|
|
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) startNode;
|
|
|
|
// ignore for stairs?
|
|
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
|
|
|
|
// for elevator edges [same (x,y) but different z] just return 1
|
|
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
|
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
|
//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
|
|
|
|
// get the heading between curNode and potentialNode
|
|
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
|
|
|
|
// compare the heading against the state's heading - the last error
|
|
const Heading stateHead = state.heading.direction + state.heading.error;
|
|
|
|
// get the difference
|
|
const float angularDiff = head.getDiffHalfRAD(stateHead);
|
|
|
|
// determine probability
|
|
return dist.getProbability(angularDiff);
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEHEADING_H
|