-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathplay_with_eigen.cpp
209 lines (158 loc) · 6.45 KB
/
play_with_eigen.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
#include <iostream>
#include <Eigen/Dense>
#include "Eigen/Eigen" // AFTER GRIDMAP!
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>
class SplineFunction {
public:
// The spline is used to interpolate antenna gain values, as we only have the graphs
SplineFunction()
{}
SplineFunction(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec)
: x_min(x_vec.minCoeff()),
x_max(x_vec.maxCoeff()),
y_min(y_vec.minCoeff()),
y_max(y_vec.maxCoeff()),
// Spline fitting here. X values are scaled down to [0, 1] for this.
spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y_vec.transpose(), std::min<int>(x_vec.rows() - 1, 6), scaled_values(x_vec))) // No more than cubic spline, but accept short vectors.
{}
// x values need to be scaled down in extraction as well.
double interpDeg(double x) const {
double y;
y = spline_(scaled_value(x))(0);
// interpolation may produce values bigger and lower than our limits ...
y = std::max(std::min(y, y_max), y_min );
return y;
}
double interpRad(double x) const {
return interpDeg(x*180.0/M_PI);
}
// Helpers to scale X values down to [0, 1]
double scaled_value(double x) const {
return (x - x_min) / (x_max - x_min);
}
private:
Eigen::RowVectorXd scaled_values(Eigen::VectorXd const &x_vec) const {
return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
}
double x_min;
double x_max;
double y_min;
double y_max;
// Spline of one-dimensional "points."
Eigen::Spline<double, 1> spline_;
};
/////////////////////////////
// quick build:
// g++ -I /usr/include/eigen3/ play_with_eigen.cpp -o play_with_eigen -std=c++11
using namespace std::placeholders;
using Eigen::MatrixXd;
const double C = 299792458.0;
const double SENSITIVITY = -115; // dB
const double TAG_LOSSES = -4.8;
const double LOSS_CONSTANT = 147.55;
const double freq= 865e6;
const double lambda = C/freq;
const double ANTENNA_LOSSES_LIST [25] = { -22.6, -25.2, -25, -20.2, -17.6, -15.6, -14, -11.2, -7.8, -5.2, -2.4, -0.6, 0, -0.6, -2.4, -5.2, -8.8, -12.2, -16.4, -19.2, -20.8, -24.4, -28.2, -24, -22.6};
const double ANTENNA_ANGLES_LIST [25] = {-180.0, -165.0, -150.0, -135.0, -120.0, -105.0, -90.0, -75.0, -60.0, -45.0, -30.0, -15.0, 0.0, 15.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0, 135.0, 150.0, 165.0, 180.0};
//! Generates a mesh, just like Matlab's meshgrid
// Template specialization for column vectors (Eigen::VectorXd)
// in : x, y column vectors
// X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, -1, 1>& x,
const Eigen::Matrix<Scalar, -1, 1>& y,
Eigen::Matrix<Scalar, -1, -1>& X,
Eigen::Matrix<Scalar, -1, -1>& Y) {
const long nx = x.size(), ny = y.size();
X.resize(ny, nx);
Y.resize(ny, nx);
for (long i = 0; i < ny; ++i) {
X.row(i) = x.transpose();
}
// for (long j = 0; j < nx; ++j) {
// Y.col(j) = y;
// }
for (long j = 0; j < nx; ++j) {
Y.col(j) = y.reverse();
}
}
//! Generates a mesh, just like Matlab's meshgrid
// Template specialization for row vectors (Eigen::RowVectorXd)
// in : x, y row vectors
// X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, 1, -1>& x,
const Eigen::Matrix<Scalar, 1, -1>& y,
Eigen::Matrix<Scalar, -1, -1>& X,
Eigen::Matrix<Scalar, -1, -1>& Y) {
Eigen::Matrix<Scalar, -1, 1> xt = x.transpose(),
yt = y.transpose();
meshgrid(xt, yt, X, Y);
}
typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXd Mat;
int main(int argc, char **argv)
{
int Nx, Ny, x_min, x_max, y_min, y_max;
double x_m, y_m;
Mat X, Y, R, A, propL, antL,totalLoss, rxPower;
Vec x,y;
double txPower = 0; //dB
///////////////////////// // build spline to interpolate antenna gains;
std::vector<double> xVec(ANTENNA_ANGLES_LIST, ANTENNA_ANGLES_LIST + 25);
Eigen::VectorXd xvals = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(xVec.data(), xVec.size());
std::vector<double> yVec(ANTENNA_LOSSES_LIST, ANTENNA_LOSSES_LIST + 25);
Eigen::VectorXd yvals= Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(yVec.data(), yVec.size());
SplineFunction _antenna_gains= SplineFunction(xvals, yvals);
////////////////////////
Nx = 5;
Ny = 8;
x_min = -1;
x_max = 1;
y_min = -2;
y_max = 2;
x_m = 0.5;
y_m = 1.2;
x = Vec::LinSpaced(Nx, x_min, x_max);
y = Vec::LinSpaced(Ny, y_min, y_max);
std::cout << "x" << std::endl;
std::cout << x << std::endl;
std::cout << "y" << std::endl;
std::cout << y << std::endl;
// create X,Y meshgrids
meshgrid(x, y, X, Y);
// distance to point m
X = X.array() - x_m;
Y = Y.array() - y_m;
std::cout << "X" << std::endl;
std::cout << X << std::endl;
std::cout << "Y" << std::endl;
std::cout << Y << std::endl;
// create R,Ang matrixes
R = (X.array().square() + Y.array().square()).array().sqrt();
A = Y.binaryExpr(X, std::ptr_fun(atan2));
std::cout << "R" << std::endl;
std::cout << R << std::endl;
std::cout << "A" << std::endl;
std::cout << (A*180.0/3.141592) << std::endl;
// Create a propagation matrix without taking obstacles
auto funtor = std::bind(&SplineFunction::interpRad, _antenna_gains, _1) ;
antL = TAG_LOSSES + A.unaryExpr( funtor ).array();
std::cout << "antL" << std::endl;
std::cout << antL << std::endl;
propL = LOSS_CONSTANT - (20.0 * (R * freq).unaryExpr(std::ptr_fun(log10))).array() ;
std::cout << "propL" << std::endl;
std::cout << propL << std::endl;
// signal goes from antenna to tag and comes back again, so we double the losses
totalLoss = 2*antL + 2*propL;
std::cout << "totalLoss" << std::endl;
std::cout << totalLoss << std::endl;
rxPower = txPower + totalLoss.array();
// this should remove points where friis is not applicable
rxPower = (R.array()>2*lambda).select(rxPower,SENSITIVITY);
// this should remove points where received power is too low
rxPower = (rxPower.array()>SENSITIVITY).select(rxPower,SENSITIVITY);
std::cout << "rxPower" << std::endl;
std::cout << rxPower << std::endl;
}