worked on signal-strength-estimation

add this information to grid nodes
evaluate this information
new test-cases
This commit is contained in:
2016-07-15 15:29:07 +02:00
parent 34e52cd7f0
commit 99ee95ce7b
21 changed files with 568 additions and 26 deletions

4
grid/DefaultGridNode.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef DEFAULTGRIDNODE_H
#define DEFAULTGRIDNODE_H
#endif // DEFAULTGRIDNODE_H

View File

@@ -24,14 +24,18 @@ private:
/** INTERNAL: node's index array-index */
int _idx;
/** semantic annotation for this node */
uint8_t _type;
/** INTERNAL: store neighbors (via index) */
int _neighbors[10];
/** INTERNAL: number of neighbors */
uint8_t _numNeighbors;
/** INTERNAL: store neighbors (via index) */
int _neighbors[10];
/** semantic annotation for this node */
uint8_t _type;
public:
@@ -43,7 +47,9 @@ public:
public:
GridNode() : _idx(-1), _numNeighbors(0), _neighbors() {;}
/** ctor */
GridNode() : _idx(-1), _neighbors(), _numNeighbors(0), _type(0) {;}
/** get the node's index within its grid */
int getIdx() const {return _idx;}

View File

@@ -48,8 +48,15 @@ struct GridPoint {
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
/** cast to Point3 */
operator Point3() const {return Point3(x_cm, y_cm, z_cm);}
///** cast to Point3 */
//operator Point3() const {return Point3(x_cm, y_cm, z_cm);}
/** convert to Point3 in centimeter */
Point3 inCentimeter() const {return Point3(x_cm, y_cm, z_cm);}
/** convert to Point3 in meter */
Point3 inMeter() const {return Point3(x_cm/100.0f, y_cm/100.0f, z_cm/100.0f);}
/** cast to string */
operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}

View File

@@ -159,12 +159,12 @@ public:
template <typename T> bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
MiniMat2 m;
Point3 center = nSrc;
Point3 center = nSrc.inCentimeter();
// calculate the centroid of the nSrc's nearest-neighbors
Point3 centroid(0,0,0);
for (const T* n : neighbors) {
centroid = centroid + (Point3)*n;
centroid = centroid + n->inCentimeter();
}
centroid /= neighbors.size();
@@ -174,7 +174,7 @@ public:
// build covariance of the nearest-neighbors
int used = 0;
for (const T* n : neighbors) {
Point3 d = (Point3)*n - center;
Point3 d = n->inCentimeter() - center;
if (d.length() > 100) {continue;} // radius search
m.addSquared(d.x, d.y);
++used;

View File

@@ -0,0 +1,27 @@
#ifndef LOCATEDACCESSPOINT_H
#define LOCATEDACCESSPOINT_H
#include "AccessPoint.h"
#include "../../geo/Point3.h"
#include "../../floorplan/v2/Floorplan.h"
/**
* describes an access-point including its position
*/
class LocatedAccessPoint : public AccessPoint, public Point3 {
public:
/** ctor */
LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) {
;
}
/** ctor */
LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) {
;
}
};
#endif // LOCATEDACCESSPOINT_H

View File

