huge commit

- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases

tests/sensors/imu/TestTurnDetection.cpp

@@ -0,0 +1,71 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/imu/TurnDetection.h"
+
+TEST(TurnDetection, rotationMatrix) {
+
+	Eigen::Vector3f dst; dst << 0, 0, 1;
+	Eigen::Vector3f src; src << 1, 1, 0; src.normalize();
+
+	// get a matrix that rotates "src" into "dst"
+	Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
+
+	Eigen::Vector3f res = rot * src;
+
+	ASSERT_NEAR(dst(0), res(0), 0.01);
+	ASSERT_NEAR(dst(1), res(1), 0.01);
+	ASSERT_NEAR(dst(2), res(2), 0.01);
+
+}
+
+TEST(TurnDetection, gyroRotate) {
+
+
+	Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+	Eigen::Vector3f acc; acc << 0, 7.0, 7.0;
+
+	Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis);
+
+	Eigen::Vector3f gyro; gyro << 0, 60, 60;
+
+	Eigen::Vector3f gyro2; gyro2 << 0, 0, 84;
+
+	Eigen::Vector3f gyro3 = rot * gyro;
+
+	ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
+
+}
+
+
+TEST(TurnDetection, xx) {
+
+	Eigen::Vector3f dst; dst << 0, 0, 1;
+	Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize();		// sample accelerometer readings
+
+	Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
+
+//	Eigen::Vector3f x; x << 1, 0, 0;
+//	Eigen::Vector3f z = src.normalized();
+//	Eigen::Vector3f y = z.cross(x);
+
+//	Eigen::Matrix3f rot;
+//	rot.row(0) = x;
+//	rot.row(1) = y;
+//	rot.row(2) = z;
+
+	Eigen::Vector3f res = rot * src;
+//	ASSERT_NEAR(dst(0), res(0), 0.01);
+//	ASSERT_NEAR(dst(1), res(1), 0.01);
+//	ASSERT_NEAR(dst(2), res(2), 0.01);
+
+	Eigen::Vector3f gyro; gyro << 0, 10, 30;
+
+	Eigen::Vector3f gyro2 = rot * gyro;
+	int i = 0; (void) i;
+
+}
+
+
+#endif

tests/sensors/offline/TestOffline.cpp

@@ -0,0 +1,28 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/offline/OfflineAndroid.h"
+
+TEST(Offline, test1) {
+	OfflineAndroid off(getDataFile("offlineAndroid.dat"));
+	ASSERT_EQ(6, off.getGroundTruth().size());
+	ASSERT_EQ(1, off.getWiFi().size());
+}
+
+// cleanup pattern
+// ^[0-9]*;(-1|0|1|2|3|4|5|6|7|9|10|11);.*
+TEST(Offline, test2) {
+	OfflineAndroid off(getDataFile("path4wifi.dat"));
+	ASSERT_EQ(20, off.getGroundTruth().size());
+}
+
+
+TEST(Offline, testPath) {
+	OfflineAndroid off(getDataFile("train/walks/path4_nexus_backward.dat"));
+	ASSERT_EQ(21, off.getGroundTruth().size());
+	ASSERT_EQ(off.getWalkedPath().pos_cm[0], off.getWalkedPath().pos_cm[1]);	// same waypoint twice. not moving within first few seconds
+	off.getWalkedPathInterpolatorCM();
+}
+
+#endif

tests/sensors/pressure/TestBarometer.cpp

