Python scipy.linalg.block_diag() Examples

def test_simplex_sigma_points_2D():
    """ tests passing 1D data into sigma_points"""

    sp = SimplexSigmaPoints(4)

    Wm, Wc = sp.Wm, sp.Wc
    assert np.allclose(Wm, Wc, 1e-12)
    assert len(Wm) == 5

    mean = np.array([-1, 2, 0, 5])

    cov1 = np.array([[1, 0.5],
                     [0.5, 1]])

    cov2 = np.array([[5, 0.5],
                     [0.5, 3]])

    cov = linalg.block_diag(cov1, cov2)

    Xi = sp.sigma_points(mean, cov)
    xm, ucov = unscented_transform(Xi, Wm, Wc)

    assert np.allclose(xm, mean)
    assert np.allclose(cov, ucov) 

def test_rotated_squeezed(self, setup_eng, hbar, tol):
        """Testing decomposed rotated squeezed state"""
        eng, prog = setup_eng(3)

        r = 0.1
        phi = 0.2312
        v1 = (hbar / 2) * np.diag([np.exp(-r), np.exp(r)])
        A = changebasis(3)
        cov = A.T @ block_diag(*[rot(phi) @ v1 @ rot(phi).T] * 3) @ A

        with prog.context as q:
            ops.Gaussian(cov) | q

        state = eng.run(prog).state
        assert np.allclose(state.cov(), cov, atol=tol)
        assert len(eng.run_progs[-1]) == 3 

def test_matrix_static_gain(self):
        """Regression: can we create matrix static gains?"""
        d1 = np.array([[1, 2, 3], [4, 5, 6]])
        d2 = np.array([[7, 8], [9, 10], [11, 12]])
        g1 = StateSpace([], [], [], d1)

        # _remove_useless_states was making A = [[0]]
        self.assertEqual((0, 0), g1.A.shape)

        g2 = StateSpace([], [], [], d2)
        g3 = StateSpace([], [], [], d2.T)

        h1 = g1 * g2
        np.testing.assert_array_equal(np.dot(d1, d2), h1.D)
        h2 = g1 + g3
        np.testing.assert_array_equal(d1 + d2.T, h2.D)
        h3 = g1.feedback(g2)
        np.testing.assert_array_almost_equal(
            solve(np.eye(2) + np.dot(d1, d2), d1), h3.D)
        h4 = g1.append(g2)
        np.testing.assert_array_equal(block_diag(d1, d2), h4.D) 

def test_matrix_static_gain(self):
        """Regression: can we create matrix static gains?"""
        d1 = np.matrix([[1, 2, 3], [4, 5, 6]])
        d2 = np.matrix([[7, 8], [9, 10], [11, 12]])
        g1 = StateSpace([], [], [], d1)

        # _remove_useless_states was making A = [[0]]
        self.assertEqual((0, 0), g1.A.shape)

        g2 = StateSpace([], [], [], d2)
        g3 = StateSpace([], [], [], d2.T)

        h1 = g1 * g2
        np.testing.assert_array_equal(d1 * d2, h1.D)
        h2 = g1 + g3
        np.testing.assert_array_equal(d1 + d2.T, h2.D)
        h3 = g1.feedback(g2)
        np.testing.assert_array_almost_equal(
            solve(np.eye(2) + d1 * d2, d1), h3.D)
        h4 = g1.append(g2)
        np.testing.assert_array_equal(block_diag(d1, d2), h4.D) 

def __init__(self, vector_relative_degree, permutation_idxs=None):
        """Initialize a FeedbackLinearizableOutput object.

        Inputs:
        List of relative degrees, vector_relative_degree: int list
        Permutation indices, permutation_idxs: numpy array (p,)
        """

        self.vector_relative_degree = vector_relative_degree
        output_size = sum(vector_relative_degree)
        if permutation_idxs is None:
            permutation_idxs = arange(output_size)
        self.permutation_idxs = permutation_idxs
        self.reverse_permutation_idxs = argsort(permutation_idxs)
        self.relative_degree_idxs = cumsum(vector_relative_degree) - 1
        non_relative_degree_idxs = delete(arange(output_size), self.relative_degree_idxs)
        self.blocking_idxs = concatenate([non_relative_degree_idxs, self.relative_degree_idxs])
        self.unblocking_idxs = argsort(self.blocking_idxs)

        F = block_diag(*[diag(ones(gamma - 1), 1) for gamma in vector_relative_degree])
        G = block_diag(*[concatenate([zeros(gamma - 1), ones(1)]) for gamma in vector_relative_degree]).T

        self.F = self.reverse_permute(self.reverse_permute(F).T).T
        self.G = self.reverse_permute(G) 

