import sympy as sp
def get_equations(self):
"""
:return: Functions to calculate A, B and f given state x and input u
"""
f = sp.zeros(3, 1) # Matrix([[0], [0], [0]])
x = sp.Matrix(sp.symbols('x y theta', real=True)) # Matrix([[x], [y], [theta]])
u = sp.Matrix(sp.symbols('v w', real=True)) # Matrix([[v], [w]])
f[0, 0] = u[0, 0] * sp.cos(x[2, 0])
f[1, 0] = u[0, 0] * sp.sin(x[2, 0])
f[2, 0] = u[1, 0]
f = sp.simplify(f)
A = sp.simplify(f.jacobian(x))
B = sp.simplify(f.jacobian(u))
f_func = sp.lambdify((x, u), f, 'numpy')
A_func = sp.lambdify((x, u), A, 'numpy')
B_func = sp.lambdify((x, u), B, 'numpy')
return f_func, A_func, B_func
jacobian
\[ f.jacobian(x) = \nabla_x f(x, u) = \begin{bmatrix} \partial_{x} f_1 (x, u) & \partial_{y} f_1 (x, u) & \partial_{\theta} f_1 (x, u) \\ \partial_{x} f_2 (x, u) & \partial_{y} f_2 (x, u) & \partial_{\theta} f_2 (x, u) \\ \partial_{x} f_3 (x, u) & \partial_{y} f_3 (x, u) & \partial_{\theta} f_3 (x, u) \end{bmatrix} \; with \; x=\begin{bmatrix} x\\ y \\ \theta \end{bmatrix} \]
'STUDY' 카테고리의 다른 글
numpy / matplotlib (0) | 2024.07.05 |
---|