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

Reworked trilat code

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-27 16:59:03 +01:00
parent 992b8edc60
commit 3433bdaf66
5 changed files with 394 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
code/trilateration.h
View File

@@ -1,95 +1,14 @@
#pragma once
#include <cmath>
#include <vector>
#include <eigen3/Eigen/Eigen>
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
namespace Trilateration
{
// see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances);
Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances);
Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
{
// To locate position on a 2d plan, have to get at least 3 becaons,
// otherwise return false.
if (positions.size() < 3)
assert(false);
if (positions.size() != distances.size())
assert(false);
// Define the matrix that we are going to use
size_t count = positions.size();
size_t rows = count * (count - 1) / 2;
Eigen::MatrixXd m(rows, 2);
Eigen::VectorXd b(rows);
// Fill in matrix according to the equations
size_t row = 0;
double x1, x2, y1, y2, r1, r2;
for (size_t i = 0; i < count; ++i) {
for (size_t j = i + 1; j < count; ++j) {
x1 = positions[i].x, y1 = positions[i].y;
x2 = positions[j].x, y2 = positions[j].y;
r1 = distances[i];
r2 = distances[j];
m(row, 0) = x1 - x2;
m(row, 1) = y1 - y2;
b(row) = ((pow(x1, 2) - pow(x2, 2)) +
(pow(y1, 2) - pow(y2, 2)) -
(pow(r1, 2) - pow(r2, 2))) / 2;
row++;
}
}
// Then calculate to solve the equations, using the least square solution
Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
return Point2(location.x(), location.y());
}
Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
{
// To locate position in a 3D space, have to get at least 4 becaons
if (positions.size() < 4)
assert(false);
if (positions.size() != distances.size())
assert(false);
// Define the matrix that we are going to use
size_t count = positions.size();
size_t rows = count * (count - 1) / 2;
Eigen::MatrixXd m(rows, 3);
Eigen::VectorXd b(rows);
// Fill in matrix according to the equations
size_t row = 0;
double x1, x2, y1, y2, z1, z2, r1, r2;
for (size_t i = 0; i < count; ++i) {
for (size_t j = i + 1; j < count; ++j) {
x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
r1 = distances[i];
r2 = distances[j];
m(row, 0) = x1 - x2;
m(row, 1) = y1 - y2;
m(row, 2) = z1 - z2;
b(row) = ((pow(x1, 2) - pow(x2, 2)) +
(pow(y1, 2) - pow(y2, 2)) +
(pow(z1, 2) - pow(z2, 2)) -
(pow(r1, 2) - pow(r2, 2))) / 2;
row++;
}
}
// Then calculate to solve the equations, using the least square solution
Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
return Point3(location.x(), location.y(), location.z());
}
}
Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances);
};