Gradient Descent TDOA localization

Hi there. I have a minimization problem regarding the TDOA localization. Briefly describing, there is a system comprising some transmitters and receivers. The trasmitters send unltrasound signals. The receivers get the signals and record the time of arrival (corresponding to the specific transmitter). Foe example for 5 trasmitters and 5 receivers, every receiver knows the time of arrival of the signal form each trasmitter. Between every transmitter and receiver, there exists a mathematical expression as where and are the coordinates of receiver i and transmitter j respectively and is the time when transmitter j sends the signal and is the time when receiver i receives the signal and c is the speed of sound. Here and and are unknown. This equation does not hold in reality because of the exsiting noise sources such as the noise in measuring time of arrivals. So the problem comes down to

where and the minimum is taken subject to all and and . All these variables are in vector u and we wnat to optimize for vector u. I wrote the Matlab code based on http://archive.cone.informatik.uni-freiburg.de/wendeberg/files/papers/LBS2013_wendeberg_Draft.pdf

Here is the Matlab code.

clc
clear all
close all

%   Setup
c = 340;   % velocity of sound
num_b = 6;   % number of beacons
num_t = 5;   % number of tags
pos_b = 20*rand(num_b,2);   % beacon(receiver) positions (20*20 room)
pos_t = 20*rand(num_t,2);   % tag(sender) positions (20*20 room)
tag_send = 5*rand(num_t,1);   % tag sending times (they start sending in the first 5 seconds)
gamma = 0.01;   % adaptive factor for step width
epsilon = 5;    % if step amplitude is less than epsilon, the algorithm stops

%   Calculating arrival times at beacons
T = zeros(num_b,num_t);   % matrix of arrival times with columns corresponding to tags
for i=1:num_t
   distances = sqrt(((pos_b - ones(num_b,1)*pos_t(i,:)).^2)*ones(2,1));
   T(:,i) = tag_send(i) + distances/c;
end

%   Gradient descent
u = [20*rand(1,2*num_t),5*rand(1,num_t),20*rand(1,2*num_b)]';    % producing initiall value for the parameters vector
help = u;

while 1
    Q = zeros(num_b*num_t,3*num_t+2*num_b);
    for i=1:num_b   % calculating Q matrix
        for j=1:num_t
            norm2 = sqrt((u(3*num_t+2*(i-1)+1)-u(2*(j-1)+1))^2+(u(3*num_t+2*(i-1)+2)-u(2*(j-1)+2))^2); 
            p_derivative = [u(3*num_t+2*(i-1)+1)-u(2*(j-1)+1), u(3*num_t+2*(i-1)+2)-u(2*(j-1)+2)]/norm2;
            Q((i-1)*num_t+j,2*(j-1)+1) = p_derivative(1);
            Q((i-1)*num_t+j,2*(j-1)+2) = p_derivative(2);
       
            Q((i-1)*num_t+j,2*num_t+j) = -c;
       
            Q((i-1)*num_t+j,3*num_t+2*(i-1)+1) = -p_derivative(1);
            Q((i-1)*num_t+j,3*num_t+2*(i-1)+2) = -p_derivative(2);
        end
    end

    b = [];
    for i=1:num_b   % calculating b vector
        for j=1:num_t
            norm2 = sqrt((u(3*num_t+2*(i-1)+1)-u(2*(j-1)+1))^2+(u(3*num_t+2*(i-1)+2)-u(2*(j-1)+2))^2); 
            b = [b; c*(T(i,j)-u(2*num_t+j))-norm2];
        end
    end

    uhat = gamma*Q'*b;
    u = u - uhat;
    if sqrt(uhat'*uhat)<epsilon
        break;
    end
    sqrt(uhat'*uhat)    % for testing if uhat gets smaller
end

Now the problem is that the code does not converge, i.e. the uhat value does not get smaller and eventually diminished and get bigger instead. Could you please help me with this? I have checked the code several times but nothing is wrong based on the paper linked.

[Benqwit]

Hi. Do you have any updates?

[Ali Gholami]

Yeah it had a small error. Solved!

[Keith Madison]

Hi. Would you mind elaborating on this error?

I'm attempting to solve a very similar problem now. I have 4 receivers and 1 emitter. The emitter emits a signal at an unknown time, and an unknown location, but with known velocity. The 4 receivers record the time of arrival. I want to determine the coordinates of the emitter. The receivers and emitter are in a 3-dimensional space.