@@ -0,0 +1,125 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/pressure/RelativePressure.h"
+#include "../../../sensors/pressure/PressureTendence.h"
+
+#include <random>
+
+TEST(Barometer, relCalib) {
+
+	RelativePressure baro;
+	baro.setCalibrationTimeframe(Timestamp::fromMS(3000));
+
+	ASSERT_EQ(0, baro.getBaseAvg());
+	ASSERT_EQ(0, baro.getSigma());
+
+	std::minstd_rand gen;
+	std::normal_distribution<float> dist(930, 0.15);
+
+	for (int i = 0; i <= 4000; i += 25) {
+		const Timestamp ts = Timestamp::fromMS(i);
+		baro.add(ts, BarometerData(dist(gen)));
+
+
+		if (i < 3000) {	// not yet calibrated
+			ASSERT_EQ(0, baro.getBaseAvg());
+			ASSERT_EQ(0, baro.getSigma());
+			ASSERT_EQ(0, baro.getPressureRealtiveToStart());
+		} else {		// calibrated
+			ASSERT_NEAR(930, baro.getBaseAvg(), 0.1);
+			ASSERT_NEAR(0.15, baro.getSigma(), 0.02);
+		}
+
+	}
+
+	// calibration OK?
+	ASSERT_NEAR(930, baro.getBaseAvg(), 0.1);
+	ASSERT_NEAR(0.15, baro.getSigma(), 0.02);
+
+	// get a HUGE value realtive to the calibration
+	baro.add(Timestamp::fromMS(9000), BarometerData(2000));
+	ASSERT_NEAR(2000-930, baro.getPressureRealtiveToStart(), 0.1);
+
+	// has no impact on calibration
+	ASSERT_NEAR(930, baro.getBaseAvg(), 0.1);
+	ASSERT_NEAR(0.15, baro.getSigma(), 0.02);
+
+	baro.reset();
+
+	// everything back to zero
+	ASSERT_EQ(0, baro.getBaseAvg());
+	ASSERT_EQ(0, baro.getSigma());
+	ASSERT_EQ(0, baro.getPressureRealtiveToStart());
+
+}
+
+#include <fstream>
+
+TEST(Barometer, tendence) {
+
+	std::ifstream inp ("/tmp/baro1.dat");
+
+	const float timeframe = 5.0;
+	PressureTendence tend(Timestamp::fromSec(timeframe));
+
+	while (inp.is_open() && !inp.eof()) {
+
+		int msec;
+		float hPa;
+
+		inp >> msec;
+		inp >> hPa;
+
+		tend.add(Timestamp::fromMS(msec), BarometerData(hPa));
+		tend.get();
+
+	}
+
+	sleep(1000);
+
+}
+
+TEST(Barometer, tendence2) {
+
+
+	const float timeframe = 5.0;
+	PressureTendence tend(Timestamp::fromSec(timeframe));
+
+	ASSERT_EQ(0, tend.get());
+
+	const float noiseSigma = 0.04;		// typical barometer noise
+	const float start = 930;			// start at 930 hPa
+	const float incPerSec = 0.100;		// and incremrent the pressure by 0.1 hPa per second
+	float curhPa = start;
+
+	// add zero-mean noise
+	std::minstd_rand gen;
+	std::normal_distribution<float> dist(0, noiseSigma);
+
+	// 10 Hz for 30 seconds
+	for (int i = 0; i < 30000; i += 100) {
+
+		// next pressure (add increment)
+		curhPa += sin(i/1200.0f) * 0.02;//incPerSec * 100 / 1000;
+
+		// add noise
+		float curhPaNoised = curhPa + dist(gen) + sin(i/200.0f)*0.1;
+
+		// calculate tendence
+		tend.add(Timestamp::fromMS(i), BarometerData(curhPaNoised));
+		std::cout << "w/o noise: " << curhPa << "\twith noise: " << curhPaNoised << "\ttendence/sec:" << (tend.get()/timeframe) << std::endl;
+
+	}
+
+	sleep(1000);
+
+
+	// tendence must be clear and smaller than the sigma
+	ASSERT_NEAR(incPerSec * timeframe, tend.get(), noiseSigma);
+
+}
+
+
+#endif

tests/sensors/radio/TestLogDistanceCeilingModel.cpp

