FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Use kalman to predict missing measurements

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-13 15:08:40 +01:00
parent 2258fa92aa
commit d645ab7675
4 changed files with 114 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
code/Eval.cpp
View File

@@ -1,5 +1,8 @@
#include "Eval.h"
#include <array>
#include <vector>
#include "Settings.h"
#include <Indoor/math/distribution/Normal.h>
@@ -8,6 +11,8 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
{
double result = 1.0;
std::array<bool, 4> hadMeas = {false};
for (WiFiMeasurement wifi : measurements)
{
if (wifi.getNumSuccessfulMeasurements() < 3)
@@ -34,12 +39,12 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
if (ftmDist > 0)
{
//double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
double sigma = 5;
double sigma = 3;
if (ftmKalmanFilters != nullptr)
if (false && ftmKalmanFilters != nullptr)
{
Kalman& kalman = ftmKalmanFilters->at(mac);
ftmDist = kalman.predict(currentTime, ftmDist);
ftmDist = kalman.predictAndUpdate(currentTime, ftmDist);
//sigma = std::sqrt(kalman.P(0, 0));
Assert::isTrue(sigma > 0, "sigma");
@@ -54,6 +59,8 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
result *= x;
}
hadMeas[nucIndex] = true;
}
}
else
@@ -66,5 +73,49 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
}
}
// 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;
}