Algorithm 1: 3D trilateration

1 new data from N beacons; 2 trilateration with the isotropic formulation and the on-line parameters from the previous step; 3 identification of closest voxel bin of the 3D voxel volume(matrix), at the ijk indices, from the solved xyz position; 4 update of $s^2$ and $Ws$; 5 trilateration with updated parameters; 6 update the ijk cell of the voxel volume;