Implementation on DWM1000

[maraltgs]

Hi, did you implement this algorithm on DWM1000?
I'm trying to implement it on DWM1000 and I can't get the desired TDOA.
I have 3 anchors and 1 tag. Anchor1 sends packet once in 200ms and anchor2 sends packet after 50ms receiving from anchor1 and anchor3 sends after 100ms. Tag receives all three of them and save the timestamps.

t = 0 anchor1 sends, anchor2, anchor3 and tag receives
t = 50ms anchor2 sends, tag receives
t =100ms anchor3 sends, tag receives
t = 200ms anchor1 sends, anchor2, anchor3 and tag receives
t = 250ms anchor2 sends, tag receives
...

Saved timestamps: (64byte from (0x15) // Lens = 14, ROD, Receive Message Time of Arrival (in double buffer set) register)
rx_1: 163757110878
rx_2: 167065521859
rx_3: 170312547991
rx_1: 195832532567
rx_2: 199140983045
rx_3: 202388169013

DWM time conversion:
1 uus = 65536 dtu
1 dtu = 15.65e-12 s
where, uus - ultra-wide band microsecond, dtu - device time unit

PROGRAM:

//# anchor locations:
anchor1 = [2.0, 1.0]
anchor2 = [0.5, 1.0]
anchor3 = [1.5, 2.5]
tag = [1.31, 1.39] # TO FIND

//# time delay [s]
T_d = 0.05
T_D21 = T_d * 1
T_D31 = T_d * 2
time_unit = 15.65 * 10 ** (-12)

//# distance between anchors [m]
d_21 = math.dist(anchor2, anchor1)
d_31 = math.dist(anchor3, anchor1)

//# time of flight [s]
tof_21 = d_21 / cl
tof_31 = d_31 / cl

//# measured timestamp
dtu_1 = 163757110878
dtu_2 = 167065521859
dtu_3 = 170312547991

//# DTU -> UUS
t_1 = dtu_1 * time_unit
t_2 = dtu_2 * time_unit
t_3 = dtu_3 * time_unit

//# measured TDOA
tdoa_21= (t_2 - t_1) - tof_21 - T_D21
tdoa_31 = (t_3 - t_1) - tof_31 - T_D31

//# distance differences [m]
dd21 = cl * tdoa_21
dd22 = cl * tdoa_31

Result:

tdoa_21, tdoa_31: 0.0017766268491885909 0.00259258480503749
dd21, dd31: 532619.330067043 777237.3712756399

Desired tdoa: 3.5494140085714735e-10 -7.576722263542789e-10
Desired distance difference: 0.10640875500892755 0.22714441909708172

How to get the desired TDOA? Am I doing this wrong?

I really appreciate your help. Hope you can help me.