b9b9d8f9ac
filter is updated every step and every 1000 ms an evaluation is calculated also added sink or swim method from frank, works great
93 lines
2.6 KiB
C++
93 lines
2.6 KiB
C++
#ifndef NAVCONTROLLERMESH_H
|
|
#define NAVCONTROLLERMESH_H
|
|
|
|
#include "../sensors/AccelerometerSensor.h"
|
|
#include "../sensors/GyroscopeSensor.h"
|
|
#include "../sensors/BarometerSensor.h"
|
|
#include "../sensors/WiFiSensor.h"
|
|
#include "../sensors/SensorFactory.h"
|
|
#include "../sensors/StepSensor.h"
|
|
#include "../sensors/TurnSensor.h"
|
|
#include "../sensors/ActivitySensor.h"
|
|
|
|
#include <Indoor/navMesh/NavMeshLocation.h>
|
|
#include <Indoor/navMesh/NavMesh.h>
|
|
|
|
#include <Indoor/Assertions.h>
|
|
#include <thread>
|
|
|
|
//#include "State.h"
|
|
#include "FilterMesh.h"
|
|
|
|
#include "../Controller.h"
|
|
#include "../NavController.h"
|
|
|
|
namespace MeshBased {
|
|
|
|
class NavControllerMesh : public NavController {
|
|
|
|
private:
|
|
|
|
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
|
|
WiFiModel* wifiModel;
|
|
|
|
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
|
|
|
|
MyObservation curObs;
|
|
MyControl curCtrl;
|
|
|
|
public:
|
|
|
|
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
|
|
|
|
|
|
void start() override;
|
|
|
|
void stop() override;
|
|
|
|
|
|
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
|
|
|
|
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
|
|
|
|
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
|
|
|
|
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
|
|
|
|
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
|
|
|
|
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;
|
|
|
|
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
|
|
|
|
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
|
|
|
|
|
|
private:
|
|
|
|
/** called when any sensor has received new data */
|
|
void gotSensorData(const Timestamp ts);
|
|
|
|
void debugActivity(const ActivityData& activity);
|
|
|
|
/** particle-filter update loop */
|
|
void filterUpdateLoop();
|
|
|
|
/** check whether its time for a filter update, and if so, execute the update and return true */
|
|
bool filterUpdateIfNeeded();
|
|
|
|
/** perform a filter-update (called from a background-loop) */
|
|
void filterUpdate();
|
|
|
|
/** perform a filter-update only with estimation (called from a background-loop) */
|
|
void filterUpdateEstimationOnly();
|
|
|
|
/** UI update loop */
|
|
void updateMapViewLoop();
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // NAVCONTROLLERMESH_H
|