def create_covariance_matrix(n, p, cov_u, var_u):
    if p == 1:
        K = np.ones((n, n)) * cov_u
        np.fill_diagonal(K, var_u)
    else:
        v = np.eye(p)
        diagonal = [v] * n
        K_v = block_diag(*diagonal)

        r = np.ones((p, p)) * cov_u
        np.fill_diagonal(r, var_u)
        diagonal = [r] * n
        K_r = block_diag(*diagonal)

        K = np.kron(np.ones((n, n)), r)

        K = K - K_r + K_v

    return K 

def output_shrink(K, L):
    """
    shrink the convolution output to half the size.
    used when both the annihilating filter and the uniform samples of sinusoids satisfy
    Hermitian symmetric.
    :param K: the annihilating filter size: K + 1
    :param L: length of the (complex-valued) b vector
    :return:
    """
    out_len = L - K
    if out_len % 2 == 0:
        half_out_len = np.int(out_len / 2.)
        mtx_r = np.hstack((np.eye(half_out_len),
                           np.zeros((half_out_len, half_out_len))))
        mtx_i = mtx_r
    else:
        half_out_len = np.int((out_len + 1) / 2.)
        mtx_r = np.hstack((np.eye(half_out_len),
                           np.zeros((half_out_len, half_out_len - 1))))
        mtx_i = np.hstack((np.eye(half_out_len - 1),
                           np.zeros((half_out_len - 1, half_out_len))))
    return linalg.block_diag(mtx_r, mtx_i) 

def test_rotated_squeezed(self, setup_eng, cutoff, hbar, tol):
        eng, prog = setup_eng(3)

        r = 0.1
        phi = 0.2312
        in_state = squeezed_state(r, phi, basis="fock", fock_dim=cutoff)

        v1 = (hbar / 2) * np.diag([np.exp(-2 * r), np.exp(2 * r)])
        A = changebasis(3)
        cov = A.T @ block_diag(*[rot(phi) @ v1 @ rot(phi).T] * 3) @ A

        with prog.context as q:
            ops.Gaussian(cov) | q

        state = eng.run(prog).state
        assert len(eng.run_progs[-1]) == 3

        for n in range(3):
            assert np.allclose(state.fidelity(in_state, n), 1, atol=tol) 

def mtx_fri2visi_ri_multiband(M, p_mic_x_all, p_mic_y_all, D1, D2, aslist=False):
    """
    build the matrix that maps the Fourier series to the visibility in terms of
    REAL-VALUED entries only. (matrix size double)
    :param M: the Fourier series expansion is limited from -M to M
    :param p_mic_x_all: a matrix that contains microphones x coordinates
    :param p_mic_y_all: a matrix that contains microphones y coordinates
    :param D1: expansion matrix for the real-part
    :param D2: expansion matrix for the imaginary-part
    :return:
    """
    num_bands = p_mic_x_all.shape[1]
    if aslist:
        return [mtx_fri2visi_ri(M, p_mic_x_all[:, band_count],
                                p_mic_y_all[:, band_count], D1, D2)
                for band_count in range(num_bands)]
    else:
        return linalg.block_diag(*[mtx_fri2visi_ri(M, p_mic_x_all[:, band_count],
                                                   p_mic_y_all[:, band_count], D1, D2)
                                   for band_count in range(num_bands)]) 

