Merge branch 'master' of https://git.frank-ebner.de/FHWS/FtmPrologic

code/CMakeLists.txt

@@ -22,6 +22,8 @@ INCLUDE_DIRECTORIES(
     ../../
     ../../../
     ../../../../
+	
+	../../eigen3
 )
 
 
@@ -49,6 +51,7 @@ FILE(GLOB SOURCES
     mainProb.cpp
     Eval.cpp
     FtmKalman.cpp
+	trilateration.cpp	
 )
 
 

code/Plotty.h

@@ -492,7 +492,7 @@ public:
         double max = -999;
         for (const T& p : particles) {
             const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
-            const double prob = std::pow(p.weight, 0.25);
+            const double prob = p.weight; // std::pow(p.weight, 0.25);
             this->particles.add(p3, prob);
             if (prob > max) {max = prob;}
             if (prob < min) {min = prob;}

code/Settings.h

@@ -12,6 +12,9 @@ namespace Settings {
     const std::string plotDataDir = "../plots/data/";
     const std::string outputDir = "../output/";
     
+    const bool PlotCircles = false;
+    const bool PlotToPng = false;
+
     const bool UseKalman = false;
     const bool UseRSSI = false;
 
@@ -412,14 +415,14 @@ namespace Settings {
                 dataDir + "Pixel2/path21/17527721931557.csv",
                 dataDir + "Pixel2/path21/18001426901565.csv",
                 dataDir + "Pixel2/path21/18177010770775.csv",
-
+                
-                dataDir + "Pixel3/path21/15786943753685.csv",
+                //  dataDir + "Pixel3/path21/15786943753685.csv", // bug
                 dataDir + "Pixel3/path21/16075928276171.csv",
                 dataDir + "Pixel3/path21/16268193326887.csv",
-                dataDir + "Pixel3/path21/16424251083631.csv",
+                //  dataDir + "Pixel3/path21/16424251083631.csv", // bug
             },
             {
-                // NUC, ID Pos X   Y    Z  offset loss   kalman stddev
+                // NUC, ID Pos X   Y    Z  offset      loss   kalman stddev
                 { NUC1, {1, { 67  , 33  , 0.8}, -2.00, 3.000, 3.0f} }, // NUC 1  
                 { NUC2, {2, { 27  , 49.6, 0.8}, -2.00, 3.000, 3.0f} }, // NUC 2  
                 { NUC3, {3, { 15  , 32.4, 0.8}, -2.00, 3.000, 3.0f} }, // NUC 3  
@@ -466,6 +469,6 @@ namespace Settings {
         };
     } data;
 
-    static DataSetup CurrentPath = data.Path22;
+    static DataSetup CurrentPath = data.Path21;
 }
 

code/filter.h

@@ -183,7 +183,8 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl
         Distribution::Uniform<float> dHeading;
 
         MyPFTransRandom()
-            : dStepSize(0.7f, 0.5f), dHeading(0, 2*M_PI)
+            //: dStepSize(2.5f, 0.7f), dHeading(0, 2*M_PI)
+            : dStepSize(2.0f, 0.5f), dHeading(0, 2 * M_PI)
         {}
 
         void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {

code/main.cpp

@@ -399,7 +399,6 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
         {
             // Do update step
             Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
-            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
 
             // TODO
             //gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
@@ -504,44 +503,52 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
             estPos = est.pos.pos;
             ctrl.lastEstimate = estPos;
 
-            plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
-            plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
-
             // Error
-            distErrorFtm = gtPos.getDistance(estPos.xy());
+            if (Settings::UseRSSI)
-            errorStats.ftm.add(distErrorFtm);
-
-            // draw wifi ranges
-            plot.clearDistanceCircles();
-
-            for (size_t i = 0; i < obs.ftm.size(); i++)
             {
-                WiFiMeasurement wifi2 = obs.ftm[i];
+                distErrorRssi = gtPos.getDistance(estPos.xy());
-
+                errorStats.rssi.add(distErrorRssi);
-                Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position;
+            }
-
+            else
-                K::GnuplotColor color;
+            {
-                switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
+                distErrorFtm = gtPos.getDistance(estPos.xy());
-                {
+                errorStats.ftm.add(distErrorFtm);
-                case 1:  color = K::GnuplotColor::fromRGB(0, 255, 0); break;
-                case 2:  color = K::GnuplotColor::fromRGB(0, 0, 255); break;
-                case 3:  color = K::GnuplotColor::fromRGB(255, 255, 0); break;
-                case 6:  color = K::GnuplotColor::fromRGB(0, 255, 255); break;
-                default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
-                }
-
-                float plotDist = wifi2.getFtmDist();
-                if (Settings::UseRSSI)
-                {
-                    float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss;
-                    float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, wifi2.getRSSI());
-                    plotDist = rssiDist;
-                }
-                
-                plot.addDistanceCircle(apPos.xy(), plotDist, color);
             }
 
+            // draw wifi ranges
+            if (Settings::PlotCircles)
+            {
+                plot.clearDistanceCircles();
+
+                for (size_t i = 0; i < obs.ftm.size(); i++)
+                {
+                    WiFiMeasurement wifi2 = obs.ftm[i];
+
+                    Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position;
+
+                    K::GnuplotColor color;
+                    switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
+                    {
+                    case 1:  color = K::GnuplotColor::fromRGB(0, 255, 0); break;
+                    case 2:  color = K::GnuplotColor::fromRGB(0, 0, 255); break;
+                    case 3:  color = K::GnuplotColor::fromRGB(255, 255, 0); break;
+                    case 6:  color = K::GnuplotColor::fromRGB(0, 255, 255); break;
+                    default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
+                    }
+
+                    float plotDist = wifi2.getFtmDist() + Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ftm_offset;
+                    if (Settings::UseRSSI)
+                    {
+                        float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss;
+                        float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, wifi2.getRSSI());
+                        plotDist = rssiDist;
+                    }
+
+                    plot.addDistanceCircle(apPos.xy(), plotDist, color);
+                }
+            }
             
+
             obs.wifi.clear();
             obs.ftm.clear();
 
@@ -562,7 +569,25 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
             //distsPlot.add("ftmDists", ftmDistances);
             //distsPlot.frame();
 
+            // Png Output
+            if (Settings::PlotToPng)
+            {
+                plot.gp.setTerminal("png", K::GnuplotSize(1280, 720));
+                auto pngPath = outputDir / "png" / "frame.png";
+
+                // clear folder
+                //std::filesystem::remove_all(pngPath);
+                forceDirectories(pngPath.parent_path());
+                //std::filesystem::create_directory(pngPath);
+
+                plot.gp.setOutput(appendFileSuffixToPath(pngPath, ts.ms()).string());
+            }
+
+
             // Plotting
+            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
+            plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
+            plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
             plot.showParticles(pf.getParticles());
             plot.setCurEst(estPos);
             plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
@@ -574,15 +599,13 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
             //plot.splot.getView().setCamera(0, 0);
             //plot.splot.getView().setEqualXY(true);
 
+            
+
             plot.plot();
             //std::this_thread::sleep_for(100ms);
         }
     }
     