@@ -0,0 +1,39 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/radio/model/WiFiModelLogDistCeiling.h"
+
+TEST(LogDistanceCeilingModel, calc) {
+
+	Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
+	Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
+	Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
+
+	Floorplan::IndoorMap map;
+	map.floors.push_back(f0);
+	map.floors.push_back(f1);
+	map.floors.push_back(f2);
+
+	LocatedAccessPoint ap0("00:00:00:00:00:00", Point3(0,0,0));
+	LocatedAccessPoint ap25("00:00:00:00:00:00", Point3(0,0,2.5));
+
+	WiFiModelLogDistCeiling model(-40, 1.0, -8.0, &map);
+
+
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(1,0,0)));
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,1,0)));
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,0,1)));
+
+	ASSERT_EQ(-40, model.getRSSI(ap25, Point3(1,0,2.5)));
+	ASSERT_EQ(-40, model.getRSSI(ap25, Point3(0,1,2.5)));
+	ASSERT_EQ(-40-8, model.getRSSI(ap25, Point3(0,0,3.5)));		// one floor within
+
+	ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,8,0)));
+	ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,0,8))+8+8);		// two ceilings within
+
+}
+
+
+
+#endif

tests/sensors/radio/TestVAPGrouper.cpp

@@ -0,0 +1,96 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/radio/VAPGrouper.h"
+
+/** test the RSSI storage. [we use only a single byte due to memory constraints, but allow 0.25 dB steps!] */
+TEST(WiFiVAPGrouper, baseMAC) {
+
+	VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+
+	// first VAP group
+	const MACAddress ap1B("aa:bb:cc:dd:ee:f0");		// base-mac (last digit zero)
+	const MACAddress ap10("aa:bb:cc:dd:ee:f3");		// vap 1
+	const MACAddress ap11("aa:bb:cc:dd:ee:f7");		// vap 2
+	const MACAddress ap12("aa:bb:cc:dd:ee:ff");		// vap 3
+
+	// second VAP group
+	const MACAddress ap2B("ff:ee:dd:cc:bb:a0");		// base-mac (last digit zero)
+	const MACAddress ap20("ff:ee:dd:cc:bb:a9");		// vap 1
+	const MACAddress ap21("ff:ee:dd:cc:bb:a8");		// vap 2
+	const MACAddress ap22("ff:ee:dd:cc:bb:ae");		// vap 3
+
+	// all in first group must equal
+	ASSERT_EQ( ap1B, vg.getBaseMAC(ap10) );
+	ASSERT_EQ( vg.getBaseMAC(ap10), vg.getBaseMAC(ap11) );
+	ASSERT_EQ( vg.getBaseMAC(ap11), vg.getBaseMAC(ap12) );
+
+	// all in second group must equal
+	ASSERT_EQ( ap2B, vg.getBaseMAC(ap20) );
+	ASSERT_EQ( vg.getBaseMAC(ap20), vg.getBaseMAC(ap21) );
+	ASSERT_EQ( vg.getBaseMAC(ap21), vg.getBaseMAC(ap22) );
+
+	// first and second must be different
+	ASSERT_NE( vg.getBaseMAC(ap10), vg.getBaseMAC(ap20) );
+
+}
+
+TEST(WiFiVAPGrouper, aggregation) {
+
+	VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+	VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN);
+	VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
+
+	WiFiMeasurements scan;
+
+	const AccessPoint vap10("aa:bb:cc:dd:ee:f3");
+	const AccessPoint vap11("aa:bb:cc:dd:ee:f5");
+	const AccessPoint vap12("aa:bb:cc:dd:ee:fe");
+
+	const AccessPoint vap20("01:bb:cc:dd:11:a3");
+	const AccessPoint vap21("01:bb:cc:dd:11:a5");
+	const AccessPoint vap22("01:bb:cc:dd:11:ae");
+	const AccessPoint vap23("01:bb:cc:dd:11:ae");
+
+	scan.entries.push_back(WiFiMeasurement(vap10, -75, Timestamp::fromMS(11)));
+	scan.entries.push_back(WiFiMeasurement(vap11, -70, Timestamp::fromMS(12)));
+	scan.entries.push_back(WiFiMeasurement(vap12, -71, Timestamp::fromMS(13)));
+
+	scan.entries.push_back(WiFiMeasurement(vap20, -69, Timestamp::fromMS(22)));
+	scan.entries.push_back(WiFiMeasurement(vap21, -61, Timestamp::fromMS(25)));
+	scan.entries.push_back(WiFiMeasurement(vap22, -62, Timestamp::fromMS(23)));
+	scan.entries.push_back(WiFiMeasurement(vap23, -60, Timestamp::fromMS(20)));
+
+	const WiFiMeasurements gAvg = vgAvg.group(scan);
+	const WiFiMeasurements gMedian = vgMedian.group(scan);
+	const WiFiMeasurements gMax = vgMax.group(scan);
+
+	// correct number of grouped entries?
+	ASSERT_EQ(2, gAvg.entries.size());
+	ASSERT_EQ(2, gMedian.entries.size());
+	ASSERT_EQ(2, gMax.entries.size());
+
+	// correct average values?
+	ASSERT_EQ(-72, gAvg.entries.back().getRSSI());
+	ASSERT_EQ(-63, gAvg.entries.front().getRSSI());
+	ASSERT_EQ(Timestamp::fromMS(11), gAvg.entries.back().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(22), gAvg.entries.front().getTimestamp());
+
+	// correct median values?
+	ASSERT_EQ(-71, gMedian.entries.back().getRSSI());
+	ASSERT_EQ(-61.5, gMedian.entries.front().getRSSI());
+	ASSERT_EQ(Timestamp::fromMS(11), gMedian.entries.back().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(22), gMedian.entries.front().getTimestamp());
+
+	// correct max values?
+	ASSERT_EQ(-70, gMax.entries.back().getRSSI());
+	ASSERT_EQ(-60, gMax.entries.front().getRSSI());
+	ASSERT_EQ(Timestamp::fromMS(11), gMax.entries.back().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(22), gMax.entries.front().getTimestamp());
+
+
+}
+
+
+#endif