def constr(self):
        def fun(x):
            x_coord, y_coord, z_coord = self._get_cordinates(x)
            return x_coord**2 + y_coord**2 + z_coord**2 - 1

        def jac(x):
            x_coord, y_coord, z_coord = self._get_cordinates(x)
            Jx = 2 * np.diag(x_coord)
            Jy = 2 * np.diag(y_coord)
            Jz = 2 * np.diag(z_coord)

            return csc_matrix(np.hstack((Jx, Jy, Jz)))

        def hess(x, v):
            D = 2 * np.diag(v)
            return block_diag(D, D, D)

        return NonlinearConstraint(fun, ("less",), jac, hess) 

def const_vel_filter_2d(dt, x_ndim=1, P_diag=(1., 1, 1, 1), R_std=1.,
                        Q_var=0.0001):
    """ helper, constructs 1d, constant velocity filter"""

    kf = KalmanFilter(dim_x=4, dim_z=2)

    kf.x = np.array([[0., 0., 0., 0.]]).T
    kf.P *= np.diag(P_diag)
    kf.F = np.array([[1., dt, 0., 0.],
                     [0., 1., 0., 0.],
                     [0., 0., 1., dt],
                     [0., 0., 0., 1.]])

    kf.H = np.array([[1., 0, 0, 0],
                     [0., 0, 1, 0]])

    kf.R *= np.eye(2) * (R_std**2)
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)
    kf.Q = block_diag(q, q)

    return kf 

def test_regression():
    # Test Adjacency Regression
    m1 = block_diag(np.ones((4, 4)), np.zeros((4, 4)), np.zeros((4, 4)))
    m2 = block_diag(np.zeros((4, 4)), np.ones((4, 4)), np.zeros((4, 4)))
    m3 = block_diag(np.zeros((4, 4)), np.zeros((4, 4)), np.ones((4, 4)))
    Y = Adjacency(m1*1+m2*2+m3*3, matrix_type='similarity')
    X = Adjacency([m1, m2, m3], matrix_type='similarity')

    stats = Y.regress(X)
    assert np.allclose(stats['beta'], np.array([1, 2, 3]))

    # Test Design_Matrix Regression
    n = 10
    d = Adjacency([block_diag(np.ones((4, 4))+np.random.randn(4, 4)*.1, np.zeros((8, 8))) for x in range(n)],
                  matrix_type='similarity')
    X = Design_Matrix(np.ones(n))
    stats = d.regress(X)
    out = stats['beta'].within_cluster_mean(clusters=['Group1']*4 + ['Group2']*8)
    assert np.allclose(np.array([out['Group1'], out['Group2']]), np.array([1, 0]), rtol=1e-01)  # np.allclose(np.sum(stats['beta']-np.array([1,2,3])),0) 

def Sa(self, x_surface, geom):
        """Covariance of prior distribution, calculated at state x. We find
        the covariance in a normalized space (normalizing by z) and then un-
        normalize the result for the calling function."""

        lamb = self.calc_lamb(x_surface, geom)
        lamb_ref = lamb[self.idx_ref]
        ci = self.component(x_surface, geom)
        Cov = self.components[ci][1]
        Cov = Cov * (self.norm(lamb_ref)**2)

        # If there are no other state vector elements, we're done.
        if len(self.statevec_names) == len(self.idx_lamb):
            return Cov

        # Embed into a larger state vector covariance matrix
        nprefix = self.idx_lamb[0]
        nsuffix = len(self.statevec_names) - self.idx_lamb[-1] - 1
        Cov_prefix = np.zeros((nprefix, nprefix))
        Cov_suffix = np.zeros((nsuffix, nsuffix))
        return block_diag(Cov_prefix, Cov, Cov_suffix) 

def constr(self):
        def fun(x):
            x_coord, y_coord, z_coord = self._get_cordinates(x)
            return x_coord**2 + y_coord**2 + z_coord**2 - 1

        if self.constr_jac is None:
            def jac(x):
                x_coord, y_coord, z_coord = self._get_cordinates(x)
                Jx = 2 * np.diag(x_coord)
                Jy = 2 * np.diag(y_coord)
                Jz = 2 * np.diag(z_coord)
                return csc_matrix(np.hstack((Jx, Jy, Jz)))
        else:
            jac = self.constr_jac

        if self.constr_hess is None:
            def hess(x, v):
                D = 2 * np.diag(v)
                return block_diag(D, D, D)
        else:
            hess = self.constr_hess

        return NonlinearConstraint(fun, -np.inf, 0, jac, hess) 