@@ -0,0 +1,84 @@
#ifndef WIFIGRIDESTIMATOR_H
#define WIFIGRIDESTIMATOR_H
#include "../../grid/Grid.h"
#include "model/WiFiModel.h"
#include "WiFiGridNode.h"
#include "../../Assertions.h"
#include <fstream>
/**
* estimates the signal-strength for all APs at a given GridNode
* and stores this precalculated value onto the node
*/
class WiFiGridEstimator {
public:
/**
* perform a signal-strength estimation for all of the given access points
* using the provided signal-strength prediction model.
* store the estimated strength onto each node.
* as nodes only provide a limited number of rssi-entries,
* store only the strongest ones.
*/
template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const std::vector<LocatedAccessPoint> aps) {
// sanity checks
Assert::isTrue(Node::getMapAPs().empty(), "there are already some processed APs available!");
// attach the access-points to the shared node-vector
for (const LocatedAccessPoint& ap : aps) {
Node::getMapAPs().push_back(ap);
}
// process each node
for (Node& n : grid) {
// keep the strongest APs to attach to this node
std::vector<WiFiGridNodeAP> nodeAPs;
// process each AP
for (int apIdx = 0; apIdx < (int) aps.size(); ++apIdx) {
// estimate the signal-strength
const float rssi = mdl.getRSSI(aps[apIdx], n.inMeter());
// keep it
nodeAPs.push_back(WiFiGridNodeAP(apIdx, rssi));
}
// sort all APs by signal strength
auto comp = [] (const WiFiGridNodeAP& ap1, const WiFiGridNodeAP& ap2) {return ap1.getRSSI() > ap2.getRSSI();};
std::sort(nodeAPs.begin(), nodeAPs.end(), comp);
// attach the strongest X to the node
const int cnt = std::min( n.getMaxAPs(), (int) nodeAPs.size() );
for (int i = 0; i < cnt; ++i) {
n.strongestAPs[i] = nodeAPs[i];
}
}
}
/** gnuplot visualization dump for the given AP */
template <typename Node> static void dump(Grid<Node>& grid, const std::string& mac, const std::string& filename) {
std::ofstream out(filename);
out << "splot '-' with points palette \n";
for (Node& n : grid) {
out << n.x_cm << " " << n.y_cm << " " << n.z_cm << " " << n.getRSSI(mac) << "\n";
}
out << "e\n";
out.close();
}
};
#endif // WIFIGRIDESTIMATOR_H

View File

@@ -0,0 +1,123 @@
#ifndef WIFIGRIDNODE_H
#define WIFIGRIDNODE_H
#include <cstdint>
#include "AccessPoint.h"
/**
* rssi model-estimation for one AP, denoted by its index [among all APs present within the map]
* the signal strength is stored as uint8_t and allows a granularity of 0.25 dB between -40 and -103 dB
*/
struct WiFiGridNodeAP {
private:
static const int UNUSED_IDX = 255;
private:
/** access-point index -> max 255 APs/Map */
uint8_t idx;
/** access-point rssi */
uint8_t rssi;
public:
/** empty ctor */
WiFiGridNodeAP() : idx(UNUSED_IDX), rssi(0) {;}
/** ctor */
WiFiGridNodeAP(const int idx, const float rssi) : idx(idx), rssi(stretch(rssi)) {;}
/** is this entry valid/used? */
bool isValid() const {return idx != UNUSED_IDX;}
/** set AP and its signal strength */
void set(const int apIdx, const float rssi) {
this->idx = apIdx;
this->rssi = stretch(rssi);
}
/** get the predicted signal-strength */
float getRSSI() const {return unstretch(rssi);}
/** get the access-point's index */
int getAPIdx() const {return idx;}
private:
/** [-40:-103] -> [0:252] => this allows 0.25 steps */
static inline int8_t stretch(float rssi) {
if (rssi > -40) {rssi = -40;}
if (rssi < -103) {rssi = -103;}
return std::round((-rssi - 40) * 4);
}
/** [0:252] -> [-40:-103] */
static inline float unstretch(const float rssi) {
return -(rssi / 4.0f + 40.0f);
}
};// __attribute__((packed));
/**
* base-class for grid-nodes that contain wifi signal-strength estimations.
* such estimations are calculated offline and added to each node.
* this also allows for using very complex signal-strength estimation models
*/
template <int maxAccessPoints> struct WiFiGridNode {
/** contains the 10 strongest APs measureable at this position */
WiFiGridNodeAP strongestAPs[maxAccessPoints];
/** ctor */
WiFiGridNode() {;}
/** shared vector of all accesspoints the are present within the map. the IDX referes to this vector */
static std::vector<AccessPoint>& getMapAPs() {
static std::vector<AccessPoint> list;
return list;
}
/** get the maximum number of APs each node is able to store */
int getMaxAPs() const {return maxAccessPoints;}
/**
* get the RSSI for the given AP
* returns 0 if unknown
*/
float getRSSI(const std::string& mac) const {
return getRSSI(MACAddress(mac));
}
/**
* get the RSSI for the given AP
* returns 0 if unknown
*/
float getRSSI(const MACAddress mac) const {
for (const WiFiGridNodeAP ap : strongestAPs) {
if (!ap.isValid()) {break;} // reached the end
if (getMapAPs()[ap.getAPIdx()].mac == mac) {return ap.getRSSI();}
}
return 0;
}
/** get the number of access-points visible at this node */
int getNumVisibleAPs() const {
for (int i = 0; i < getMaxAPs(); ++i) {
if (!strongestAPs[i].isValid()) {return i;}
}
return getMaxAPs();
}
};// __attribute__((packed));
#endif // WIFIGRIDNODE_H

