538 lines
48 KiB
Plaintext
538 lines
48 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# 1 System Modelling\n",
|
|
"\n",
|
|
"This notebook contains the theory on using the vehicle Kinematics Equations to derive the linearized state space model"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:06.003291Z",
|
|
"start_time": "2024-10-23T07:18:05.220024Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from scipy.integrate import odeint\n",
|
|
"from scipy.interpolate import interp1d\n",
|
|
"import cvxpy as cp\n",
|
|
"\n",
|
|
"import matplotlib.pyplot as plt\n",
|
|
"\n",
|
|
"plt.style.use(\"ggplot\")\n",
|
|
"\n",
|
|
"import time"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## kinematics model equations\n",
|
|
"\n",
|
|
"The variables of the model are:\n",
|
|
"\n",
|
|
"* $x$ coordinate of the robot\n",
|
|
"* $y$ coordinate of the robot\n",
|
|
"* $v$ velocity of the robot\n",
|
|
"* $\\theta$ heading of the robot\n",
|
|
"\n",
|
|
"The inputs of the model are:\n",
|
|
"\n",
|
|
"* $a$ acceleration of the robot\n",
|
|
"* $\\delta$ steering of the robot\n",
|
|
"\n",
|
|
"These are the differential equations f(x,u) of the model:\n",
|
|
"\n",
|
|
"$\\dot{x} = f(x,u)$\n",
|
|
"\n",
|
|
"* $\\dot{x} = v\\cos{\\theta}$ \n",
|
|
"* $\\dot{y} = v\\sin{\\theta}$\n",
|
|
"* $\\dot{v} = a$\n",
|
|
"* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n",
|
|
"\n",
|
|
"Discretize with forward Euler Integration for time step dt:\n",
|
|
"\n",
|
|
"${x_{t+1}} = x_{t} + f(x,u)dt$\n",
|
|
"\n",
|
|
"* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n",
|
|
"* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n",
|
|
"* ${v_{t+1}} = v_{t} + a_tdt$\n",
|
|
"* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n",
|
|
"\n",
|
|
"----------------------\n",
|
|
"\n",
|
|
"The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
|
|
"\n",
|
|
"$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
|
|
"\n",
|
|
"This can be rewritten usibg the State Space model form Ax+Bu :\n",
|
|
"\n",
|
|
"$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
|
|
"\n",
|
|
"Where:\n",
|
|
"\n",
|
|
"$\n",
|
|
"A =\n",
|
|
"\\quad\n",
|
|
"\\begin{bmatrix}\n",
|
|
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
|
|
"\\end{bmatrix}\n",
|
|
"\\quad\n",
|
|
"=\n",
|
|
"\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n",
|
|
"$\n",
|
|
"\n",
|
|
"and\n",
|
|
"\n",
|
|
"$\n",
|
|
"B = \n",
|
|
"\\quad\n",
|
|
"\\begin{bmatrix}\n",
|
|
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
|
|
"\\end{bmatrix}\n",
|
|
"\\quad\n",
|
|
"= \n",
|
|
"\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n",
|
|
"$\n",
|
|
"\n",
|
|
"are the *Jacobians*.\n",
|
|
"\n",
|
|
"\n",
|
|
"\n",
|
|
"So the discretized model is given by:\n",
|
|
"\n",
|
|
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n",
|
|
"\n",
|
|
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
|
|
"\n",
|
|
"The LTI-equivalent kinematics model is:\n",
|
|
"\n",
|
|
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
|
|
"\n",
|
|
"with:\n",
|
|
"\n",
|
|
"$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
|
|
"\n",
|
|
"$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
|
|
"\n",
|
|
"$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"-----------------\n",
|
|
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
|
|
"\n",
|
|
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
|
|
"\n",
|
|
"Typically it is possible to assume that the system is operating about some nominal\n",
|
|
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
|
|
"\n",
|
|
"Recall that the Taylor Series expansion of f(x) around the\n",
|
|
"point $\\bar{x}$ is given by:\n",
|
|
"\n",
|
|
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
|
|
"\n",
|
|
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
|
|
"\n",
|
|
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
|
|
"is given by:\n",
|
|
"\n",
|
|
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
|
|
"\n",
|
|
"Where:\n",
|
|
"\n",
|
|
"$ A =\n",
|
|
"\\quad\n",
|
|
"\\begin{bmatrix}\n",
|
|
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
|
|
"\\end{bmatrix}\n",
|
|
"\\quad\n",
|
|
"$ and $ B = \\quad\n",
|
|
"\\begin{bmatrix}\n",
|
|
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
|
|
"\\end{bmatrix}\n",
|
|
"\\quad $\n",
|
|
"\n",
|
|
"Then:\n",
|
|
"\n",
|
|
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
|
|
"\n",
|
|
"-----------------"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## ODE Model\n",
|
|
"Motion Prediction: using scipy intergration"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:06.008037Z",
|
|
"start_time": "2024-10-23T07:18:06.005433Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"# Define process model\n",
|
|
"# This uses the continuous model\n",
|
|
"def kinematics_model(x, t, u):\n",
|
|
" \"\"\"\n",
|
|
" Returns the set of ODE of the vehicle model.\n",
|
|
" \"\"\"\n",
|
|
"\n",
|
|
" L = 0.3 # vehicle wheelbase\n",
|
|
" dxdt = x[2] * np.cos(x[3])\n",
|
|
" dydt = x[2] * np.sin(x[3])\n",
|
|
" dvdt = u[0]\n",
|
|
" dthetadt = x[2] * np.tan(u[1]) / L\n",
|
|
"\n",
|
|
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
|
|
"\n",
|
|
" return dqdt\n",
|
|
"\n",
|
|
"\n",
|
|
"def predict(x0, u):\n",
|
|
" \"\"\" \"\"\"\n",
|
|
"\n",
|
|
" x_ = np.zeros((N, T + 1))\n",
|
|
"\n",
|
|
" x_[:, 0] = x0\n",
|
|
"\n",
|
|
" # solve ODE\n",
|
|
" for t in range(1, T + 1):\n",
|
|
"\n",
|
|
" tspan = [0, DT]\n",
|
|
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
|
|
"\n",
|
|
" x0 = x_next[1]\n",
|
|
" x_[:, t] = x_next[1]\n",
|
|
"\n",
|
|
" return x_"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:06.167910Z",
|
|
"start_time": "2024-10-23T07:18:06.008459Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "NameError",
|
|
"evalue": "name 'M' is not defined",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
|
|
"\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)",
|
|
"File \u001B[0;32m<timed exec>:1\u001B[0m\n",
|
|
"\u001B[0;31mNameError\u001B[0m: name 'M' is not defined"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"%%time\n",
|
|
"\n",
|
|
"u_bar = np.zeros((M, T))\n",
|
|
"u_bar[0, :] = 0.2 # m/ss\n",
|
|
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
|
|
"\n",
|
|
"x0 = np.zeros(N)\n",
|
|
"x0[0] = 0\n",
|
|
"x0[1] = 1\n",
|
|
"x0[2] = 0\n",
|
|
"x0[3] = np.radians(0)\n",
|
|
"\n",
|
|
"x_bar = predict(x0, u_bar)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:06.382763Z",
|
|
"start_time": "2024-10-23T07:18:06.166338Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "NameError",
|
|
"evalue": "name 'x_bar' is not defined",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
|
|
"\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)",
|
|
"Cell \u001B[0;32mIn[4], line 3\u001B[0m\n\u001B[1;32m 1\u001B[0m \u001B[38;5;66;03m# plot trajectory\u001B[39;00m\n\u001B[1;32m 2\u001B[0m plt\u001B[38;5;241m.\u001B[39msubplot(\u001B[38;5;241m2\u001B[39m, \u001B[38;5;241m2\u001B[39m, \u001B[38;5;241m1\u001B[39m)\n\u001B[0;32m----> 3\u001B[0m plt\u001B[38;5;241m.\u001B[39mplot(x_bar[\u001B[38;5;241m0\u001B[39m, :], x_bar[\u001B[38;5;241m1\u001B[39m, :])\n\u001B[1;32m 4\u001B[0m plt\u001B[38;5;241m.\u001B[39mplot(np\u001B[38;5;241m.\u001B[39mlinspace(\u001B[38;5;241m0\u001B[39m, \u001B[38;5;241m10\u001B[39m, T \u001B[38;5;241m+\u001B[39m \u001B[38;5;241m1\u001B[39m), np\u001B[38;5;241m.\u001B[39mzeros(T \u001B[38;5;241m+\u001B[39m \u001B[38;5;241m1\u001B[39m), \u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mb-\u001B[39m\u001B[38;5;124m\"\u001B[39m)\n\u001B[1;32m 5\u001B[0m plt\u001B[38;5;241m.\u001B[39maxis(\u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mequal\u001B[39m\u001B[38;5;124m\"\u001B[39m)\n",
|
|
"\u001B[0;31mNameError\u001B[0m: name 'x_bar' is not defined"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"text/plain": "<Figure size 640x480 with 1 Axes>",
|
|
"image/png": "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"
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"# plot trajectory\n",
|
|
"plt.subplot(2, 2, 1)\n",
|
|
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
|
|
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
|
|
"plt.axis(\"equal\")\n",
|
|
"plt.ylabel(\"y\")\n",
|
|
"plt.xlabel(\"x\")\n",
|
|
"\n",
|
|
"plt.subplot(2, 2, 2)\n",
|
|
"plt.plot(np.degrees(x_bar[2, :]))\n",
|
|
"plt.ylabel(\"theta(t) [deg]\")\n",
|
|
"# plt.xlabel('time')\n",
|
|
"\n",
|
|
"\n",
|
|
"plt.tight_layout()\n",
|
|
"plt.show()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## State Space Linearized Model"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 17,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:54.770415Z",
|
|
"start_time": "2024-10-23T07:18:54.762519Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"\"\"\"\n",
|
|
"Control problem statement.\n",
|
|
"\"\"\"\n",
|
|
"\n",
|
|
"N = 4 # number of state variables\n",
|
|
"M = 2 # number of control variables\n",
|
|
"T = 40 # Prediction Horizon\n",
|
|
"DT = 0.2 # discretization step"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 18,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:18:55.153233Z",
|
|
"start_time": "2024-10-23T07:18:55.151535Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"def get_linear_model(x_bar, u_bar):\n",
|
|
" \"\"\"\n",
|
|
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
|
|
" \"\"\"\n",
|
|
"\n",
|
|
" L = 0.3 # vehicle wheelbase\n",
|
|
"\n",
|
|
" x = x_bar[0]\n",
|
|
" y = x_bar[1]\n",
|
|
" v = x_bar[2]\n",
|
|
" theta = x_bar[3]\n",
|
|
"\n",
|
|
" a = u_bar[0]\n",
|
|
" delta = u_bar[1]\n",
|
|
"\n",
|
|
" A = np.zeros((N, N))\n",
|
|
" A[0, 2] = np.cos(theta)\n",
|
|
" A[0, 3] = -v * np.sin(theta)\n",
|
|
" A[1, 2] = np.sin(theta)\n",
|
|
" A[1, 3] = v * np.cos(theta)\n",
|
|
" A[3, 2] = v * np.tan(delta) / L\n",
|
|
" A_lin = np.eye(N) + DT * A\n",
|
|
"\n",
|
|
" B = np.zeros((N, M))\n",
|
|
" B[2, 0] = 1\n",
|
|
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
|
|
" B_lin = DT * B\n",
|
|
"\n",
|
|
" f_xu = np.array(\n",
|
|
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
|
|
" ).reshape(N, 1)\n",
|
|
" C_lin = DT * (\n",
|
|
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
|
|
" )\n",
|
|
"\n",
|
|
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 21,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:19:24.277670Z",
|
|
"start_time": "2024-10-23T07:19:24.269670Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"CPU times: user 4.69 ms, sys: 3.46 ms, total: 8.15 ms\n",
|
|
"Wall time: 5.45 ms\n"
|
|
]
|
|
},
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:17: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" A[0, 2] = np.cos(theta)\n",
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:18: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" A[0, 3] = -v * np.sin(theta)\n",
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:19: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" A[1, 2] = np.sin(theta)\n",
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:20: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" A[1, 3] = v * np.cos(theta)\n",
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:21: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" A[3, 2] = v * np.tan(delta) / L\n",
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_16440/2229978461.py:26: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"%%time\n",
|
|
"\n",
|
|
"u_bar = np.zeros((M, T))\n",
|
|
"u_bar[0, :] = 0.2 # m/s\n",
|
|
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
|
|
"\n",
|
|
"x0 = np.zeros(N)\n",
|
|
"x0[0] = 0\n",
|
|
"x0[1] = 1\n",
|
|
"x0[2] = 0\n",
|
|
"x0[3] = np.radians(0)\n",
|
|
"\n",
|
|
"x_bar = np.zeros((N, T + 1))\n",
|
|
"x_bar[:, 0] = x0\n",
|
|
"\n",
|
|
"for t in range(1, T + 1):\n",
|
|
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
|
|
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
|
|
"\n",
|
|
" A, B, C = get_linear_model(xt, ut)\n",
|
|
"\n",
|
|
" xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
|
|
"\n",
|
|
" x_bar[:, t] = np.squeeze(xt_plus_one)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 22,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-23T07:19:24.878011Z",
|
|
"start_time": "2024-10-23T07:19:24.817493Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": "<Figure size 640x480 with 2 Axes>",
|
|
"image/png": "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"
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"# plot trajectory\n",
|
|
"plt.subplot(2, 2, 1)\n",
|
|
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
|
|
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
|
|
"plt.axis(\"equal\")\n",
|
|
"plt.ylabel(\"y\")\n",
|
|
"plt.xlabel(\"x\")\n",
|
|
"\n",
|
|
"plt.subplot(2, 2, 2)\n",
|
|
"plt.plot(np.degrees(x_bar[2, :]))\n",
|
|
"plt.ylabel(\"theta(t)\")\n",
|
|
"# plt.xlabel('time')\n",
|
|
"\n",
|
|
"\n",
|
|
"plt.tight_layout()\n",
|
|
"plt.show()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"The results are the same as expected, so the linearized model is equivalent as expected."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"start_time": "2024-10-23T07:18:06.387396Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": []
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"name": "python3",
|
|
"language": "python",
|
|
"display_name": "Python 3 (ipykernel)"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 3
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython3",
|
|
"version": "3.8.5"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|