def test_controlled_phase(self, tol):
        """Test the CZ symplectic transform."""

        s = 0.543
        S = controlled_phase(s)

        # test that S = R_2(pi/2) CX(s) R_2(pi/2)^\dagger
        R2 = block_diag(np.identity(2), rotation(np.pi/2))[:, [0, 2, 1, 3]][[0, 2, 1, 3]]
        expected = R2 @ controlled_addition(s) @ R2.conj().T
        assert S == pytest.approx(expected, abs=tol)

        # test that S[x1, x2, p1, p2] -> [x1, x2, p1+sx2, p2+sx1]
        x1 = 0.5432
        x2 = -0.453
        p1 = 0.154
        p2 = -0.123
        out = S @ np.array([x1, x2, p1, p2])*np.sqrt(2*hbar)
        expected = np.array([x1, x2, p1+s*x2, p2+s*x1])*np.sqrt(2*hbar)
        assert out == pytest.approx(expected, abs=tol) 

def test_controlled_addition(self, tol):
        """Test the CX symplectic transform."""

        s = 0.543
        S = controlled_addition(s)

        # test that S = B(theta+pi/2, 0) [S(z) x S(-z)] B(theta, 0)
        r = np.arcsinh(-s/2)
        theta = 0.5*np.arctan2(-1/np.cosh(r), -np.tanh(r))
        Sz = block_diag(squeezing(r, 0), squeezing(-r, 0))[:, [0, 2, 1, 3]][[0, 2, 1, 3]]

        expected = beamsplitter(theta+np.pi/2, 0) @ Sz @ beamsplitter(theta, 0)
        assert S == pytest.approx(expected, abs=tol)

        # test that S[x1, x2, p1, p2] -> [x1, x2+sx1, p1-sp2, p2]
        x1 = 0.5432
        x2 = -0.453
        p1 = 0.154
        p2 = -0.123
        out = S @ np.array([x1, x2, p1, p2])*np.sqrt(2*hbar)
        expected = np.array([x1, x2+s*x1, p1-s*p2, p2])*np.sqrt(2*hbar)
        assert out == pytest.approx(expected, abs=tol) 

def SO3_irreps(g, irreps):
    global Jd

    # First, compute sinusoids at all required frequencies, i.e.
    # cos(n x) for n=0, ..., max(irreps)
    # sin(n x) for n=-max(irreps), ..., max(irreps)
    # where x ranges over the three parameters of SO(3).

    # In theory, it may be faster to evaluate cos(x) once and then use
    # Chebyshev polynomials to obtain cos(n*x), but in practice this appears
    # to be slower than just evaluating cos(n*x).
    dim = np.sum(2 * np.array(irreps) + 1)
    T = np.empty((dim, dim, g.shape[1]))
    for i in range(g.shape[1]):
        T[:, :, i] = block_diag(*[rot_mat(g[0, i], g[1, i], g[2, i], l, Jd[l]) for l in irreps])
    return T 

def test_two_mode_squeezing(self, tol):
        """Test the two mode squeezing symplectic transform."""

        r = 0.543
        phi = 0.123
        S = two_mode_squeezing(r, phi)

        # test that S = B^\dagger(pi/4, 0) [S(z) x S(-z)] B(pi/4)
        B = beamsplitter(np.pi/4, 0)
        Sz = block_diag(squeezing(r, phi), squeezing(-r, phi))[:, [0, 2, 1, 3]][[0, 2, 1, 3]]
        expected = B.conj().T @ Sz @ B
        assert S == pytest.approx(expected, abs=tol)

        # test that S |a1, a2> = |ta1+ra2, ta2+ra1>
        a1 = 0.23+0.12j
        a2 = 0.23+0.12j
        out = S @ np.array([a1.real, a2.real, a1.imag, a2.imag])*np.sqrt(2*hbar)

        T = np.cosh(r)
        R = np.exp(1j*phi)*np.sinh(r)
        a1out = T*a1 + R*np.conj(a2)
        a2out = T*a2 + R*np.conj(a1)
        expected = np.array([a1out.real, a2out.real, a1out.imag, a2out.imag])*np.sqrt(2*hbar)
        assert out == pytest.approx(expected, abs=tol) 

