Merge branch 'master' of https://git.frank-ebner.de/FHWS/FtmPrologic
This commit is contained in:
6
.gitignore
vendored
6
.gitignore
vendored
@@ -3,4 +3,8 @@ measurements/error
|
|||||||
output/
|
output/
|
||||||
|
|
||||||
__pycache__/
|
__pycache__/
|
||||||
.vscode/
|
.vscode/
|
||||||
|
|
||||||
|
*.tex.ini
|
||||||
|
*.txt.user*
|
||||||
|
build-*/
|
||||||
|
|||||||
@@ -22,8 +22,9 @@ INCLUDE_DIRECTORIES(
|
|||||||
../../
|
../../
|
||||||
../../../
|
../../../
|
||||||
../../../../
|
../../../../
|
||||||
|
|
||||||
../../eigen3
|
../../eigen3
|
||||||
|
/usr/include/eigen3
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
#include <Indoor/geo/Point3.h>
|
#include <Indoor/geo/Point3.h>
|
||||||
|
|
||||||
|
|||||||
@@ -21,7 +21,7 @@ float Kalman::predictAndUpdate(const Timestamp timestamp, const float measurment
|
|||||||
Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
|
Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
|
||||||
|
|
||||||
// init kalman filter
|
// init kalman filter
|
||||||
if (isnan(lastTimestamp))
|
if (std::isnan(lastTimestamp))
|
||||||
{
|
{
|
||||||
P << 10, 0,
|
P << 10, 0,
|
||||||
0, 10; // Initial Uncertainty
|
0, 10; // Initial Uncertainty
|
||||||
@@ -30,7 +30,7 @@ float Kalman::predictAndUpdate(const Timestamp timestamp, const float measurment
|
|||||||
0;
|
0;
|
||||||
}
|
}
|
||||||
|
|
||||||
const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
|
const float dt = std::isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
|
||||||
lastTimestamp = timestamp.sec();
|
lastTimestamp = timestamp.sec();
|
||||||
|
|
||||||
Eigen::Matrix<float, 1, 2> H; // Measurement function
|
Eigen::Matrix<float, 1, 2> H; // Measurement function
|
||||||
@@ -62,7 +62,7 @@ float Kalman::predictAndUpdate(const Timestamp timestamp, const float measurment
|
|||||||
|
|
||||||
KalmanPrediction Kalman::predict(const Timestamp timestamp)
|
KalmanPrediction Kalman::predict(const Timestamp timestamp)
|
||||||
{
|
{
|
||||||
if (isnan(lastTimestamp))
|
if (std::isnan(lastTimestamp))
|
||||||
{
|
{
|
||||||
// Kalman has no data => nothing to predict
|
// Kalman has no data => nothing to predict
|
||||||
return KalmanPrediction{ NAN, NAN };
|
return KalmanPrediction{ NAN, NAN };
|
||||||
|
|||||||
@@ -1,7 +1,10 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
#include <Indoor/data/Timestamp.h>
|
#include <Indoor/data/Timestamp.h>
|
||||||
|
|
||||||
|
|
||||||
struct KalmanPrediction
|
struct KalmanPrediction
|
||||||
{
|
{
|
||||||
float distance;
|
float distance;
|
||||||
|
|||||||
@@ -147,7 +147,7 @@ namespace Plotta
|
|||||||
template<>
|
template<>
|
||||||
inline plottastream& operator<<(plottastream& stream, const float& value)
|
inline plottastream& operator<<(plottastream& stream, const float& value)
|
||||||
{
|
{
|
||||||
return stream << (isnan(value) ? "float(\"nan\")" : std::to_string(value));
|
return stream << (std::isnan(value) ? "float(\"nan\")" : std::to_string(value));
|
||||||
}
|
}
|
||||||
|
|
||||||
template<>
|
template<>
|
||||||
|
|||||||
@@ -583,7 +583,7 @@ public:
|
|||||||
|
|
||||||
#if defined(_WINDOWS)
|
#if defined(_WINDOWS)
|
||||||
stream << "set terminal windows size 2000,1500\n";
|
stream << "set terminal windows size 2000,1500\n";
|
||||||
#elif
|
#else
|
||||||
stream << "set terminal x11 size 2000,1500\n";
|
stream << "set terminal x11 size 2000,1500\n";
|
||||||
#endif
|
#endif
|
||||||
stream << gp.getBuffer();
|
stream << gp.getBuffer();
|
||||||
|
|||||||
@@ -620,10 +620,6 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
|
|||||||
|
|
||||||
errorPlot.frame();
|
errorPlot.frame();
|
||||||
|
|
||||||
|
|
||||||
std::cout << "Press any key to continue ..." << std::endl;
|
|
||||||
std::cin.get();
|
|
||||||
|
|
||||||
// MATLAB output
|
// MATLAB output
|
||||||
//{
|
//{
|
||||||
// std::ofstream matlab_error_out;
|
// std::ofstream matlab_error_out;
|
||||||
@@ -736,8 +732,8 @@ int main(int argc, char** argv)
|
|||||||
//Settings::data.Path9,
|
//Settings::data.Path9,
|
||||||
//Settings::data.Path10,
|
//Settings::data.Path10,
|
||||||
//Settings::data.Path11
|
//Settings::data.Path11
|
||||||
//Settings::data.Path20,
|
Settings::data.Path20,
|
||||||
//Settings::data.Path21,
|
Settings::data.Path21,
|
||||||
Settings::data.Path22,
|
Settings::data.Path22,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -192,7 +192,7 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
|
|||||||
|
|
||||||
|
|
||||||
plot.plot();
|
plot.plot();
|
||||||
Sleep(100);
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||||
}
|
}
|
||||||
|
|
||||||
obs.clear();
|
obs.clear();
|
||||||
@@ -281,4 +281,4 @@ int mainTrilat(int argc, char** argv)
|
|||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
10
code/misc.h
10
code/misc.h
@@ -69,10 +69,10 @@ struct WifiMeas {
|
|||||||
int numSucessMeas() const {
|
int numSucessMeas() const {
|
||||||
int count = 0;
|
int count = 0;
|
||||||
|
|
||||||
if (!isnan(ftmDists[0])) count++;
|
if (!std::isnan(ftmDists[0])) count++;
|
||||||
if (!isnan(ftmDists[1])) count++;
|
if (!std::isnan(ftmDists[1])) count++;
|
||||||
if (!isnan(ftmDists[2])) count++;
|
if (!std::isnan(ftmDists[2])) count++;
|
||||||
if (!isnan(ftmDists[3])) count++;
|
if (!std::isnan(ftmDists[3])) count++;
|
||||||
|
|
||||||
return count;
|
return count;
|
||||||
}
|
}
|
||||||
@@ -177,4 +177,4 @@ struct CmdArguments
|
|||||||
{
|
{
|
||||||
return std::find(arguments.begin(), arguments.end(), arg) != arguments.end();
|
return std::find(arguments.begin(), arguments.end(), arg) != arguments.end();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -246,4 +246,4 @@ namespace Trilateration
|
|||||||
|
|
||||||
return levenPos;
|
return levenPos;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user