Help with TDOA model

[Ita97]

Good morning everyone,

I am working on GM-PHD filters, and for research, I needed a time difference of arrival (TDOA) model.
Now, the TDOA is the difference between two ranges, and the measurement is a hyperbola. The hyperbola needs two forces, which coincide with the two sensor positions.

Here below is the custom model I developed:

class TDoAMeasurementModel(NonLinearGaussianMeasurement):

    def __init__(self, *args,**kwargs):
        super().__init__(*args, **kwargs)
        self.bs_positions = []
    
    def add_bs(self, bs_states):
        # add sensor positions (x, y, z)
        # the first sensor is the master
        for state in bs_states:
            self.bs_positions.append(StateVector(state[self.mapping,:])) 

    @property
    def ndim_meas(self) -> int:
        """ndim_meas getter method

        Returns
        -------
        :class:`int`
            The number of measurement dimensions
        """
        return 1
    
    def function(self, state, noise=False, **kwargs):
        if isinstance(noise, bool) or noise is None:
            if noise:
                noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs)
            else:
                noise = 0
        if not self.bs_positions:
            raise ValueError("BS positions are not set")
        target_position = state.state_vector[self.mapping, :]
        # compute the distances between the target and the sensors
        distance_to_master = np.linalg.norm(target_position - self.bs_positions[0])
        distances = [np.linalg.norm(target_position - pos) for pos in self.bs_positions[1:]]
        
        # Calcola il TDoA (distanza relativa divisa per la velocità del segnale)
        tdoas = (distance_to_master - distances) 
        return  StateVectors([tdoas]) + noise
    

    def jacobian(self, state, **kwargs):
        if not self.bs_positions:
            raise ValueError("BS positions are not set")
        target_position = state.state_vector[self.mapping, :]
        distance_to_master = np.linalg.norm(target_position - self.bs_positions[0])
        #print(self.bs_positions[1:])
        distances = [np.linalg.norm(target_position - pos) for pos in self.bs_positions[1:]]
        M = len(distances)
        D = self.ndim_state
        jacobian = np.zeros((M, D))
        
        for i, distance in enumerate(distances):
            diff_to_bs = (target_position - self.bs_positions[i+1]) / distance
            diff_to_master = (target_position - self.bs_positions[0]) / distance_to_master
            for j in range(len(self.mapping)):
                jacobian[i,self.mapping[j]] = (diff_to_bs[j] - diff_to_master[j]).flatten()
        
        return jacobian 

I tried to use it in the GM-PHD filter with 6 pairs of sensors to track a vehicle, but the position diverged.
So something is wrong, could you help me?
With the native stone soup models, everything works, so the problem should be here.

I thank you all in advance!