if k >= 5:
            q_v = np.hstack((q_0[k], q_1[k], q_2[k]))
            q_w = np.hstack((q_0[k - 1], q_1[k - 1], q_2[k - 1]))
        elif k >= 3:
            q_v = np.hstack((q_0[k], q_1[k]))