uwb_tracker_node.py 文件源码

def _ikf_iteration(self, x, n, ranges, h, H, z, estimate, R):
        """Update tracker based on a multi-range message.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.

        Returns:
            new_estimate (StateEstimate): Updated position estimate.
        """
        new_position = n[0:3]
        self._compute_measurements_and_jacobians(ranges, new_position, h, H, z)
        res = z - h
        S = np.dot(np.dot(H, estimate.covariance), H.T) + R
        K = np.dot(estimate.covariance, self._solve_equation_least_squares(S.T, H).T)
        mahalanobis = np.sqrt(np.dot(self._solve_equation_least_squares(S.T, res).T, res))
        if res.size not in self.outlier_thresholds:
            self.outlier_thresholds[res.size] = scipy.stats.chi2.isf(self.outlier_threshold_quantile, res.size)
        outlier_threshold = self.outlier_thresholds[res.size]
        if mahalanobis < outlier_threshold:
            n = x + np.dot(K, (res - np.dot(H, x - n)))
            outlier_flag = False
        else:
            outlier_flag = True
        return n, K, outlier_flag