121 lines
3.7 KiB
C++
121 lines
3.7 KiB
C++
#include "Eval.h"
|
|
|
|
#include <array>
|
|
#include <vector>
|
|
|
|
#include "Settings.h"
|
|
|
|
#include <Indoor/math/distribution/Normal.h>
|
|
|
|
double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector<WiFiMeasurement>& measurements, std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters)
|
|
{
|
|
double result = 1.0;
|
|
|
|
std::array<bool, 4> hadMeas = {false};
|
|
|
|
for (WiFiMeasurement wifi : measurements)
|
|
{
|
|
if (wifi.getNumSuccessfulMeasurements() < 3)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
const MACAddress& mac = wifi.getAP().getMAC();
|
|
int nucIndex = Settings::nucIndex(mac);
|
|
|
|
const Point3 apPos = Settings::CurrentPath.nucInfo(nucIndex).position;
|
|
// particlePos.z = 1.3; // smartphone höhe
|
|
const float apDist = particlePos.getDistance(apPos);
|
|
|
|
// compute ftm distance
|
|
float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset;
|
|
float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset
|
|
|
|
float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss;
|
|
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
|
|
|
|
if (UseSensor == SensorMode::FTM)
|
|
{
|
|
if (ftmDist > 0)
|
|
{
|
|
//double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
|
|
double sigma = 8;
|
|
|
|
if (ftmKalmanFilters != nullptr)
|
|
{
|
|
Kalman& kalman = ftmKalmanFilters->at(mac);
|
|
ftmDist = kalman.predictAndUpdate(currentTime, ftmDist);
|
|
//sigma = std::sqrt(kalman.P(0, 0));
|
|
|
|
Assert::isTrue(sigma > 0, "sigma");
|
|
|
|
double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
|
|
|
|
result *= x;
|
|
}
|
|
else
|
|
{
|
|
double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
|
|
|
|
result *= x;
|
|
}
|
|
|
|
// hadMeas[nucIndex] = true; TODO
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// RSSI
|
|
double sigma = 8;
|
|
|
|
double x = Distribution::Normal<double>::getProbability(rssiDist, sigma, apDist);
|
|
result *= x;
|
|
}
|
|
}
|
|
|
|
// Use kalman to predict missing measurments
|
|
//if (UseSensor == SensorMode::FTM && ftmKalmanFilters != nullptr)
|
|
//{
|
|
// for (size_t i = 0; i < 4; i++)
|
|
// {
|
|
// if (!hadMeas[i])
|
|
// {
|
|
// double sigma = 5;
|
|
|
|
// Kalman& kalman = ftmKalmanFilters->at(Settings::nucFromIndex(i));
|
|
|
|
// if (!isnan(kalman.lastTimestamp))
|
|
// {
|
|
// KalmanPrediction prediction = kalman.predict(currentTime);
|
|
// sigma = std::sqrt(prediction.P[0]);
|
|
|
|
// sigma = sigma > 0 ? sigma : 5;
|
|
|
|
// const Point3 apPos = Settings::CurrentPath.nucInfo(i).position;
|
|
// // particlePos.z = 1.3; // smartphone höhe
|
|
// const float apDist = particlePos.getDistance(apPos);
|
|
|
|
// float ftmDist = prediction.distance;
|
|
// double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
|
|
|
|
// if (x > 1e-80)
|
|
// {
|
|
// Assert::isNot0(x, "");
|
|
|
|
// volatile double oldResult = result;
|
|
|
|
// result *= x;
|
|
|
|
|
|
// Assert::isNot0(result, "");
|
|
|
|
// printf("");
|
|
// }
|
|
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
return result;
|
|
} |