two_wheel.py 文件源码

def two_wheel_2d_model(x, u, dt):
    """Two wheel 2D motion model

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

    u :

    dt :


    Returns
    -------

    """
    gdot = np.array([[u[0, 0] * cos(x[2, 0]) * dt],
                     [u[0, 0] * sin(x[2, 0]) * dt],
                     [u[1, 0] * dt]])

    return x + gdot