two_wheel.py 文件源码

def two_wheel_3d_deriv():
    """ """
    x1, x2, x3, x4, x5, x6, x7 = sympy.symbols("x1,x2,x3,x4,x5,x6,x7")
    dt = sympy.symbols("dt")

    # x1 - x
    # x2 - y
    # x3 - z
    # x4 - theta
    # x5 - v
    # x6 - omega
    # x7 - vz

    # x, y, z, theta, v, omega, vz
    f1 = x1 + x5 * sympy.cos(x4) * dt
    f2 = x2 + x5 * sympy.sin(x4) * dt
    f3 = x3 + x7 * dt
    f4 = x4 + x6 * dt
    f5 = x5
    f6 = x6
    f7 = x7

    F = sympy.Matrix([f1, f2, f3, f4, f5, f6, f7])
    pprint(F.jacobian([x1, x2, x3, x4, x5, x6, x7]))