-
-    //std::cin.get();
-
-
     printErrorStats(errorStats);
 
     std::ofstream plot_out;
@@ -597,6 +620,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
 
     errorPlot.frame();
 
+
+    std::cout << "Press any key to continue ..." << std::endl;
+    std::cin.get();
+
     // MATLAB output
     //{
     //    std::ofstream matlab_error_out;
@@ -718,7 +745,6 @@ int main(int argc, char** argv)
     {
         Settings::CurrentPath = setupToRun;
 
-
         for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
         {
             std::cout << "Executing walk " << walkIdx << "\n";

code/mainTrilat.cpp

@@ -35,20 +35,29 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
 {
     // reading file
     Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
-    Offline::FileReader fr(setup.training[walkIdx]);
+    Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
 
     // ground truth
-    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath);
+    std::vector<int> gtPath = setup.gtPath;
+    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
     CombinedStats<float> errorStats;
 
     //calculate distance of path
     std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
-    double distance = 0;
+    double gtTotalDistance = 0;
+    Stats::Statistics<double> gtWalkingSpeed;
     for (int i = 1; i < gtEntries.size(); ++i) {
-        distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
+        double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value);
+        double timeDiff = static_cast<double>(gtEntries[i].key - gtEntries[i - 1].key);
+
+        double walkingSpeed = distance / (timeDiff / 1000.0f); // m / s
+
+        gtWalkingSpeed.add(walkingSpeed);
+        gtTotalDistance += distance;
     }
 
-    std::cout << "Distance of Path: " << distance << std::endl;
+    std::cout << "Distance of Path: " << gtTotalDistance << std::endl;
+    std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl;
 
     // debug show
 //MeshPlotter dbg;
@@ -60,138 +69,139 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
 
     Plotty plot(map);
     plot.buildFloorplan();
-    plot.setGroundTruth(setup.gtPath);
+    plot.setGroundTruth(gtPath);
     plot.setView(30, 0);
+    // APs Positions
+    for (auto& nucConfig : setup.NUCs)
+    {
+        plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
+    }
     plot.plot();
 
+    std::vector<WiFiMeasurement> obs;
+
+    Timestamp lastTimestamp = Timestamp::fromMS(0);
+
     Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");
+    //plotta.add("apPos", apPositions);
 
-    std::vector<Point2> apPositions{
-        Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
-        Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(),
-        Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(),
-        Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
-    };
-
-    plotta.add("apPos", apPositions);
-
-    std::vector<WifiMeas> data = filterOfflineData(fr);
-
-    const bool UseFTM = false;
     const int movAvgWnd = 10;
-    std::array<MovingAVG<float>, 4> movAvgsFtm { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
+    std::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
-    std::array<MovingAVG<float>, 4> movAvgsRssi { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
+    std::unordered_map<MACAddress, MovingAVG<float>> movAvgsRssi;
 
+    for (auto& nucConfig : setup.NUCs)
+    {
+        movAvgsFtm.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
+        movAvgsRssi.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
+    }
+    
     std::vector<float> errorValuesFtm, errorValuesRssi;
     std::vector<int> timestamps;
 
-    std::vector<Point2> gtPath, estPathFtm, estPathRssi;
+    std::vector<Point2> estPathFtm, estPathRssi;
 
-    for (const WifiMeas& wifi : data)
+    for (const Offline::Entry& e : fr.getEntries())
     {
-        Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
+        if (e.type != Offline::Sensor::WIFI_FTM) {
-        plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
+            continue;
-        gtPath.push_back(gtPos);
+        }
-
+
-        float distErrorFtm = 0;
+        // TIME
-        float distErrorRssi = 0;
+        const Timestamp ts = Timestamp::fromMS(e.ts);
-
+
-        //if (wifi.numSucessMeas() == 4)
+        auto wifiFtm = fr.getWifiFtm()[e.idx].data;
-        {
+        obs.push_back(wifiFtm);
-            // FTM
+
-            {
+        if (ts - lastTimestamp >= Timestamp::fromMS(500))
-                std::vector<float> avgDists;
+        {
-
+            // Do update
-                for (size_t i = 0; i < 4; i++)
+            Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
-                {
+            plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
-                    float dist = wifi.ftmDists[i];
+            
-
+            std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;
-                    if (!isnan(dist))
+
-                    {
+            for (const WiFiMeasurement& wifi : obs)
-                        movAvgsFtm[i].add(dist);
+            {
-                    }
+                if (wifi.getNumSuccessfulMeasurements() < 3)
-
+                    continue;
-
+
-                    if (movAvgsFtm[i].getNumUsed() == 0)
+                const MACAddress& mac = wifi.getAP().getMAC();
-                    {
+                float ftm_offset = setup.NUCs.at(mac).ftm_offset;
-                        avgDists.push_back(0);
+                float ftmDist = wifi.getFtmDist() + ftm_offset;
-                    }
+
-                    else
+                float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
-                    {
+                float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
-                        avgDists.push_back(movAvgsFtm[i].get());
+
-                    }                   
+                movAvgsFtm[mac].add(ftmDist);
-                }
+                movAvgsRssi[mac].add(rssiDist);
-
+
-                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
+                apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
-
+            }
-                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
+
-                plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
+            if (apPosDistMap.size() > 3)
-
+            {
-                // draw wifi ranges           
+                // Do update for real
-                for (size_t i = 0; i < 4; i++)
+                std::vector<Point2> apPositions;
-                {
+                std::vector<float> ftmDists;
-                    plot.addCircle(i + 1, apPositions[i], avgDists[i]);
+                std::vector<float> rssiDists;
-                }
+
-
+                for (const auto& kvp : apPosDistMap)
-                // Error
+                {
-                distErrorFtm = gtPos.getDistance(estPos);
+                    apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
-                errorStats.ftm.add(distErrorFtm);
+                    ftmDists.push_back(kvp.second.first);
-                estPathFtm.push_back(estPos);
+                    rssiDists.push_back(kvp.second.second);
-            }
+                }
-
+
-
+                Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
-            // RSSI
+                Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);
-            {
+
-                std::vector<float> avgDists;
+                // Error
-
+                float distErrorFtm = gtPos.getDistance(estFtmPos);
-                for (size_t i = 0; i < 4; i++)
+                errorStats.ftm.add(distErrorFtm);
-                {
+                estPathFtm.push_back(estFtmPos);
-                    float dist = wifi.rssiDists[i];
+
-
+                float  distErrorRssi = gtPos.getDistance(estRssiPos);
-                    if (!isnan(dist))
+                errorStats.rssi.add(distErrorRssi);
-                    {
+                estPathRssi.push_back(estRssiPos);
-                        movAvgsRssi[i].add(dist);
+
-                    }
+                errorValuesFtm.push_back(distErrorFtm);
-
+                errorValuesRssi.push_back(distErrorRssi);
-
+                timestamps.push_back(ts.ms());
-                    if (movAvgsRssi[i].getNumUsed() == 0)
+
-                    {
+                plotta.add("t", timestamps);
-                        avgDists.push_back(0);
+                plotta.add("errorFtm", errorValuesFtm);
-                    }
+                plotta.add("errorRssi", errorValuesRssi);
-                    else
+                plotta.frame();
-                    {
+
-                        avgDists.push_back(movAvgsRssi[i].get());
+                // Plot
-                    }
+                plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1));
-                }
+                plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1));
-
+                plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1));
-                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
+
-
+                // draw wifi ranges           
-                plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
+                if (Settings::PlotCircles)
-
+                {
-                // Error
+                    plot.clearDistanceCircles();
-                distErrorRssi = gtPos.getDistance(estPos);
+
-                errorStats.rssi.add(distErrorRssi);
+                    for (size_t i = 0; i < ftmDists.size(); i++)
-                estPathRssi.push_back(estPos);
+                    {
-            }
+                        plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
-            
+                        plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
-            //std::cout << wifi.ts.ms() << " " << distError << "\n";
+                    }
-            
+                }
-            errorValuesFtm.push_back(distErrorFtm);
+
-            errorValuesRssi.push_back(distErrorRssi);
+
-            timestamps.push_back(wifi.ts.ms());
+                plot.plot();
-
+                Sleep(100);
-            plotta.add("t", timestamps);
+            }
-            plotta.add("errorFtm", errorValuesFtm);
+
-            plotta.add("errorRssi", errorValuesRssi);
+            obs.clear();
-            plotta.frame();
+            lastTimestamp = ts;
         }
 