def _rotation(phi, bare=False):
    r"""Utility function, returns the Heisenberg transformation of a phase rotation gate.

    The transformation matrix returned is:

    .. math:: M = \begin{bmatrix}
        1 & 0 & 0\\
        0 & \cos\phi & -\sin\phi\\
        0 & \sin\phi & \cos\phi
        \end{bmatrix}

    Args:
        phi (float): rotation angle.
        bare (bool): if True, return a simple 2d rotation matrix

    Returns:
        array[float]: transformation matrix
    """
    c = math.cos(phi)
    s = math.sin(phi)
    temp = np.array([[c, -s], [s, c]])
    if bare:
        return temp
    return block_diag(1, temp)  # pylint: disable=no-member 

def test_decompose(self, tol):
        """Test the two mode squeezing symplectic transform decomposes correctly."""
        r = 0.543
        phi = 0.123
        S = symplectic.two_mode_squeezing(r, phi)

        # test that S = B^\dagger(pi/4, 0) [S(z) x S(-z)] B(pi/4)
        # fmt: off
        B = np.array([[1, -1, 0, 0], [1, 1, 0, 0], [0, 0, 1, -1], [0, 0, 1, 1]])/np.sqrt(2)

        Sq1 = np.array([[np.cosh(r)-np.cos(phi)*np.sinh(r), -np.sin(phi)*np.sinh(r)],
                        [-np.sin(phi)*np.sinh(r), np.cosh(r)+np.cos(phi)*np.sinh(r)]])

        Sq2 = np.array([[np.cosh(-r)-np.cos(phi)*np.sinh(-r), -np.sin(phi)*np.sinh(-r)],
                        [-np.sin(phi)*np.sinh(-r), np.cosh(-r)+np.cos(phi)*np.sinh(-r)]])
        # fmt: on

        Sz = block_diag(Sq1, Sq2)[:, [0, 2, 1, 3]][[0, 2, 1, 3]]
        expected = B.conj().T @ Sz @ B
        assert np.allclose(S, expected, atol=tol, rtol=0) 

def set_wrench(self, wrench):
        """
        Set contact wrench directly.

        Parameters
        ----------
        wrench : array, shape=(6,)
            Wrench coordinates given in the contact frame.

        Notes
        -----
        This function switches the contact to "managed" mode, as opposed to the
        default "supporting" mode where the wrench distributor finds contact
        wrenches by numerical optimization.
        """
        if not type(wrench) is ndarray:
            wrench = array(wrench)
        if not self.is_managed:
            self.set_color('b')
        self.is_managed = True
        self.wrench = dot(block_diag(self.R, self.R), wrench) 

def _construct_array(self):
        return sla.block_diag(*(block.array for block in self._blocks)) 

def Sa(self, x, geom):
        """Calculate the prior covariance of the state vector (the 
        concatenation of state vectors for the surface and radiative transfer 
        model).

        NOTE: the surface prior depends on the current state; this
        is so we can calculate the local prior.
        """

        x_surface = x[self.idx_surface]
        Sa_surface = self.surface.Sa(x_surface, geom)[:, :]
        Sa_RT = self.RT.Sa()[:, :]
        Sa_instrument = self.instrument.Sa()[:, :]
        return block_diag(Sa_surface, Sa_RT, Sa_instrument) 

def __init__(self, robotic_system_output, P, Q, epsilon):
        M = block_diag(identity(robotic_system_output.k) / epsilon, identity(robotic_system_output.k))
        P = dot(M, dot(P, M)) * epsilon
        Q = dot(M, dot(Q, M))
        alpha = min(eigvals(Q)) / max(eigvals(P))
        QuadraticControlLyapunovFunction.__init__(self, robotic_system_output, P, alpha) 

