started changes for implementing fast smoothing

different new evaluations
small changes in transition

CMakeLists.txt

@@ -63,7 +63,8 @@ ADD_DEFINITIONS(
 
 	-DWITH_TESTS
 	-DWITH_ASSERTIONS
-	-DWITH_DEBUG_LOG
+        #-DWITH_DEBUG_LOG
+        -DWITH_DEBUG_PLOT
 
 
 )

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_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, 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<int> path3 = {10, 8, 7, 4, 11, 12, 13, 14, 6};
         const std::vector<int> path4 = {0, 1, 2, 3, 4, 5, 6, 71, 7, 8, 9, 10};
         const std::vector<int> path5 = {40, 41, 85, 117, 31, 32, 33, 34, 35, 36, 37, 12, 3, 4, 5, 6, 71};
-        const std::vector<int> path6 = {28, 27, 26, 25, 24, 23, 22, 21, 20, 1, 2, 3, 4, 5, 6, 5};
+        const std::vector<int> path6 = {28, 27, 26, 25, 24, 23, 22, 21, 20, 1, 2, 3, 4, 6, 5};
 
     }
 

filter/Logic.h

@@ -24,6 +24,8 @@
 #include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
 #include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
 
+#include <Indoor/sensors/activity/ActivityDetector.h>
+
 #include <KLib/math/filter/particles/ParticleFilterMixing.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -171,7 +173,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
     PFTrans(Grid<MyNode>& 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<MyState, MyContro
 struct BFTrans : public K::BackwardFilterTransition<MyState>{
 
 
+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<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_qt,
+                                                std::vector<K::Particle<MyState>>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<std::vector<double>> 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<double> 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<MyState>{
+
+
 public:
 
     /**
@@ -473,20 +579,20 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
         return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
     }
 
-    double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
+    double getStairProb(const K::Particle<MyState>& 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 Activity::WALKING_UP:
-        case ActivityButterPressure::Activity::DOWN:
+        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<MyState, MyObs> {
                 //p.weight = std::pow(p.weight, 0.5);
             }
 
-            const double prob = pWifi * pBaroPressure * pStairProb;
+            const double prob = pWifi;// * pStairProb;
 
             p.weight = prob;
 

filter/Structs.h

@@ -12,6 +12,8 @@
 #include <Indoor/grid/factory/v2/GridNodeImportance.h>
 #include <Indoor/math/distribution/KernelDensity.h>
 
+
+#include <Indoor/sensors/activity/Activity.h>
 #include <Indoor/grid/walk/v2/GridWalker.h>
 #include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
 #include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
@@ -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;

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<float> 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<float> run(DataSetup setup, int numFile, std::string folder, std::
     std::vector<int64_t> tsHistory;
 
     //filter init
-    K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
+    //K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
-    //K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
+    K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
 
     pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
     //pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
@@ -159,7 +174,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
         //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingKLD<MyState>>(new K::ParticleFilterResamplingKLD<MyState>()));
     }
 
-    pf.setNEffThreshold(0.85);
+    pf.setNEffThreshold(0.95);
 
     //estimation
     pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -192,9 +207,11 @@ K::Statistics<float> 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<float> run(DataSetup setup, int numFile, std::string folder, std::
                 ++ctrl.numStepsSinceLastTransition;
             }
             const Offline::TS<AccelerometerData>& _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<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
@@ -248,7 +265,8 @@ K::Statistics<float> 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<float> 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<int>(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<float> 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());