-        plot.plot();
-        //Sleep(250);
         printf("");
     }
 
-    plotta.add("gtPath", gtPath);
     plotta.add("estPathFtm", estPathFtm);
     plotta.add("estPathRssi", estPathRssi);
     plotta.frame();
@@ -211,46 +221,64 @@ int mainTrilat(int argc, char** argv)
     CombinedStats<float> statsQuantil;
     CombinedStats<float> tmp;
 
-    std::string evaluationName = "prologic/tmp";
+    std::string evaluationName = "prologic/trilat";
 
-    for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
+    std::vector<Settings::DataSetup> setupsToRun = {
+        //Settings::data.Path5,
+        //Settings::data.Path7,
+        //Settings::data.Path8,
+        //Settings::data.Path9,
+        //Settings::data.Path10,
+        //Settings::data.Path11
+        //Settings::data.Path20,
+        Settings::data.Path21,
+        //Settings::data.Path22,
+    };
+
+    for (Settings::DataSetup setupToRun : setupsToRun)
     {
-        std::cout << "Executing walk " << walkIdx << "\n";
+        Settings::CurrentPath = setupToRun;
-        for (int i = 0; i < 1; ++i)
+
+        for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
         {
-            std::cout << "Start of iteration " << i << "\n";
+            std::cout << "Executing walk " << walkIdx << "\n";
+            for (int i = 0; i < 1; ++i)
+            {
+                std::cout << "Start of iteration " << i << "\n";
 
-            tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
+                tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
 
-            statsAVG.ftm.add(tmp.ftm.getAvg());
+                statsAVG.ftm.add(tmp.ftm.getAvg());
-            statsMedian.ftm.add(tmp.ftm.getMedian());
+                statsMedian.ftm.add(tmp.ftm.getMedian());
-            statsSTD.ftm.add(tmp.ftm.getStdDev());
+                statsSTD.ftm.add(tmp.ftm.getStdDev());
-            statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
+                statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
 
-            statsAVG.rssi.add(tmp.rssi.getAvg());
+                statsAVG.rssi.add(tmp.rssi.getAvg());
-            statsMedian.rssi.add(tmp.rssi.getMedian());
+                statsMedian.rssi.add(tmp.rssi.getMedian());
-            statsSTD.rssi.add(tmp.rssi.getStdDev());
+                statsSTD.rssi.add(tmp.rssi.getStdDev());
-            statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
+                statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
 
-            std::cout << "Iteration " << i << " completed" << std::endl;
+                std::cout << "Iteration " << i << " completed" << std::endl;
+            }
         }
+
+        std::cout << "Results for path " << Settings::CurrentPath.name << std::endl;
+        std::cout << "==========================================================" << std::endl;
+        std::cout << "Average of all statistical data FTM: " << std::endl;
+        std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
+        std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
+        std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
+        std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
+        std::cout << "==========================================================" << std::endl;
+
+        std::cout << "==========================================================" << std::endl;
+        std::cout << "Average of all statistical data RSSI: " << std::endl;
+        std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
+        std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
+        std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
+        std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
+        std::cout << "==========================================================" << std::endl;
     }
 
