84 lines
2.1 KiB
C++
Executable File
84 lines
2.1 KiB
C++
Executable File
#ifndef MYTRANSITIONSIMPLE_H
|
|
#define MYTRANSITIONSIMPLE_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
|
#include <KLib/math/distribution/Normal.h>
|
|
#include "MyState.h"
|
|
#include "MyControl.h"
|
|
|
|
class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
private:
|
|
|
|
/** a simple normal distribution */
|
|
K::NormalDistribution distX;
|
|
K::NormalDistribution distY;
|
|
K::NormalDistribution distZ;
|
|
K::NormalDistribution distBaro;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
|
|
distX.setSeed(1234);
|
|
distY.setSeed(1235);
|
|
distZ.setSeed(1236);
|
|
distBaro.setSeed(5678);
|
|
}
|
|
|
|
public:
|
|
|
|
uint64_t ts = 0;
|
|
uint64_t deltaMS = 0;
|
|
|
|
/** set the current time in millisconds */
|
|
void setCurrentTime(const uint64_t ts) {
|
|
if (this->ts == 0) {
|
|
this->ts = ts;
|
|
deltaMS = 0;
|
|
} else {
|
|
deltaMS = ts - this->ts;
|
|
this->ts = ts;
|
|
}
|
|
}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
p.state.heading_old = p.state.heading;
|
|
p.state.x_cm_old = p.state.x_cm;
|
|
p.state.y_cm_old = p.state.y_cm;
|
|
p.state.z_nr_old = p.state.z_nr;
|
|
|
|
p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
|
|
p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
|
|
p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
|
|
p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old, p.state.x_cm, p.state.y_cm);
|
|
|
|
// if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
|
|
// if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
|
|
// if (p.state.x_cm < 0) {p.state.x_cm = 0;}
|
|
// if (p.state.y_cm < 0) {p.state.y_cm = 0;}
|
|
|
|
|
|
|
|
//update barometer
|
|
p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
|
|
|
|
// update walked distance (2D)
|
|
const double dx = p.state.x_cm_old - p.state.x_cm;
|
|
const double dy = p.state.y_cm_old - p.state.y_cm;
|
|
p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
#endif // MYTRANSITIONSIMPLE_H
|