uwb_tracker_node.py 文件源码

def initial_guess(self, ranges):
        """Computes an initial position guess based on range measurements.

        The initial position is computed using Gauss-Newton method.
        The behavior can be modified with some parameters: `self.initial_guess_...`.

        Args:
             ranges (list of floats): Range measurements.

        Returns:
            initial_state (numpy.ndarray): Initial state vector (velocity components are zero).
        """
        num_of_units = len(ranges)
        position = self.initial_guess_position
        H = np.zeros((num_of_units, position.size))
        z = np.zeros((num_of_units, 1))
        h = np.zeros((num_of_units, 1))
        residuals = np.zeros((num_of_units, 1))
        for i in xrange(self.initial_guess_iterations):
            self._compute_measurements_and_jacobians(ranges, position, h, H, z)
            new_residuals = z - h
            position = position + np.dot(self._solve_equation_least_squares(np.dot(H.T, H), H.T), new_residuals)
            if np.sum((new_residuals - residuals) ** 2) < self.initial_guess_tolerance:
                break
            residuals = new_residuals
        rospy.loginfo('initial guess residuals: {}'.format(residuals))
        if np.any(np.abs(residuals) > self.initial_guess_residuals_threshold):
            # This initial guess is not good enough
            return None
        initial_state = np.zeros((6, 1))
        initial_state[0:3] = position
        return initial_state