View File

@@ -0,0 +1,100 @@
#ifndef WIFIOBSERVATION_H
#define WIFIOBSERVATION_H
#include <vector>
#include <fstream>
#include "../MACAddress.h"
#include "../../math/Distributions.h"
/** observed one AP with the given signal strength */
struct WiFiObservationEntry {
/** AP's MAC address */
MACAddress mac;
/** AP's RSSI */
float rssi;
/** ctor */
WiFiObservationEntry(const MACAddress& mac, const float rssi) : mac(mac), rssi(rssi) {;}
};
/** all observed APs and their signal strength */
struct WiFiObservation {
/** one entry per AP */
std::vector<WiFiObservationEntry> entries;
};
class WiFiObserver {
private:
float sigma = 8.0f;
public:
/** ctor */
WiFiObserver(const float sigma) : sigma(sigma) {
;
}
/**
* get the given GridNode's probability based on the provided WiFi measurements
*/
template <typename Node> double getProbability(const Node& n, const WiFiObservation& obs, const int age_ms = 0) {
double prob = 0;
// sigma grows with measurement age
const double sigma = this->sigma * (1 + age_ms / 500.0f);
// process each observed AP
for (const WiFiObservationEntry& ap : obs.entries) {
// the RSSI from the scan
const float measuredRSSI = ap.rssi;
// the RSSI from the model (if available!)
const float modelRSSI = n.getRSSI(ap.mac);
// no model RSSI available?
if (modelRSSI == 0) {continue;}
// compare both
const double p = Distribution::Normal<double>::getProbability(measuredRSSI, sigma, modelRSSI);
// adjust using log
prob += std::log(p);
}
//return std::pow(std::exp(prob), 0.1);
return std::exp(prob);
}
/** gnuplot debug dump */
template <typename Node> void dump(Grid<Node>& grid, const WiFiObservation& obs, const std::string& fileName) {
std::ofstream out(fileName);
out << "splot '-' with points palette\n";
for (const Node& n : grid) {
const float p = getProbability(n, obs);
out << n.x_cm << " " << n.y_cm << " " << n.z_cm << " " << p << "\n";
}
out << "e\n";
out.close();
}
};
#endif // WIFIOBSERVATION_H

View File

@@ -13,9 +13,9 @@ public:
}
/** convert from a distance (in meter) to the expected RSSI */
static float distanceToRssi(const float txPower, const float pathLoss, const float distance) {
if (distance <= 1) {return txPower;}
return (txPower - (10 * pathLoss * std::log10(distance)));
static float distanceToRssi(const float txPower, const float pathLoss, const float distance_m) {
if (distance_m <= 1) {return txPower;}
return (txPower - (10 * pathLoss * std::log10(distance_m)));
}
};

View File

@@ -0,0 +1,18 @@
#ifndef WIFIMODEL_H
#define WIFIMODEL_H
#include "../LocatedAccessPoint.h"
/**
* interface for signal-strength prediction models
*/
class WiFiModel {
public:
/** get the given access-point's RSSI at the provided location */
virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
};
#endif // WIFIMODEL_H

View File

