tets

for i in range(N):

        phi_t = x_ref[0, i]
        theta_t = x_ref[1, i]
        psi_t = x_ref[2, i]

        I_world = np.array([[(Ixx*cos(psi_t) + Iyx*sin(psi_t))*cos(psi_t) + (Ixy*cos(psi_t) + Iyy*sin(psi_t))*sin(psi_t), -(Ixx*cos(psi_t) + Iyx*sin(psi_t))*sin(psi_t) + (Ixy*cos(psi_t) + Iyy*sin(psi_t))*cos(psi_t), Ixz*cos(psi_t) + Iyz*sin(psi_t)], [(-Ixx*sin(psi_t) + Iyx*cos(psi_t))*cos(psi_t) + (-Ixy*sin(psi_t) + Iyy*cos(psi_t))*sin(psi_t), -(-Ixx*sin(psi_t) + Iyx*cos(psi_t))*sin(psi_t) + (-Ixy*sin(psi_t) + Iyy*cos(psi_t))*cos(psi_t), -Ixz*sin(psi_t) + Iyz*cos(psi_t)], [Ixy*sin(psi_t) + Izx*cos(psi_t), Ixy*cos(psi_t) - Izx*sin(psi_t), Izz]])
        # Location of the force vector being applied by the left foot.
        r_x_left = 0.15
        r_y_left = 0
        r_z_left = 0

        # Location of the force vector being applied by the right foot.
        r_x_right = -0.15
        r_y_right = 0
        r_z_right = 0

        # Skew symmetric versions for the 3x1 foot position vector resembling the matrix version of the cross product of two vectors. This is needed for the matrix form.
        r_left_skew_symmetric = np.array([[0, -r_z_left, r_y_left],
                                        [r_z_left, 0, -r_x_left],
                                        [-r_y_left, r_x_left, 0]])

        r_right_skew_symmetric = np.array([[0, -r_z_right, r_y_right],
                                        [r_z_right, 0, -r_x_right],
                                        [-r_y_right, r_x_right, 0]])

        A_c = np.array([[0, 0, 0, 0, 0, 0, math.cos(psi_t), math.sin(psi_t), 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, -math.sin(psi_t), math.cos(psi_t), 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],

                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],

                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],

                            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])

        B_c = np.block([[0, 0, 0, 0, 0, 0],
                        [0, 0, 0, 0, 0, 0],
                        [0, 0, 0, 0, 0, 0],
                        [0, 0, 0, 0, 0, 0],
                        [0, 0, 0, 0, 0, 0],
                        [0, 0, 0, 0, 0, 0],
                        [I_world @ r_left_skew_symmetric, I_world @ r_right_skew_symmetric],
                        [1/m_value, 0, 0, 1/m_value, 0, 0],
                        [0, 1/m_value, 0, 0, 1/m_value, 0],
                        [0, 0, 1/m_value, 0, 0, 1/m_value],
                        [0, 0, 0, 0, 0, 0]])

        A_d, B_d = discretize_ss(A_c, B_c, dt)

        P_param[:, 1 + N + (i*n):1 + N + (i*n)+n] = A_d
        P_param[:, 1 + N + n * N + (i*m):1 + N + n * N + (i*m)+m] = B_d