tests/sensors/radio/TestWiFiEval.cpp

@@ -0,0 +1,427 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+#include "../../../sensors/offline/OfflineAndroid.h"
+#include "../../../sensors/radio/WiFiProbabilityFree.h"
+#include "../../../sensors/radio/model/WiFiModelLogDist.h"
+#include "../../../sensors/radio/model/WiFiModelLogDistCeiling.h"
+#include "../../../sensors/radio/VAPGrouper.h"
+
+#include "../../../geo/Point3.h"
+#include "../../../math/Interpolator.h"
+
+#include "../../../floorplan/v2/FloorplanReader.h"
+
+#include <KLib/math/statistics/Statistics.h>
+#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
+
+std::vector<std::string> getTrainingFiles() {
+
+	// all training files
+	return {
+
+//		"train/walks/path4_galaxy_forward.dat",
+//		"train/walks/path4_galaxy_backward.dat",
+
+//		"train/walks/path3_galaxy_forward.dat",
+//		"train/walks/path3_galaxy_backward.dat",
+
+//		"train/walks/path2_galaxy_forward.dat",
+//		"train/walks/path2_galaxy_backward.dat",
+
+//		"train/walks/path1_galaxy_forward.dat",
+//		"train/walks/path1_galaxy_backward.dat",
+
+		"train/walks/path4_nexus_forward.dat",
+		"train/walks/path4_nexus_backward.dat",
+
+		"train/walks/path3_nexus_forward.dat",
+		"train/walks/path3_nexus_backward.dat",
+
+		"train/walks/path2_nexus_forward.dat",
+		"train/walks/path2_nexus_backward.dat",
+
+		"train/walks/path1_nexus_forward.dat",
+		"train/walks/path1_nexus_backward.dat",
+
+	};
+
+}
+
+struct APParams {
+	float txp;
+	float exp;
+	float waf;
+	float offset;
+};
+
+/*
+WiFiObservation groupVAP(const WiFiObservation& inp) {
+	struct Debug {
+		std::string mac;
+		float rssi;
+		Debug(const std::string& mac, const float rssi) : mac(mac), rssi(rssi) {;}
+	};
+
+	struct Params{
+		float rssiSum = 0;
+		int cnt = 0;
+		std::vector<Debug> single;
+	};
+	std::unordered_map<MACAddress, Params> map;
+	WiFiObservation out;
+	for (const WiFiObservationEntry& e : inp.entries) {
+		std::string mac = e.mac.asString();
+		mac[mac.size()-1] = '9';
+		map[mac].rssiSum += e.rssi;
+		map[mac].cnt += 1;
+		map[mac].single.push_back(Debug(e.mac.asString(), e.rssi));
+	}
+	for (auto it : map) {
+		const Params& params = it.second;
+		WiFiObservationEntry entry(it.first, params.rssiSum / params.cnt);
+		out.entries.push_back(entry);
+	}
+	return out;
+}
+*/
+
+struct WiFiEvalTestBase {
+
+	// all training files
+	std::vector<std::string> files = getTrainingFiles();
+
+	// parser for each file
+	std::vector<OfflineAndroid> parsers;
+
+
+	// load the floorplan
+	Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile(getDataFile("SHL.xml"));
+
+	// all access points within the floorplan
+	std::vector<LocatedAccessPoint> aps;
+	std::unordered_map<MACAddress, LocatedAccessPoint> apsMap;
+
+	WiFiEvalTestBase() {
+
+		for (Floorplan::Floor* f : im->floors) {
+			for (Floorplan::AccessPoint* ap : f->accesspoints) {
+
+//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:A4:A9") {continue;}
+				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:77:EB:99") {continue;}
+				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:82:79") {continue;}
+//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:6E:F9") {continue;}
+
+//				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:46:09") {continue;}
+				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:64:99") {continue;}
+				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:3A:A9") {continue;}
+				if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:DB:69") {continue;}
+
+
+
+
+				aps.push_back(LocatedAccessPoint(*ap));
+				apsMap.insert(std::pair<MACAddress, LocatedAccessPoint>(ap->mac, LocatedAccessPoint(*ap)));
+			}
+		}
+
+		for (const std::string& file : files) {
+			parsers.push_back(OfflineAndroid(getDataFile(file)));
+		}
+
+	}
+
+	double getError(APParams params) {
+
+		if (params.waf > 0) {return 99999;}
+
+//		params.txp = -40;
+//		params.offset = -6;
+
+		double dB_err = 0;
+		int cnt = 0;
+
+		// model
+		WiFiModelLogDistCeiling model(params.txp, params.exp, params.waf, im);
+
+		VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+
+		// evaluate each training file
+		for (const OfflineAndroid& parser : parsers) {
+
+			// ground-truth interpolator
+			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();
+
+			// evaluate each WiFi reading
+			for (const OfflineEntry<WiFiMeasurements>& _obs : parser.getWiFi()) {
+
+				WiFiMeasurements obs = vg.group(_obs.data);
+
+				// ground-truth at the time of measurement
+				const Point3 pos = interpol.get(_obs.ts) / 100;
+
+				// dB diff
+				for (const WiFiMeasurement& wifiObs : obs.entries) {
+
+					// only use configured APs
+					auto it = apsMap.find(wifiObs.getAP().getMAC());
+					if (it == apsMap.end()) {continue;}
+
+					// get model RSSI
+					const float mdlRSSI = model.getRSSI(it->second, pos) + params.offset;
+
+					// difference to scan RSSI
+					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());
+
+					//stats[wifiObs.mac].add(diff);
+
+					dB_err += diff;
+					++cnt;
+
+				}
+
+			}
+
+		}
+
+		return dB_err / cnt;
+
+	}
+
+
+	std::unordered_map<MACAddress, K::Statistics<float>> getStats(APParams params) {
+
+		std::unordered_map<MACAddress, K::Statistics<float>> stats;
+
+		// model
+		WiFiModelLogDistCeiling model(params.txp, params.exp, params.waf, im);
+
+		// evaluate each training file
+		for (const OfflineAndroid& parser : parsers) {
+
+			// ground-truth interpolator
+			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();
+
+			// evaluate each WiFi reading
+			for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {
+
+				// ground-truth at the time of measurement
+				const Point3 pos = interpol.get(obs.ts) / 100;
+
+				// dB diff
+				for (const WiFiMeasurement& wifiObs : obs.data.entries) {
+
+					// only use configured APs
+					auto it = apsMap.find(wifiObs.getAP().getMAC());
+					if (it == apsMap.end()) {continue;}
+
+					// get model RSSI
+					const float mdlRSSI = model.getRSSI(it->second, pos) + params.offset;
+
+					// difference to scan RSSI
+					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());
+
+					stats[wifiObs.getAP().getMAC()].add(diff);
+
+				}
+
+			}
+
+		}
+
+		return stats;
+
+	}
+
+};
+
+
+
+TEST(WiFiEval, VAP) {
+
+	WiFiEvalTestBase base;
+
+	float errSum = 0;
+	int cnt = 0;
+
+	VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+
+	// evaluate each training file
+	for (const OfflineAndroid& parser : base.parsers) {
+
+		// evaluate each WiFi reading
+		for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {
+
+			WiFiMeasurements obsUngrouped = obs.data;
+			WiFiMeasurements obsGrouped = vg.group(obsUngrouped);
+
+			struct Comp {
+				float rssiUngrouped = 0;
+				float rssiGrouped = 0;
+			};
+
+			std::unordered_map<MACAddress, Comp> map;
+
+			for (const WiFiMeasurement& entryUngrouped : obsUngrouped.entries) {
+				map[entryUngrouped.getAP().getMAC()].rssiUngrouped = entryUngrouped.getRSSI();
+			}
+
+			for (const WiFiMeasurement& entryGrouped : obsGrouped.entries) {
+				map[entryGrouped.getAP().getMAC()].rssiGrouped = entryGrouped.getRSSI();
+			}
+
+			for (auto it : map) {
+				Comp c = it.second;
+				if (c.rssiUngrouped != 0 && c.rssiGrouped != 0) {
+					const float diff = std::abs(c.rssiGrouped - c.rssiUngrouped);
+					errSum += diff;
+					++cnt;
+				}
+			}
+
+		}
+
+	}
+
+	std::cout << "average diff is: " << (errSum/cnt) << std::endl;
+
+}
+
+TEST(WiFiEval, optimize) {
+
+
+	WiFiEvalTestBase base;
+
+	APParams params;
+	params.exp = 2;
+	params.txp = -40;
+	params.waf = -4;
+	params.offset = -4;
+
+	auto func = [&] (const float* data) {
+		APParams* params = (APParams*) data;
+		return base.getError(*params);
+	};
+
+	K::NumOptAlgoDownhillSimplex<float, 4> opt;
+	opt.setMaxIterations(100);
+	opt.setNumRestarts(3);
+	opt.calculateOptimum(func, (float*) &params);
+
+	std::cout << params.exp << ":" << params.txp << ":" << params.waf << ":" << params.offset << std::endl;
+	std::cout << base.getError(params) << std::endl;
+
+	for (auto it : base.getStats(params)) {
+		std::cout << it.first.asString() << ": " << it.second.asString() << std::endl;
+	}
+
+}
+
+
+TEST(aaiFiEval, getBestTXP_EXP) {
+
+	// load the floorplan
+	Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile(getDataFile("SHL.xml"));
+
+	// all access points within the floorplan
+	std::vector<LocatedAccessPoint> aps;
+	std::unordered_map<MACAddress, LocatedAccessPoint> apsMap;
+	for (Floorplan::Floor* f : im->floors) {
+		for (Floorplan::AccessPoint* ap : f->accesspoints) {
+
+//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6C:A4:A9") {continue;}
+//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:64:99") {continue;}
+//			if (Floorplan::toUpperCase(ap->mac) == "00:04:96:6B:82:79") {continue;}
+
+			aps.push_back(LocatedAccessPoint(*ap));
+			apsMap.insert(std::pair<MACAddress, LocatedAccessPoint>(ap->mac, LocatedAccessPoint(*ap)));
+
+		}
+	}
+
+	std::vector<std::string> files = getTrainingFiles();
+
+
+	// parser each file
+	std::vector<OfflineAndroid> parsers;
+	for (const std::string& file : files) {
+		parsers.push_back(OfflineAndroid(getDataFile(file)));
+	}
+
+	std::unordered_map<MACAddress, K::Statistics<float>> stats;
+
+	const float sigma = 8;
+	const float waf = -4;
+	const float txp = -40;
+
+	// eval using different parameters
+	for (float exp = 1.5; exp < 3.5; exp += 0.05) {
+	//){ float exp = 2.5;
+
+		double err = 0;
+		double sum = 0;
+		int cnt = 0;
+		int apCnt = 0;
+
+		// model
+		//WiFiModelLogDist model(-40, exp);
+		WiFiModelLogDistCeiling model(txp, exp, waf, im);
+		WiFiObserverFree observer(sigma, model, aps);
+
+		// evaluate each training file
+		for (const OfflineAndroid& parser : parsers) {
+
+			// ground-truth interpolator
+			Interpolator<Timestamp, Point3> interpol = parser.getWalkedPathInterpolatorCM();
+
+			// evaluate each WiFi reading
+			for (const OfflineEntry<WiFiMeasurements>& obs : parser.getWiFi()) {
+
+				// ground-truth at the time of measurement
+				const Point3 pos = interpol.get(obs.ts) / 100;
+
+				// probability
+				sum += std::log(observer.getProbability(pos, obs.ts, obs.data, 0));
+				++cnt;
+
+				// dB diff
+				for (const WiFiMeasurement& wifiObs : obs.data.entries) {
+
+					auto it = apsMap.find(wifiObs.getAP().getMAC());
+					if (it == apsMap.end()) {continue;}
+
+					const float mdlRSSI = model.getRSSI(it->second, pos);
+					const float diff = std::abs(mdlRSSI - wifiObs.getRSSI());
+
+					//stats[wifiObs.mac].add(diff);
+
+					err += diff;
+					++apCnt;
+
+				}
+
+			}
+
+		}
+
+		std::cout << exp << ":" << (sum/cnt) << std::endl;
+		std::cout << exp << ":" << (err/apCnt) << std::endl;
+		std::cout << std::endl;
+
+
+
+		int i = 0; (void) i;
+
+	}
+
+//	for (auto it : stats) {
+//		std::cout << it.first.asString() << ": " << it.second.asString() << std::endl;
+//	}
+
+
+
+
+}
+
+
+#endif