def interpolation_lagrange_matrix(old_grid, new_grid):
    """
    Evaluate lagrange matrix to interpolate state and control values from the solved grid onto the new grid

    Parameters
    ----------
    old_grid: GridData
        The GridData object representing the grid on which the problem has been solved
    new_grid: GridData
        The GridData object representing the new, higher-order grid

    Returns
    -------
    L: np.ndarray
        The lagrange interpolation matrix

    """
    L_blocks = []

    for iseg in range(old_grid.num_segments):
        i1, i2 = old_grid.subset_segment_indices['all'][iseg, :]
        indices = old_grid.subset_node_indices['all'][i1:i2]
        nodes_given = old_grid.node_stau[indices]

        i1, i2 = new_grid.subset_segment_indices['all'][iseg, :]
        indices = new_grid.subset_node_indices['all'][i1:i2]
        nodes_eval = new_grid.node_stau[indices]

        L_block, _ = lagrange_matrices(nodes_given, nodes_eval)

        L_blocks.append(L_block)

    L = block_diag(*L_blocks)

    return L 

def integration_matrix(grid):
    """
    Evaluate the Integration matrix of the given grid.

    Parameters
    ----------
    grid: GridData
        The GridData object representing the grid on which the integration matrix is to be evaluated

    Returns
    -------
    I: np.ndarray
        The integration matrix used to propagate initial states over segments

    """
    I_blocks = []

    for iseg in range(grid.num_segments):
        i1, i2 = grid.subset_segment_indices['all'][iseg, :]
        indices = grid.subset_node_indices['all'][i1:i2]
        nodes_given = grid.node_stau[indices]

        i1, i2 = grid.subset_segment_indices['all'][iseg, :]
        indices = grid.subset_node_indices['all'][i1:i2]
        nodes_eval = grid.node_stau[indices][1:]

        _, D_block = lagrange_matrices(nodes_given, nodes_eval)
        I_block = np.linalg.inv(D_block[:, 1:])
        I_blocks.append(I_block)

    I = block_diag(*I_blocks)

    return I 

def test_four_modes(hbar):
    """ Test that probabilities are correctly updates for a four modes system under loss"""
    # All this block is to generate the correct covariance matrix.
    # It correnponds to num_modes=4 modes that undergo two mode squeezing between modes i and i + (num_modes / 2).
    # Then they undergo displacement.
    # The signal and idlers see and interferometer with unitary matrix u2x2.
    # And then they see loss by amount etas[i].
    num_modes = 4
    theta = 0.45
    phi = 0.7
    u2x2 = np.array([[np.cos(theta / 2), np.exp(1j * phi) * np.sin(theta / 2)],
                    [-np.exp(-1j * phi) * np.sin(theta / 2), np.cos(theta / 2)]])

    u4x4 = block_diag(u2x2, u2x2)

    cov = np.identity(2 * num_modes) * hbar / 2
    means = 0.5 * np.random.rand(2 * num_modes) * np.sqrt(hbar / 2)
    rs = [0.1, 0.9]
    n_half = num_modes // 2

    for i, r_val in enumerate(rs):
        Sexpanded = expand(two_mode_squeezing(r_val, 0.0), [i, n_half + i], num_modes)
        cov = Sexpanded @ cov @ (Sexpanded.T)

    Su = expand(interferometer(u4x4), range(num_modes), num_modes)
    cov = Su @ cov @ (Su.T)
    cov_lossless = np.copy(cov)
    means_lossless = np.copy(means)
    etas = [0.9, 0.7, 0.9, 0.1]

    for i, eta in enumerate(etas):
        means, cov = loss(means, cov, eta, i, hbar=hbar)

    cutoff = 3
    probs_lossless = probabilities(means_lossless, cov_lossless, 4 * cutoff, hbar=hbar)
    probs = probabilities(means, cov, cutoff, hbar=hbar)
    probs_updated = update_probabilities_with_loss(etas, probs_lossless)
    assert np.allclose(probs, probs_updated[:cutoff, :cutoff, :cutoff, :cutoff], atol=1e-6) 