-    std::cout << "==========================================================" << std::endl;
-    std::cout << "Average of all statistical data FTM: " << std::endl;
-    std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
-    std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
-    std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
-    std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
-    std::cout << "==========================================================" << std::endl;
-
-    std::cout << "==========================================================" << std::endl;
-    std::cout << "Average of all statistical data RSSI: " << std::endl;
-    std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
-    std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
-    std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
-    std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
-    std::cout << "==========================================================" << std::endl;
-
     return 0;
 }

code/misc.h

@@ -12,6 +12,43 @@
 
 #include "Settings.h"
 
+static std::string timeForFilename(const std::time_t& time)
+{
+    std::stringstream ss;
+    ss << std::put_time(std::localtime(&time), "%F_%T"); // ISO 8601 without timezone information.
+    auto s = ss.str();
+    std::replace(s.begin(), s.end(), ':', '-');
+    return s;
+}
+
+static std::string currentTimeForFilename()
+{
+    auto time = std::time(nullptr);
+    return timeForFilename(time);
+}
+
+static std::filesystem::path appendCurrentTimeToFilename(const std::filesystem::path& fileName)
+{
+    return fileName.stem().string() + "_" + currentTimeForFilename() + fileName.extension().string();
+}
+
+template<typename T>
+inline std::filesystem::path appendFileSuffixToPath(const std::filesystem::path& path, T value)
+{
+    std::string filename = path.stem().string() + "_" + std::to_string(value) + path.extension().string();
+    return path.parent_path() / filename;
+}
+
+
+static bool forceDirectories(const std::filesystem::path& path)
+{
+    if (!std::filesystem::exists(path)) {
+        return std::filesystem::create_directories(path);
+    }
+    return true;
+}
+
+
 template<typename T, int Size>
 std::vector<T> asVector(const std::array<T, Size>& src)
 {

code/trilateration.cpp

@@ -0,0 +1,207 @@
+#include "trilateration.h"
+
+#include <cmath>
+#include <iostream>
+
+#include <Eigen/Eigen>
+
+#include <unsupported/Eigen/NonLinearOptimization>
+#include <unsupported/Eigen/NumericalDiff>
+
+namespace Trilateration
+{
+    // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
+
+    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    {
+        // To locate position on a 2d plan, have to get at least 3 becaons,
+        // otherwise return false.
+        if (positions.size() < 3)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 2);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, r1, r2;
+
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y;
+                x2 = positions[j].x, y2 = positions[j].y;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                    (pow(y1, 2) - pow(y2, 2)) -
+                    (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        //Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+        Eigen::Vector2d pseudoInv = (m.transpose()*m).inverse() * m.transpose() *b;
+
+        return Point2(pseudoInv.x(), pseudoInv.y());
+    }
+
+    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
+    {
+        // To locate position in a 3D space, have to get at least 4 becaons
+        if (positions.size() < 4)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 3);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, z1, z2, r1, r2;
+
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
+                x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                m(row, 2) = z1 - z2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                    (pow(y1, 2) - pow(y2, 2)) +
+                    (pow(z1, 2) - pow(z2, 2)) -
+                    (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+        return Point3(location.x(), location.y(), location.z());
+    }
+
+
+    // Generic functor
+    // See http://eigen.tuxfamily.org/index.php?title=Functors
+    // C++ version of a function pointer that stores meta-data about the function
+    template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
+    struct Functor
+    {
+
+        // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
+        typedef _Scalar Scalar;
+        enum { // Required by numerical differentiation module
+            InputsAtCompileTime = NX,
+            ValuesAtCompileTime = NY
+        };
+
+        // Tell the caller the matrix sizes associated with the input, output, and jacobian
+        typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
+        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
+        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
+
+        // Local copy of the number of inputs
+        int m_inputs, m_values;
+
+        // Two constructors:
+        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
+        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
+
+        // Get methods for users to determine function input and output dimensions
+        int inputs() const { return m_inputs; }
+        int values() const { return m_values; }
+
+    };
+
+    struct DistanceFunction : Functor<double>
+    {
+    private:
+        const std::vector<Point2>& positions;
+        const std::vector<float>& distances;
+
+    public:
+        DistanceFunction(const std::vector<Point2>& positions, const std::vector<float>& distances)
+            : Functor<double>(positions.size(), positions.size()), positions(positions), distances(distances)
+        {}
+
+        int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const 
+        {
+            const Point2 p(x(0), x(1));
+
+            for (size_t i = 0; i < positions.size(); i++)
+            {
+                fvec(i) = p.getDistance(positions[i]) - distances[i];
+            }
+
+            return 0;
+        }
+    };
+
+    struct DistanceFunctionDiff : public Eigen::NumericalDiff<DistanceFunction>
+    {
+        DistanceFunctionDiff(const DistanceFunction& functor)
+            : Eigen::NumericalDiff<DistanceFunction>(functor, 1.0e-6)
+        {}
+    };
+
+    Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    {
+        Point2 pseudoInvApprox = calculateLocation2d(positions, distances);
+        Eigen::Vector2d initVal;
+        initVal << pseudoInvApprox.x, pseudoInvApprox.y;
+
+        Eigen::Vector2d startVal;
+        //startVal << pseudoInvApprox.x, pseudoInvApprox.y;
+        startVal << 0, 0;
+
+        DistanceFunction functor(positions, distances);
+        DistanceFunctionDiff numDiff(functor);
+        Eigen::LevenbergMarquardt<DistanceFunctionDiff, double> lm(numDiff);
+        lm.parameters.maxfev = 2000;
+        lm.parameters.xtol = 1.0e-10;
+        std::cout << lm.parameters.maxfev << std::endl;
+
+        Eigen::VectorXd z = startVal;
+        int ret = lm.minimize(z);
+        std::cout << "iter count: " << lm.iter << std::endl;
+        std::cout << "return status: " << ret << std::endl;
+        std::cout << "zSolver: " << z.transpose() << std::endl;
+        std::cout << "pseudoInv: " << initVal.transpose() << std::endl;
+
+
+        Point2 bla(z(0), z(1));
+
+        double errPseudo = 0;
+        double errLeven = 0;
+        for (size_t i = 0; i < positions.size(); i++)
+        {
+            double d1 = pseudoInvApprox.getDistance(positions[i]) - distances[i];
+            errPseudo += d1 * d1;
+
+            double d2 = bla.getDistance(positions[i]) - distances[i];
+            errLeven += d2 * d2;
+        }
+
+        //assert(errLeven <= errPseudo);
+
+        std::cout << "err pseud: " << errPseudo << std::endl;
+        std::cout << "err leven: " << errLeven << std::endl << std::endl;
+
+        return Point2(z(0), z(1));
+
+    }
+}

code/trilateration.h

@@ -1,95 +1,14 @@
 #pragma once
 
-#include <cmath>
 #include <vector>
 
-#include <eigen3/Eigen/Eigen>
-
 #include <Indoor/geo/Point2.h>
 #include <Indoor/geo/Point3.h>
 
 namespace Trilateration
 {
-    // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
+    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances);
+    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances);
 
