two_wheel.py 文件源码

def two_wheel_3d_model(x, u, dt):
    """Two wheel 3D motion model

    Parameters
    ----------
    x :

    u :

    dt :


    Returns
    -------

    """
    g1 = x[0, 0] + u[0] * cos(x[3, 0]) * dt
    g2 = x[1, 0] + u[0] * sin(x[3, 0]) * dt
    g3 = x[2, 0] + u[1] * dt
    g4 = x[3, 0] + u[2] * dt

    return np.array([g1, g2, g3, g4])