def compute_static_equilibrium_polygon(self, method='hull'):
        """
        Compute the static-equilibrium polygon of the center of mass.

        Parameters
        ----------
        method : string, optional
            Choice between 'bretl', 'cdd' or 'hull'.

        Returns
        -------
        vertices : list of arrays
            2D vertices of the static-equilibrium polygon.

        Notes
        -----
        The method 'bretl' is adapted from in [Bretl08]_ where the
        static-equilibrium polygon was introduced. The method 'cdd' corresponds
        to the double-description approach described in [Caron17z]_. See the
        Appendix from [Caron16]_ for a performance comparison.
        """
        if method == 'hull':
            A_O = self.compute_wrench_inequalities([0, 0, 0])
            k, a_Oz, a_x, a_y = A_O.shape[0], A_O[:, 2], A_O[:, 3], A_O[:, 4]
            B, c = hstack([-a_y.reshape((k, 1)), +a_x.reshape((k, 1))]), -a_Oz
            return compute_polygon_hull(B, c)
        G_0 = self.compute_grasp_matrix([0., 0., 0.])
        F = block_diag(*[ct.wrench_inequalities for ct in self.contacts])
        mass = 42.  # [kg]
        # mass has no effect on the output polygon, see IV.B in [Caron16]_
        E = 1. / (mass * 9.81) * vstack([-G_0[4, :], +G_0[3, :]])
        f = array([0., 0.])
        return project_polytope(
            proj=(E, f),
            ineq=(F, zeros(F.shape[0])),
            eq=(G_0[(0, 1, 2, 5), :], array([0, 0, mass * 9.81, 0])),
            method=method) 

def mtx_fri2visi_ri(M, p_mic_x, p_mic_y, D1, D2):
    """
    build the matrix that maps the Fourier series to the visibility in terms of
    REAL-VALUED entries only. (matrix size double)
    :param M: the Fourier series expansion is limited from -M to M
    :param p_mic_x: a vector that contains microphones x coordinates
    :param p_mic_y: a vector that contains microphones y coordinates
    :param D1: expansion matrix for the real-part
    :param D2: expansion matrix for the imaginary-part
    :return:
    """
    return np.dot(cpx_mtx2real(mtx_freq2visi(M, p_mic_x, p_mic_y)),
                  linalg.block_diag(D1, D2)) 

def __init__(self):
        # Initialize parameters for tracker
        self.id = 0
        self.num_hits = 0
        self.num_unmatched = 0
        self.box = []

        # Initialize parameters for the Kalman filter
        self.kf = KalmanFilter(dim_x=8, dim_z=8)
        self.dt = 1.0
        self.x_state = []

        # State transition matrix (assuming constant velocity model)
        self.kf.F = np.array([[1, self.dt, 0, 0, 0, 0, 0, 0],
                              [0, 1, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, self.dt, 0, 0, 0, 0],
                              [0, 0, 0, 1, 0, 0, 0, 0],
                              [0, 0, 0, 0, 1, self.dt, 0, 0],
                              [0, 0, 0, 0, 0, 1, 0, 0],
                              [0, 0, 0, 0, 0, 0, 1, self.dt],
                              [0, 0, 0, 0, 0, 0, 0, 1]])

        # Measurement matrix (assuming we can only measure the coordinates)
        self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, 0, 0, 0, 0, 0],
                              [0, 0, 0, 0, 1, 0, 0, 0],
                              [0, 0, 0, 0, 0, 0, 1, 0]])

        # State covariance matrix
        self.kf.P *= 100.0

        # Process uncertainty
        self.Q_comp_mat = np.array([[self.dt ** 4 / 2., self.dt ** 3 / 2.],
                                    [self.dt ** 3 / 2., self.dt ** 2]])
        self.kf.Q = block_diag(self.Q_comp_mat, self.Q_comp_mat,
                               self.Q_comp_mat, self.Q_comp_mat)

        # State uncertainty
        self.kf.R = np.eye(4)*6.25

    # Method: Used to predict and update the next state for a bounding box