-    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances);
-    {
+};
-        // To locate position on a 2d plan, have to get at least 3 becaons,
-        // otherwise return false.
-        if (positions.size() < 3)
-            assert(false);
-        if (positions.size() != distances.size())
-            assert(false);
-
-        // Define the matrix that we are going to use
-        size_t count = positions.size();
-        size_t rows = count * (count - 1) / 2;
-        Eigen::MatrixXd m(rows, 2);
-        Eigen::VectorXd b(rows);
-
-        // Fill in matrix according to the equations
-        size_t row = 0;
-        double x1, x2, y1, y2, r1, r2;
-        
-        for (size_t i = 0; i < count; ++i) {
-            for (size_t j = i + 1; j < count; ++j) {
-                x1 = positions[i].x, y1 = positions[i].y;
-                x2 = positions[j].x, y2 = positions[j].y;
-                r1 = distances[i];
-                r2 = distances[j];
-                m(row, 0) = x1 - x2;
-                m(row, 1) = y1 - y2;
-                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
-                    (pow(y1, 2) - pow(y2, 2)) -
-                    (pow(r1, 2) - pow(r2, 2))) / 2;
-                row++;
-            }
-        }
-
-        // Then calculate to solve the equations, using the least square solution
-        Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
-
-        return Point2(location.x(), location.y());
-    }
-
-    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
-    {
-        // To locate position in a 3D space, have to get at least 4 becaons
-        if (positions.size() < 4)
-            assert(false);
-        if (positions.size() != distances.size())
-            assert(false);
-
-        // Define the matrix that we are going to use
-        size_t count = positions.size();
-        size_t rows = count * (count - 1) / 2;
-        Eigen::MatrixXd m(rows, 3);
-        Eigen::VectorXd b(rows);
-
-        // Fill in matrix according to the equations
-        size_t row = 0;
-        double x1, x2, y1, y2, z1, z2, r1, r2;
-
-        for (size_t i = 0; i < count; ++i) {
-            for (size_t j = i + 1; j < count; ++j) {
-                x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
-                x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
-                r1 = distances[i];
-                r2 = distances[j];
-                m(row, 0) = x1 - x2;
-                m(row, 1) = y1 - y2;
-                m(row, 2) = z1 - z2;
-                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
-                          (pow(y1, 2) - pow(y2, 2)) +
-                          (pow(z1, 2) - pow(z2, 2)) -
-                          (pow(r1, 2) - pow(r2, 2))) / 2;
-                row++;
-            }
-        }
-
-        // Then calculate to solve the equations, using the least square solution
-        Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
-
-        return Point3(location.x(), location.y(), location.z());
-    }
-}

