//used for both spheric and hybrid multilateration
static
bool generate_meas(vec &meas, const bvec &method, const mat &bs_pos, const vec &ms_pos)
{
unsigned int method_len = length(method);
unsigned int nb_bs = bs_pos.cols();
bool column = true;
if(3 == nb_bs) {
nb_bs = bs_pos.rows();
column = false;
}
if((nb_bs < method_len) || (3 != length(ms_pos))) {
return false;
}
meas.set_size(method_len);
vec pos(3);
vec pos_ref(3);
pos_ref = column ? bs_pos.get_col(0) : bs_pos.get_row(0);
for(unsigned int k = 0; k < method_len; ++k) {
if(bin(1) == method[k]) { /* hyperbolic */
pos = column ? bs_pos.get_col(k + 1) : bs_pos.get_row(k + 1);
meas[k] = get_dist(pos, ms_pos) - get_dist(pos_ref, ms_pos);
}
else { /* spherical */
pos = column ? bs_pos.get_col(k) : bs_pos.get_row(k);
meas[k] = get_dist(pos, ms_pos);
}
}
return true;
}
//used only for hyperbolic multilateration
static
bool generate_meas(mat &meas, const bvec &method, const mat &bs_pos, const vec &ms_pos)
{
unsigned int k;
unsigned int i;
unsigned int method_len = length(method);
unsigned int nb_bs = bs_pos.cols();
bool column = true;
if(3 == nb_bs) {
nb_bs = bs_pos.rows();
column = false;
}
if((nb_bs < method_len) || (3 != length(ms_pos))) {
return false;
}
meas.set_size(nb_bs, nb_bs);
vec pos_i(3);
vec pos_k(3);
for(k = 0; k < nb_bs; ++k) {
pos_k = column ? bs_pos.get_col(k) : bs_pos.get_row(k);
for(i = 0; i < nb_bs; ++i) {
pos_i = column ? bs_pos.get_col(i) : bs_pos.get_row(i);
meas(i, k) = get_dist(pos_i, ms_pos) - get_dist(pos_k, ms_pos);
}
}
return true;
}
ivec GreedyMaxMinDesign::subsample(mat X, int n)
{
if (n <= 0)
cerr << "Invalid sample size in GreedyMaxMinDesign::subsample(...)" << endl;
int imax;
ivec isample(n); // Indices of points in sample
ivec iremain; // Indices of remaining points
vec D; // Vector of min distances to sample
iremain = to_ivec(linspace(0,X.rows()-1,X.rows()));
// Start with location of maximum Z
max(X.get_col(X.cols()-1), imax);
isample(0) = imax;
iremain.del(imax);
// Add points having maximum minimum distance to sample
// until subsample size is reached
for (int i=1; i<n; i++)
{
vec xnew = X.get_row(isample(i-1)); // Last point added
vec Dnew = dist(X.get_rows(iremain), xnew); // Distances to xnew
// Update minimum distances to sample
if (D.length() == 0)
D = Dnew;
else
D = min(D,Dnew);
// Find point with max min distance
max(D,imax);
// Add it to sample
isample(i) = iremain(imax);
// And delete it from remainder
iremain.del(imax);
D.del(imax);
}
return isample;
}
mat ls_solve(const mat &A, const mat &B)
{
vec ls_solve(const mat &L, const mat &U, const vec &b);
mat L, U;
ivec p;
vec btemp;
lu(A, L, U, p);
mat X(B.rows(), B.cols());
for (int i=0; i<B.cols(); i++) {
btemp=B.get_col(i);
interchange_permutations(btemp, p);
X.set_col(i, ls_solve(L, U, btemp));
}
return X;
}
mat cov(const mat &X, bool is_zero_mean)
{
int d = X.cols(), n = X.rows();
mat R(d, d), m2(n, d);
vec tmp;
R = 0.0;
if (!is_zero_mean) {
// Compute and remove mean
for (int i = 0; i < d; i++) {
tmp = X.get_col(i);
m2.set_col(i, tmp - mean(tmp));
}
// Calc corr matrix
for (int i = 0; i < d; i++) {
for (int j = 0; j <= i; j++) {
for (int k = 0; k < n; k++) {
R(i, j) += m2(k, i) * m2(k, j);
}
R(j, i) = R(i, j); // When i=j this is unnecassary work
}
}
}
else {
// Calc corr matrix
for (int i = 0; i < d; i++) {
for (int j = 0; j <= i; j++) {
for (int k = 0; k < n; k++) {
R(i, j) += X(k, i) * X(k, j);
}
R(j, i) = R(i, j); // When i=j this is unnecassary work
}
}
}
R /= n;
return R;
}