uwb_tracker_node.py 文件源码

def _solve_equation_least_squares(self, A, B):
        """Solve system of linear equations A X = B.
        Currently using Pseudo-inverse because it also allows for singular matrices.

        Args:
             A (numpy.ndarray): Left-hand side of equation.
             B (numpy.ndarray): Right-hand side of equation.

        Returns:
             X (numpy.ndarray): Solution of equation.
        """
        # Pseudo-inverse
        X = np.dot(np.linalg.pinv(A), B)
        # LU decomposition
        # lu, piv = scipy.linalg.lu_factor(A)
        # X = scipy.linalg.lu_solve((lu, piv), B)
        # Vanilla least-squares from numpy
        # X, _, _, _ = np.linalg.lstsq(A, B)
        # QR decomposition
        # Q, R, P = scipy.linalg.qr(A, mode='economic', pivoting=True)
        # # Find first zero element in R
        # out = np.where(np.diag(R) == 0)[0]
        # if out.size == 0:
        #     i = R.shape[0]
        # else:
        #     i = out[0]
        # B_prime = np.dot(Q.T, B)
        # X = np.zeros((A.shape[1], B.shape[1]), dtype=A.dtype)
        # X[P[:i], :] = scipy.linalg.solve_triangular(R[:i, :i], B_prime[:i, :])
        return X