tex/bare_conf.tex

@@ -65,7 +65,7 @@ $^{2}$ \quad University of Siegen - Pattern Recognition Group; marcin.grzegorzek
 \newcommand{\mat}[1]{\vec{#1}}					% the same as vec
 
 % gfx include folder
-%\graphicspath{{gfx/}}
+\graphicspath{ {./gfx/} {./gfx/placeholder/} }
 
 % input stuff
 \input{misc/keywords}
@@ -80,8 +80,6 @@ $^{2}$ \quad University of Siegen - Pattern Recognition Group; marcin.grzegorzek
 }
 
 
-\graphicspath{gfx/}
-
 \input{chapters/0_abstract}
 
 \begin{document}

tex/chapters/3_ftm.tex

@@ -37,26 +37,29 @@
 \section{Wi-Fi Range Measurements}
 \label{sec:ftm}
 
-A obvious approach to estimate a location is to measure the distance between the current unknown position and a known position.
+An obvious approach to estimate a location is to measure the distance between the current unknown position and a known position.
 Given multiple measurements to different reference points an absolute position in a local coordinate system can be found.
-With ideal distance measurements it is straightforward to calculate the current position.
+With ideal distance measurements to several known positions it is straightforward to calculate the current position.
-However, in the present of noise and imperfect measurements estimating a accurate position is a challenging problem.
+However, in the present of noisy and imperfect measurements estimating a accurate position is a challenging problem.
+For a smartphone based indoor localization system using the existing Wi-Fi infrastructure is a reasonable choice.
+In this work signal strength based and signal propagation time based distance measurements are considered.
 
 \subsection{Received Signal Strength Indication}
 % TODO dBm vs dB??
 
 Received Signal Strength Indication (RSSI) is a measure of the received RF power and is obtained by the radio hardware at the antenna connector using an analog-to-digital converter.
-It is usually expressed in \si{\dBm} and quantified to integer values.
+It's value is usually expressed in \si{\dBm} and quantified to integer values.
 For indoor localization RSSI is often used to deduce the distance from a smartphone to the access point, because it is virtually always available on common devices.
 
+In order to calculate the distance from RSSI a appropriate signal strength prediction model needs to be chosen.
 The \docLogDistance{} model is commonly used to predict the received signal strength $P_i$ from an AP $i$ at a given distance $d_i$.
-Which is formally given with
+Formally given with
 \begin{equation}
 	P_i = \mTXP - 10 \mPLE \log_{10}{\frac{\mMdlDist_i}{\mMdlDist_0}} + \mathcal{X}_{\sigma_i} \text{,}
 	\label{eq:logDistModel}
 \end{equation}
 where $\mTXP$ denotes the sending power in \si{\dBm} of the AP at reference distance $\mMdlDist_0$ (usually \SI{1}{\meter}), $\mPLE$ is the path loss exponent, which value needs to be empirically chosen for the given environment.
-The added zero-mean Gaussian random variable $\mathcal{X}_{\sigma_i}$ with a variance of $\sigma^2_i \si{\dBm}$ models signal fading and random channel noise.
+The added zero-mean Gaussian random variable $\mathcal{X}_{\sigma_i}$ with a variance of $\SIm{\sigma^2_i}{\dBm}$ models signal fading and random channel noise.
 Hence, the measured RSSI is assumed to follow a normal distribution $P_i \sim \mathcal{N}(P_i^*, \sigma_i^2)$, where $P_i^*$ is the expected RSSI and $\sigma_i^2$ is the variance of the measurement.
 %, both given in \si{\dBm}.
 
@@ -66,7 +69,8 @@ The \docLogDistance{} model can be reformulated to compute the distance $d_i$ ba
 \end{equation}
 \begin{equation}
 %  d_i = 10^{(\mTXP-P_i) / 10\mPLE} + 10^{\mathcal{X}_{\sigma^2_i}/10\mPLE}
-  d_i = 10^{\sfrac{(\mTXP-P_i + \mathcal{X}_{\sigma_i})}{10\mPLE}}
+%  d_i = 10^{\sfrac{(\mTXP-P_i + \mathcal{X}_{\sigma_i})}{10\mPLE}}
+  d_i = 10^{  (\mTXP-P_i + \mathcal{X}_{\sigma_i}) / 10\mPLE}
 \end{equation}
 
 Since $\mathcal{X}_{\sigma_i}$ is a Gaussian random variable, the logarithm of $d_i$ is normally distributed as well.
@@ -77,10 +81,10 @@ In indoor scenarios $\mPLE$ accounts for the architecture around the AP, thus a
 %This restricts the \docLogDistance{} model to a uniform view on the complete environment and does not allow to differentiate between different types of materials, and ignores  which walls are actually transmitted the signal.
 
 This restricts the \docLogDistance{} model to a uniform view on the whole environment and does not take the actual propagation path of the signal into account.
-Therefore, the model does not consider the actual signal propagation path and thus makes it impossible to differentiate between different types of obstacles or wall materials.
+Therefore, the model does not consider the actual environmental effects or geometry, and thus makes it impossible to differentiate between different types of obstacles or wall materials.
 
 In order to take walls into account the model must include the power loss of every traversed wall, which results in the wall-attenuation factor model \cite{TODO}.
-Often the dampening factors of walls are unknown and hard to measure.
+Often the dampening factors of walls are unknown or hard to measure.
 Additionally, the computation of the wall-attenuation factor model requires costly intersection tests with the geometry of the environment which can be intractable to perform on a regular smartphone.
 
 %Another approach is to take measurements at known positions distributed throughout the building.
@@ -111,6 +115,7 @@ While being precise, ToA requires costly high precision synchronized clocks, whi
 
 Two way ranging (TWR) eliminates the requirement for synchronized clocks.
 \ieeWifiFTM{} defines the fine timing measurement (FTM) protocol, which implements the TWR method for standard conform WiFi devices.
+Note that it is not necessary to implement FTM capabilities to be a \ieeWifiFTM conform device.
 This made time-based distance measurements broadly available for WiFi based systems and relevant for smartphone based indoor localization.
 Instead of using absolute time, the round trip time is measured based on time differences at the sender and receiver.
 As successive time measurements are only done at one site synchronized clocks are not required.
@@ -133,12 +138,17 @@ To exclude the processing delay of the initiator the difference between $t_2$ an
 While RF power is relatively simple to measure, obtaining accurate ToF values at a small resolution like nanoseconds needs much more caution, as the measurements are sensitive to noise.
 Relatively small deviations from the real time value result in a vast error in the distance estimate, \eg a measurement error of \SI{10}{ns} results in a distance error of \SI{3}{m}.
 For this reason the above outlined procedure is repeated multiple times to reduce the impact of noise.
-In fact, a single FTM measurement consists of many FTM-ACK exchanges and the final value $ToF^*$ is the average over the $n$ measurements
+In fact, a single FTM measurement or burst instance, consists of many FTM-ACK exchanges and the final value $\text{ToF}^*$ is the average over $n$ measurements
 \begin{equation}
-  ToF^* = \frac{1}{n} \sum_{k=1}^{n} \left[ (t_{4,k}-t_{1,k}) - (t_{3,k}-t_{2,k}) \right]
+  \text{ToF}^* = \frac{1}{n} \sum_{k=1}^{n} \left[ (t_{4,k}-t_{1,k}) - (t_{3,k}-t_{2,k}) \right]
 \end{equation}
 
 After calculating the average ToF the responder transfers the result to the initiator where the result can be processed by an application.
+With increasing $n$ the impact of noise is lessened, but the time until the FTM measurement is available for the consuming software increases.
+Therefore, the actual choice of the value of $n$ is a trade-off between precision and measurement delay.
+
+%TODO ToF -> distance ToF/2 * c
+%TODO IEEE 802.11-2016 6.3.58.1
 
 The accuracy of distance estimate depends on the ability of the hardware to detect the line-of-sight signal, or direct path.
 In an indoor environment it is very common that a signal will reach the receiver from different paths with different lengths.
@@ -150,13 +160,14 @@ This results in an over-estimate of the propagation time of the signal, and cons
 The limiting factor is the sampling rate of the receiving hardware, which is defined by the channel bandwidth.
 Hence, the time resolution is proportional to the inverse of the bandwidth.
 
-
 In \ieeWifiN the channel bandwidth is \SI{20}{Mhz} in the \SI{2.4}{GHz} range which results in a sampling rate of one sample every \SI{50}{ns}, or one sample every \SI{12.5}{ns} for \SI{80}{Mhz} channels in the \ieeWifiAC \SI{5}{GHz} range.
 Assuming that the receiver recognizes the signal at the first sample of the preamble the smallest possible resolution of the range estimate is \SI{15}{m} for \SI{20}{Mhz} bandwidth, and \SI{3.74}{m} for \SI{80}{Mhz}.
 To allow much finer resolution the receiver uses super resolution methods to allow sub-sample resolution \cite{TODO}.
 
 %Therefore, time-based distance estimates can greatly differ from the ideal euclidean distance.
 
+In addition to distance measurements the \ieeWifiFTM standard defines a format to transfer location information about the responder.
+This allows to add new access points dynamically to the localization system without updating the initiators, \ie smartphone, as the access point can be configured to know its position and can transmit this information to the smartphone.
 
 Error sources:
 multipath propagation, noise, finite sample rate

tex/chapters/4_ftmloc.tex

@@ -3,24 +3,48 @@
 
 Bei Indoor Lokalisierung geht es darum eine Position zu ermitteln. Hierfür nutzen wir unterschiedliche Verfahren. namley... usw. 
 
+After measuring several distances to different anchor points one can calculate his current position.
+%TODO Alles mit 2D, weil halt
+
 \subsection{Multilateration}
-Ganz kurz erläutern was Multilateration eigentlich ist. in 2D min 3 aps und in 3D min. 4D. Aber grundsätzlich gilt: viel hilft viel.
+%Ganz kurz erläutern was Multilateration eigentlich ist. in 2D min 3 aps und in 3D min. 4D. Aber grundsätzlich gilt: viel hilft viel.
-Es ist uns klar, dass Trilat nichts taugt aber ist halt der einfachste Schritt
+%Es ist uns klar, dass Trilat nichts taugt aber ist halt der einfachste Schritt
-Typische Nachteile: Wenn Schnittpunkt nicht analytisch exakt bestimmt werden können
+%Typische Nachteile: Wenn Schnittpunkt nicht analytisch exakt bestimmt werden können
-FTM Nachteil: Häufig fallen die Messungen aus? Was tun? Alte Werte statisch halten? Keine est berechnen?
+%FTM Nachteil: Häufig fallen die Messungen aus? Was tun? Alte Werte statisch halten? Keine est berechnen?
+%Conceptually, multilateration determines the position by analytically intersecting at least $3$ circles for a 2-dimensional position, or at least $4$ spheres in case of a 3-dimensional coordinate system.
+
+Each distance measurement $d_i$ constrains the position estimate $\hat{\mPosVec}$ to a circle, where the center of the circle is the known position $\mPosVec_i = (x,y)^T$ of AP $i$.
+Formally the distance is the euclidean distance between the known position and the estimate
+\begin{equation}
+d_i = \| \mPosVec_i - \hat{\mPosVec} \|   \text{.}
+\end{equation}
+In two dimensions three ideal distances form a system of linear equations which can be uniquely solved to obtain the position.
+Given more than three distances no solution can be found which stratifies all the constraints as the linear system is overdetermined.
+Additionally, in the presence of noise and inaccurate measurements an exact analytical solution is not possible.
+In this case an approximative solution $\mPosVec^*$ can be found by using a least squares approach which minimizes the quadratic error between the measured distance and the actual distance at a given point
+\begin{equation}
+  \mPosVec^* = \argmin_{\hat{\mPosVec}} \sum_{i}^{} \left(  \| \mPosVec_i - \hat{\mPosVec} \| - d_i \right)^2  \text{.}
+\end{equation}
+
+
+% each distance defines the position on a circle
+% intersecting the circles gives the position
+% at least 3 circles
+% only works for perfect measurments
+
 
 
 \subsection{Probabilistic}
-Dichte aus Messungen erzeugen.
+%Dichte aus Messungen erzeugen.
-Distanzern werden mit Normalverteilung gewichtet
+%Distanzern werden mit Normalverteilung gewichtet
-Vorteil: Nicht ideale Schnittpunkte sind kein Problem, weil die Dichte sowas abbilden kann
+%Vorteil: Nicht ideale Schnittpunkte sind kein Problem, weil die Dichte sowas abbilden kann
-FTM Vorteil: Fehlen von Messungen kann probabilistisch erfasst werden indem Streuung der NV größer wird / Oder es enstehen einfach mehrere Hypthesen über die Position
+%FTM Vorteil: Fehlen von Messungen kann probabilistisch erfasst werden indem Streuung der NV größer wird / Oder es enstehen einfach mehrere Hypthesen über die Position
 
 \subsection{Particle Filter}
-Warum auch noch PF? Weil... die meisten lokalisierungs systeme diesen als nicht-linearen schätzer benutzen. er ist vielseitig und kann einfach mit anderen sensoren kombiniert werden. ist das gängigste sensor fusionsverfahren. die dichte wird in samples repräsentiert und ist damit nur eine approximatino der wahrsch. dichte.. dadurch ganz ander repräsentation als probabilistic teil. außerdem ist es ein FILTER, hat also vergangenheit mit drin.
+%Warum auch noch PF? Weil... die meisten lokalisierungs systeme diesen als nicht-linearen schätzer benutzen. er ist vielseitig und kann einfach mit anderen sensoren kombiniert werden. ist das gängigste sensor fusionsverfahren. die dichte wird in samples repräsentiert und ist damit nur eine approximatino der wahrsch. dichte.. dadurch ganz ander repräsentation als probabilistic teil. außerdem ist es ein FILTER, hat also vergangenheit mit drin.
-Ganz schnell nochmal den PF einführen mit standard formel. ein größerer absatz. 
+%Ganz schnell nochmal den PF einführen mit standard formel. ein größerer absatz. 
-Einfaches Bewegungsmodell mit NV random
+%Einfaches Bewegungsmodell mit NV random
-ftm bzw. rssi einfach über normalverteilung in die evaluation rein.
+%ftm bzw. rssi einfach über normalverteilung in die evaluation rein.
 
 
 

tex/chapters/8_experiments.tex

@@ -77,6 +77,52 @@ der einsatz der probablistischen methode sieht weitaus besser azs als multilater
 	\item Hier werden die Ergebnisse langsam richtig gut. Also FTM ist weitaus besser als RSSI. Warum ist das so? (Filter hat die Vergangenheit mit drin, FTM ist somit auf Dauer stabiler und streut deswegen nicht so stark wie RSSI).
 \end{itemize}
 
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{Path2_Walk0_FTM.png}
+\caption{Path 2 FTM, Pixel 2 mean 4.94778 stdDev 2.49081 median 4.88544 count 249}
+\end{figure}
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{Path2_Walk0_FTM_Error.png}
+\caption{Path 2 Error FTM, Pixel 2 }
+\end{figure}
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{Path2_Walk0_RSSI.png}
+\caption{Path 2 RSSI, Pixel 2 mean 5.33965 stdDev 2.68662 median 5.22802 count 249}
+\end{figure}
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{Path2_Walk0_RSSI_Error.png}
+\caption{Path 2 Error RSSI, Pixel 2}
+\end{figure}
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{Path2_Walk0_FTM_RSSI_Error.png}
+\caption{Path 2 Error FTM / RSSI, Pixel 2}
+\end{figure}
+
+Unterscuhen ob die Abweichung nach untne daher kommt weil die Brandschutztür stört
+
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{VersuchsaufbauBST1.png}
+\caption{}
+\end{figure}
+
+\begin{figure}[ht]
+\centering
+\includegraphics[width=1\textwidth]{VersuchsaufbauBST2.png}
+\caption{}
+\end{figure}
+
 \subsection{Comparison and Discussion}
 Hier vergleichen wie sich die einzelen Verfahren untereinander unterscheiden und welche Vor / Nachteile sie haben. Jeweils für RSSI und FTM. (Vorher haben wir RSSI und FTM gegenübergestellt innerhalb der Verfahren und jetzt stellen wir die einzelnen Verfahren gegenüber und diskutieren deren Unterschied in Bezug auf die WI-Fi methoden)
 

tex/misc/functions.tex

@@ -129,7 +129,8 @@
 %\newcommand{\docAPs}{access-points}
 
 
-
+% \SI command for math mode formulas with correct spacing
+\newcommand{\SIm}[2]{\ensuremath{#1\,\si{#2}}}
 
 \newcommand{\R}{\mathbb{R}}
 \newcommand{\N}{\mathbb{N}}