Mulultilateration problem with solution

[Gianni]

Hi guys,

I'm trying to implement multilateration with four BSs. In this contest I wanna use TOA with spherical multilateration. I've implemented this code but seem doesn't not works (see below) and I don't know why . In my idea the code is ok but the TOA algorithm fails. I've tryed to execute the multilateration_test.cpp for compare and try to reuse this code with my data but this doesn't not works and returns one message like this:

cannot update subsets
.....
error in geo_pos_update_subsets

Where I'm wrong? Thanks a lot!

#include <iostream>
#include <sstream>
#include <iomanip>
#include <functional>
#include <vector>
#include <algorithm>
#include <math.h>
#include <assert.h>
#include "anchor.h"

#include <itpp/itcomm.h>
#include <itpp/comm/multilateration.h>

using namespace std;
using namespace itpp;

vec arrival_time(4);

void set_anchor_pos(mat &bs_pos)
{
    //Anchor A0
    bs_pos(0, 0) = 0;
    bs_pos(1, 0) = 0;
    bs_pos(2, 0) = 0;

    //Anchor A1
    bs_pos(0, 1) = 0.7;
    bs_pos(1, 1) = 0;
    bs_pos(2, 1) = 0;

    //Anchor A2
    bs_pos(0, 2) = 0.7;
    bs_pos(1, 2) = 1.5;
    bs_pos(2, 2) = 0;

    //Anchor A3
    bs_pos(0, 3) = 0;
    bs_pos(1, 3) = 1.5;
    bs_pos(2, 3) = 0;
}

int main()
{
    bool res;
    mat bs_pos;
    bvec method;
    vec target_dist(4);
    vec target_pos;

    Multilateration multi;
    vec meas_spher; //measurements vector for spheric multilateration

    method.set_size(4);
    bs_pos.set_size(3, 4);      // 4 anchor for TOA
    method.zeros();             //set all 0 for Spherical ML

    set_anchor_pos(bs_pos);
    multi.setup(method, bs_pos);

    assert(Multilateration::MULTI_SPHERICAL == multi.get_type());

    const double speed_of_light = 299792458.0;

    /* Arrival time for each anchor in seconds */
    arrival_time.clear();   
    arrival_time(0) = 16.25358532822829;    // @Anchor A0
    arrival_time(1) = 16.253585299792167;   // @Anchor A1
    arrival_time(2) = 16.253585298461914;   // @Anchor A2
    arrival_time(3) = 16.253585301012873;   // @Anchor A3

    //convert times into distances, since algorithm needs distances
    for (int i = 0; i < 4; ++i) 
    {
        target_dist(i) = arrival_time(i)*speed_of_light;
    }

    res = multi.get_pos(target_pos, target_dist);  //get location
    if (res)
        cout << "Spherical location output " << target_pos(0) << " " << target_pos(1) << " " << target_pos(2) << endl;

    return 0;
}

[Gianni]

Update. I tried to run itpp_gtest and the result il this:

[----------] 2 tests from Multilateration
[ RUN      ] Multilateration.get_pos
*** Warning in /home/uwb/Desktop/itpp-4.3.1/itpp/comm/multilateration.cpp on line 272:
cannot update subsets
*** Warning in /home/uwb/Desktop/itpp-4.3.1/itpp/comm/multilateration.cpp on line 1306:
error in geo_pos_update_subsets
/home/uwb/Desktop/itpp-4.3.1/gtests/multilateration_test.cpp:223: Failure
Value of: multi.get_pos(actual_ms_pos, meas)
  Actual: false
Expected: true
[  FAILED  ] Multilateration.get_pos (0 ms)
[ RUN      ] Multilateration.get_crlb
[       OK ] Multilateration.get_crlb (1 ms)
[----------] 2 tests from Multilateration (1 ms total)

Any suggestions?

[Gianni]

I answer to myself: I've installed again itpp from git source and not from .tar (the .tar isn't updated) and the bug is fixed.