@@ -0,0 +1,31 @@
#ifndef WIFIMODELLOGDIST_H
#define WIFIMODELLOGDIST_H
#include "WiFiModel.h"
#include "LogDistanceModel.h"
/**
* signal-strength estimation using log-distance model
*/
class WiFiModelLogDist : public WiFiModel {
private:
float txp;
float exp;
public:
/** ctor */
WiFiModelLogDist(const float txp, const float exp) : txp(txp), exp(exp) {
;
}
/** get the given access-point's RSSI at the provided location */
float getRSSI(const LocatedAccessPoint& ap, const Point3 p) override {
return LogDistanceModel::distanceToRssi(txp, exp, ap.getDistance(p));
}
};
#endif // WIFIMODELLOGDIST_H

View File

@@ -1,4 +1,37 @@
#ifndef TESTAPI_CPP
#define TESTAPI_CPP
#ifdef WITH_TESTS
#endif // TESTAPI_CPP
#include "../Tests.h"
#include "../../api/api.h"
#include "../../api/DummyAPI.h"
class LeListener : public APIListener {
virtual void onPathUpdate(const std::vector<Point3>& curPath) override {
}
virtual void onPositionUpdate(const Point3 curPos) override {
//std::cout << curPos.x << "," << curPos.y << "," << curPos.z << std::endl;
}
};
TEST(API, run) {
std::string folder = "/mnt/data/workspaces/IndoorMap/maps";
DummyAPI api(folder);
api.setTarget(1);
api.addListener(new LeListener());
api.startNavigation();
sleep(5);
api.stopNavigation();
}
#endif

View File

@@ -23,7 +23,7 @@ TEST(TestAll, Nav) {
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2.5);
float d = (n1.inCentimeter() - n2.inCentimeter()).length(2.5);
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
}

View File

@@ -12,6 +12,7 @@
#include "../../grid/walk/GridWalkWeighted.h"
#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"
//TEST(Walk, plot) {
TEST(Walk, DISABLED_plot) {
@@ -59,7 +60,7 @@ TEST(Walk, DISABLED_plot) {
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2.0);
float d = (n1.inCentimeter() - n2.inCentimeter()).length(2.0);
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
}

View File

@@ -29,16 +29,16 @@ TEST(AStar, build) {
// const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
const std::vector<GP>& getAllNodes() const {return grid.getNodes();}
decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);}
float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
float getWeightBetween(const GP& n1, const GP& n2) const {return (n1.inCentimeter() - n2.inCentimeter()).length();}
float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
} tmp(grid);
AStar<GP> nav;
std::vector<const GP*> vec = nav.get(&grid[0], &grid[4], tmp);
// std::vector<const GP*> vec = nav.get(grid[0], grid[4], tmp);
for (const GP* g : vec) {
std::cout << g << std::endl;
}
// for (const GP* g : vec) {
// std::cout << g << std::endl;
// }
}

View File

@@ -27,7 +27,7 @@ TEST(Dijkstra, build) {
TMP(Grid<GP>& grid) : grid(grid) {;}
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
float getWeightBetween(const GP& n1, const GP& n2) const {return (n1.inCentimeter() - n2.inCentimeter()).length();}
} tmp(grid);
Dijkstra<GP> d;
@@ -94,7 +94,7 @@ void dijkstra(Grid<GP>& grid) {
TMP(Grid<GP>& grid) : grid(grid) {;}
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
float getWeightBetween(const GP& n1, const GP& n2) const {return (n1.inCentimeter() - n2.inCentimeter()).length();}
} tmp(grid);
Dijkstra<GP> d;

View File

