diff --git a/CMakeLists.txt b/CMakeLists.txt index d6073f4..7c213a4 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,8 @@ ADD_DEFINITIONS( -DWITH_TESTS -DWITH_ASSERTIONS - -DWITH_DEBUG_LOG + #-DWITH_DEBUG_LOG + -DWITH_DEBUG_PLOT ) diff --git a/Settings.h b/Settings.h index 90f53aa..de88d42 100644 --- a/Settings.h +++ b/Settings.h @@ -48,15 +48,15 @@ namespace Settings { constexpr float sigma = 8.0; /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ constexpr float TXP = -40; - constexpr float EXP = 2.3; + constexpr float EXP = 2.2; constexpr float WAF = -8.0; // how to perform VAP grouping. see // - calibration in Controller.cpp // - eval in Filter.h // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! - const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, 1); - const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, 1); + const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); } namespace BeaconModel { @@ -83,7 +83,7 @@ namespace Settings { const std::vector path3 = {10, 8, 7, 4, 11, 12, 13, 14, 6}; const std::vector path4 = {0, 1, 2, 3, 4, 5, 6, 71, 7, 8, 9, 10}; const std::vector path5 = {40, 41, 85, 117, 31, 32, 33, 34, 35, 36, 37, 12, 3, 4, 5, 6, 71}; - const std::vector path6 = {28, 27, 26, 25, 24, 23, 22, 21, 20, 1, 2, 3, 4, 5, 6, 5}; + const std::vector path6 = {28, 27, 26, 25, 24, 23, 22, 21, 20, 1, 2, 3, 4, 6, 5}; } diff --git a/filter/Logic.h b/filter/Logic.h index 4b93fd2..4c14cec 100644 --- a/filter/Logic.h +++ b/filter/Logic.h @@ -24,6 +24,8 @@ #include #include +#include + #include #include #include @@ -171,7 +173,7 @@ struct PFTrans : public K::ParticleFilterTransition { PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) { walker.addModule(&modHead); - //walker.addModule(&modHeadMises); + //walker.addModule(&modHeadMises); // fürn arsch und geht net //walker.addModule(&modSpread); // might help in some situations! keep in mind! //walker.addModule(&modActivity); //walker.addModule(&modHeadUgly); @@ -334,6 +336,110 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition{ +public: + + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ + BFTrans() + { + //nothin + } + + 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 = this->ts - ts; + this->ts = ts; + } + } + + + /** + * smoothing transition starting at T with t, t-1,...0 + * @param particles_qt q_t (Forward Filter) p2 + * @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1 + */ + std::vector> transition(std::vector>const& particles_qt, + std::vector>const& particles_qt1, + const MyControl* control) override { + + + + // Forward Transition von q_t nach q_t+1* with tracking of particle using an id = p(q_t+1* | q_t) + //TODO: darf ich das einfach? einfach eine neue Dichte Sampeln? Brauch ich da nicht eine "Erlaubnis" (Two-Filter Smoother kann das) + // law of total probabality auch einfach über ziehen ?? + + // KDE auf q_t+1 Samples = p(q_t+1 | o_1:T) + + // Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T) + + // Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation + + + + // calculate alpha(m,n) = p(q_t+1(m) | q_t(n)) + // this means, predict all possible states q_t+1 with all passible states q_t + // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and + // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1)) + std::vector> predictionProbabilities; + + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(7); + #pragma omp parallel for shared(predictionProbabilities) + for (int i = 0; i < particles_old.size(); ++i) { + std::vector innerVector; + auto p1 = &particles_old[i]; + + for(int j = 0; j < particles_new.size(); ++j){ + + auto p2 = &particles_new[j]; + + const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0; + + //TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental + + const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m); + + // TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement? + double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD()); + double diffDeg = Angle::radToDeg(diffRad); + double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad); + double measurementDeg = Angle::radToDeg(measurementRad); + const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg); + + // does the angle between two particles positions is similiar to the measurement + //double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y); + + //check how near we are to the measurement + double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0; + const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ); + + //combine the probabilities + double prob = distProb;// * floorProb * headingProb; + innerVector.push_back(prob); + + } + #pragma omp critical + predictionProbabilities.push_back(innerVector); + } + + return predictionProbabilities; + + } +}; + +struct BFTransKDESlow : public K::BackwardFilterTransition{ + + public: /** @@ -473,20 +579,20 @@ struct PFEval : public K::ParticleFilterEvaluation { return Distribution::Normal::getProbability(static_cast(hPa), 0.10, static_cast(observation.relativePressure)); } - double getStairProb(const K::Particle& p, const ActivityButterPressure::Activity act) { + double getStairProb(const K::Particle& p, const Activity act) { - const float kappa = 0.75; + const float kappa = 0.65; const MyNode& gn = grid.getNodeFor(p.state.position); switch (act) { - case ActivityButterPressure::Activity::STAY: + case Activity::WALKING: if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} {return 1-kappa;} - case ActivityButterPressure::Activity::UP: - case ActivityButterPressure::Activity::DOWN: + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} {return 1-kappa;} @@ -532,7 +638,7 @@ struct PFEval : public K::ParticleFilterEvaluation { //p.weight = std::pow(p.weight, 0.5); } - const double prob = pWifi * pBaroPressure * pStairProb; + const double prob = pWifi;// * pStairProb; p.weight = prob; diff --git a/filter/Structs.h b/filter/Structs.h index 831406a..1f3f1b5 100644 --- a/filter/Structs.h +++ b/filter/Structs.h @@ -12,6 +12,8 @@ #include #include + +#include #include #include #include @@ -63,12 +65,14 @@ struct MyControl { /** turn angle (in radians) since the last transition */ float turnSinceLastTransition_rad = 0; + float sumTurn_rad = 0; /** number of steps since the last transition */ int numStepsSinceLastTransition = 0; /** current motion delta angle*/ float motionDeltaAngle_rad = 0; + float sumMotion_rad = 0; /** reset the control-data after each transition */ void resetAfterTransition() { @@ -91,7 +95,7 @@ struct MyObs { WiFiMeasurements wifi; /** detected activity */ - ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY; + Activity activity = Activity::STANDING; /** beacon measurements */ BeaconMeasurements beacons; diff --git a/main.cpp b/main.cpp index 80263c4..c5b0f61 100755 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,8 @@ //toni const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/"; //const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/"; -const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/"; +//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/"; +const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/"; const std::string errorDir = dataDir + "results/"; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -68,6 +69,20 @@ struct Data { mapDir + "grid_Stock_1-3_03.dat" }; + DataSetup MotionAxisTest = { + + mapDir + "SHL40_noElevator.xml", + + { + dataDir + "Path0_0.csv" + }, + + mapDir + "wifi_fp_all.dat", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "grid_SHL40_noElevator.dat" + }; + } data; Floorplan::IndoorMap* MyState::map; @@ -129,7 +144,7 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: plot.addStairs(map); plot.gp << "set autoscale xy\n"; //plot.addGrid(grid); - + plot.splot.getView().setEnabled(false); // init ctrl and observation MyControl ctrl; @@ -141,8 +156,8 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: std::vector tsHistory; //filter init - K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); - //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f))); + //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); + K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f))); pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); //pf.setTransition(std::unique_ptr(new PFTransKLDSampling(grid, &ctrl))); @@ -159,7 +174,7 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingKLD())); } - pf.setNEffThreshold(0.85); + pf.setNEffThreshold(0.95); //estimation pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); @@ -192,9 +207,11 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: Timestamp lastTimestamp = Timestamp::fromMS(0); StepDetection sd; - TurnDetection td; + PoseDetection pd; + TurnDetection td(&pd); MotionDetection md; ActivityButterPressure act; + //ActivityDetector act; RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); @@ -234,7 +251,7 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: ++ctrl.numStepsSinceLastTransition; } const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(ts, _acc.data); + pd.addAccelerometer(ts, _acc.data); } else if (e.type == Offline::Sensor::GYRO) { const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; @@ -248,7 +265,8 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: obs.sigmaPressure = relBaro.getSigma(); //activity recognition - obs.activity = act.add(ts, fr.getBarometer()[e.idx].data); + act.add(ts, fr.getBarometer()[e.idx].data); + obs.activity = act.get(); //activity for transition } else if (e.type == Offline::Sensor::LIN_ACC) { @@ -322,7 +340,16 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: if(Settings::useKLB){ plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n"; } - plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n"; + plot.gp << "set label 1002 at screen 0.95, 0.98 'act:" << static_cast(obs.activity) << "'\n"; + + //draw gyro angle and motion angle + //turn angle plot + static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad; + plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); + + //motion angle plot + static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad; + plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); /** Draw everything */ plot.show(); @@ -387,6 +414,7 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: plotkld.add(&lines); gp.draw(plotkld); gp.flush(); + plot.splot.getView().setEnabled(false); } std::cout << "finished" << std::endl; @@ -406,7 +434,13 @@ int main(int argc, char** argv) { for(int i = 0; i < 10; ++i){ - tmp = run(data.FloorOneToThree, 2, "Wifi-Dongle-Test", Settings::Path_DongleTest::path6); +// tmp = run(data.FloorOneToThree, 0, "Wifi-Dongle-Test", Settings::Path_DongleTest::path4); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); + + tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1); statsMedian.add(tmp.getMedian()); statsAVG.add(tmp.getAvg()); statsSTD.add(tmp.getStdDev());