tests/sensors/radio/TestWiFiGrid.cpp

@@ -1,9 +1,13 @@
 #ifdef WITH_TESTS
 
 #include "../../Tests.h"
-#include "../../../sensors/radio/WiFiGridEstimator.h"
 #include "../../../sensors/radio/model/WiFiModelLogDist.h"
-#include "../../../sensors/radio/WiFiObservation.h"
+
+#include "../../../sensors/radio/WiFiGridEstimator.h"
+#include "../../../sensors/radio/WiFiMeasurements.h"
+#include "../../../sensors/radio/WiFiProbability.h"
+#include "../../../sensors/radio/WiFiProbabilityFree.h"
+#include "../../../sensors/radio/WiFiProbabilityGrid.h"
 
 #include "../../../grid/Grid.h"
 
@@ -38,7 +42,7 @@ TEST(WiFiGridNodeAP, rssiLimits) {
 
 
 
-
+/** gnuplot debug dumps */
 TEST(WiFiGridModelLogDist, create) {
 
 	int gs = 20;
@@ -77,17 +81,19 @@ TEST(WiFiGridModelLogDist, create) {
 	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));
+	Timestamp ts = Timestamp::fromMS(10);
 
-	WiFiObserver observer(5.0f);
+	WiFiMeasurements obs;
+	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:01"), -55, ts));
+	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:02"), -55, ts));
+	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:03"), -55, ts));
+	obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:04"), -55, ts));
+
+	WiFiObserverGrid observer(5.0f);
 	const MyNode& gn = grid.getNodeFor(GridPoint(1000,1000,0));
-	const float p = observer.getProbability(gn, obs, 0);
+	const float p = observer.getProbability(gn, ts, obs);
 
-	observer.dump(grid, obs, "/tmp/eval1.gp");
+	observer.dump(grid, ts, obs, "/tmp/eval1.gp");
 
 	int i = 0; (void) i;