@@ -2,7 +2,7 @@
#include "../../Tests.h"
#include "../../../sensors/radio/LogDistanceModel.h"
#include "../../../sensors/radio/model/LogDistanceModel.h"
TEST(LogDistanceModel, calc) {

View File

@@ -0,0 +1,96 @@
#ifdef WITH_TESTS
#include "../../Tests.h"
#include "../../../sensors/radio/WiFiGridEstimator.h"
#include "../../../sensors/radio/model/WiFiModelLogDist.h"
#include "../../../sensors/radio/WiFiObservation.h"
#include "../../../grid/Grid.h"
struct MyNode : GridNode, GridPoint, WiFiGridNode<10> {
MyNode(const int x_cm, const int y_cm, const int z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};
/** test the RSSI storage. [we use only a single byte due to memory constraints, but allow 0.25 dB steps!] */
TEST(WiFiGridNodeAP, rssi) {
for (float rssi = -40; rssi > -103; rssi -= 0.1f) {
WiFiGridNodeAP ap(0, rssi);
ASSERT_NEAR(rssi, ap.getRSSI(), 0.2f); // max 0.2 dB variation from what it should be
}
}
/** test the signal-strength limit: [-40: -103] */
TEST(WiFiGridNodeAP, rssiLimits) {
// limit 1
WiFiGridNodeAP ap1(0, -10);
ASSERT_EQ(-40, ap1.getRSSI());
// limit 2
WiFiGridNodeAP ap2(0, -200);
ASSERT_EQ(-103, ap2.getRSSI());
}
TEST(WiFiGridModelLogDist, create) {
int gs = 20;
Grid<MyNode> grid(gs);
for (int x = 0; x < 2000; x += gs) {
for (int y = 0; y < 2000; y += gs) {
grid.add(MyNode(x,y,0));
}
}
std::cout << "GridNodeSize: " << sizeof(MyNode) << std::endl;
LocatedAccessPoint ap1("00:00:00:00:00:01", Point3( 0, 0,0));
LocatedAccessPoint ap2("00:00:00:00:00:02", Point3(20, 0,0));
LocatedAccessPoint ap3("00:00:00:00:00:03", Point3( 0,20,0));
LocatedAccessPoint ap4("00:00:00:00:00:04", Point3(20,20,0));
std::vector<LocatedAccessPoint> aps = {ap1, ap2, ap3, ap4};
ASSERT_EQ(0, grid[0].getNumVisibleAPs());
WiFiModelLogDist model(-40, 1.5);
WiFiGridEstimator::estimate(grid, model, aps);
ASSERT_EQ(4, grid[0].getNumVisibleAPs()); // 4 APs visible at this node
ASSERT_GT(0, grid[0].getRSSI("00:00:00:00:00:01")); // non-zero RSSI
ASSERT_GT(0, grid[0].getRSSI("00:00:00:00:00:02")); // non-zero RSSI
ASSERT_GT(0, grid[0].getRSSI("00:00:00:00:00:03")); // non-zero RSSI
ASSERT_GT(0, grid[0].getRSSI("00:00:00:00:00:04")); // non-zero RSSI
ASSERT_EQ(0, grid[0].getRSSI("00:00:00:00:00:05")); // unknown AP -> 0 RSSI
WiFiGridEstimator::dump(grid, "00:00:00:00:00:01", "/tmp/ap1.gp");
WiFiGridEstimator::dump(grid, "00:00:00:00:00:02", "/tmp/ap2.gp");
WiFiGridEstimator::dump(grid, "00:00:00:00:00:03", "/tmp/ap3.gp");
WiFiGridEstimator::dump(grid, "00:00:00:00:00:04", "/tmp/ap4.gp");
WiFiObservation obs;
obs.entries.push_back(WiFiObservationEntry(MACAddress("00:00:00:00:00:01"), -55));
obs.entries.push_back(WiFiObservationEntry(MACAddress("00:00:00:00:00:02"), -55));
obs.entries.push_back(WiFiObservationEntry(MACAddress("00:00:00:00:00:03"), -55));
obs.entries.push_back(WiFiObservationEntry(MACAddress("00:00:00:00:00:04"), -55));
WiFiObserver observer(5.0f);
const MyNode& gn = grid.getNodeFor(GridPoint(1000,1000,0));
const float p = observer.getProbability(gn, obs, 0);
observer.dump(grid, obs, "/tmp/eval1.gp");
int i = 0; (void) i;
}
#endif

4
wifi/WiFiAP.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef WIFIAP_H
#define WIFIAP_H
#endif // WIFIAP_H

4
wifi/WiFiGridNode.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef WIFIGRIDNODE_H
#define WIFIGRIDNODE_H
#endif // WIFIGRIDNODE_H

4
wifi/model/LogDistance.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef LOGDISTANCE_H
#define LOGDISTANCE_H
#endif // LOGDISTANCE_H