{ "cells": [ { "cell_type": "code", "execution_count": 77, "metadata": {}, "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", "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", "* $\\theta$ heading of the robot\n", "* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n", "* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n", "\n", "The inputs of the model are:\n", "\n", "* $v$ linear velocity of the robot\n", "* $w$ angular velocity of the robot\n", "\n", "These are the differential equations f(x,u) of the model:\n", "\n", "* $\\dot{x} = v\\cos{\\theta}$ \n", "* $\\dot{y} = v\\sin{\\theta}$\n", "* $\\dot{\\theta} = w$\n", "* $\\dot{\\psi} = -w$\n", "* $\\dot{cte} = v\\sin{\\psi}$\n", "\n", "The **Continuous** State Space Model is\n", "\n", "$ {\\dot{x}} = Ax + Bu $\n", "\n", "with:\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 \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "=\n", "\\quad\n", "\\begin{bmatrix}\n", "0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n", "0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n", "0 & 0 & 0 & 0 & 0 \\\\\n", "0 & 0 & 0 & 0 & 0 \\\\\n", "0 & 0 & 0 & vcos(\\psi) & 0 \n", "\\end{bmatrix}\n", "\\quad $\n", "\n", "\n", "$ B = \\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "=\n", "\\quad\n", "\\begin{bmatrix}\n", "\\cos{\\theta_t} & 0 \\\\\n", "\\sin{\\theta_t} & 0 \\\\\n", "0 & 1 \\\\\n", "0 & -1 \\\\\n", "\\sin{(\\psi_t)} & 0 \n", "\\end{bmatrix}\n", "\\quad $\n", "\n", "discretize with forward Euler Integration for time step 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", "* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n", "* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n", "* ${cte_{t+1}} = cte_{t} + v_t\\sin{\\psi}*dt$\n", "\n", "The **Discrete** State Space Model is then:\n", "\n", "${x_{t+1}} = Ax_t + Bu_t $\n", "\n", "with:\n", "\n", "$\n", "A = \\quad\n", "\\begin{bmatrix}\n", "1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n", "0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n", "0 & 0 & 1 & 0 & 0 \\\\\n", "0 & 0 & 0 & 1 & 0 \\\\\n", "0 & 0 & 0 & vcos{(\\psi)}dt & 1 \n", "\\end{bmatrix}\n", "\\quad\n", "$\n", "\n", "$\n", "B = \\quad\n", "\\begin{bmatrix}\n", "\\cos{\\theta_t}dt & 0 \\\\\n", "\\sin{\\theta_t}dt & 0 \\\\\n", "0 & dt \\\\\n", "0 & -dt \\\\\n", "\\sin{\\psi_t}dt & 0 \n", "\\end{bmatrix}\n", "\\quad\n", "$\n", "\n", "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n", "\n", "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n", "\n", "So:\n", "\n", "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(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 Discrete linearized kinematics model is\n", "\n", "$ x_{t+1} = A'x_t + B' u_t + C' $\n", "\n", "with:\n", "\n", "$ A' = I+dtA $\n", "\n", "$ B' = dtB $\n", "\n", "$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\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": [ "### Kinematics Model" ] }, { "cell_type": "code", "execution_count": 78, "metadata": {}, "outputs": [], "source": [ "# Control problem statement.\n", "\n", "N = 5 #number of state variables\n", "M = 2 #number of control variables\n", "T = 20 #Prediction Horizon\n", "dt = 0.25 #discretization step" ] }, { "cell_type": "code", "execution_count": 79, "metadata": {}, "outputs": [], "source": [ "def get_linear_model(x_bar,u_bar):\n", " \"\"\"\n", " \"\"\"\n", " \n", " x = x_bar[0]\n", " y = x_bar[1]\n", " theta = x_bar[2]\n", " psi = x_bar[3]\n", " cte = x_bar[4]\n", " \n", " v = u_bar[0]\n", " w = u_bar[1]\n", " \n", " A = np.zeros((N,N))\n", " A[0,2]=-v*np.sin(theta)\n", " A[1,2]=v*np.cos(theta)\n", " A[4,3]=v*np.cos(psi)\n", " A_lin=np.eye(N)+dt*A\n", " \n", " B = np.zeros((N,M))\n", " B[0,0]=np.cos(theta)\n", " B[1,0]=np.sin(theta)\n", " B[2,1]=1\n", " B[3,1]=-1\n", " B[4,0]=np.sin(psi)\n", " B_lin=dt*B\n", " \n", " f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n", " C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", " \n", " return A_lin,B_lin,C_lin" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using scipy intergration" ] }, { "cell_type": "code", "execution_count": 80, "metadata": {}, "outputs": [], "source": [ "# Define process model\n", "def kinematics_model(x,t,u):\n", " \"\"\"\n", " \"\"\"\n", "\n", " dxdt = u[0]*np.cos(x[2])\n", " dydt = u[0]*np.sin(x[2])\n", " dthetadt = u[1]\n", " dpsidt = -u[1]\n", " dctedt = u[0]*np.sin(-x[3])\n", "\n", " dqdt = [dxdt,\n", " dydt,\n", " dthetadt,\n", " dpsidt,\n", " dctedt]\n", "\n", " return dqdt\n", "\n", "def predict(x0,u):\n", " \"\"\"\n", " \"\"\"\n", " \n", " x_bar = np.zeros((N,T+1))\n", " \n", " x_bar[:,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,\n", " x0,\n", " tspan,\n", " args=(u[:,t-1],))\n", "\n", " x0 = x_next[1]\n", " x_bar[:,t]=x_next[1]\n", " \n", " return x_bar" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Validate the model, here the status w.r.t a straight line with constant heading 0" ] }, { "cell_type": "code", "execution_count": 81, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 7.22 ms, sys: 0 ns, total: 7.22 ms\n", "Wall time: 5.83 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 1 #m/s\n", "u_bar[1,:] = np.radians(-10) #rad/s\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = np.radians(0)\n", "x0[3] = 0\n", "x0[4] = 1\n", "\n", "x_bar=predict(x0,u_bar)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Check the model prediction" ] }, { "cell_type": "code", "execution_count": 82, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "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", "plt.subplot(2, 2, 3)\n", "plt.plot(np.degrees(x_bar[3,:]))\n", "plt.ylabel('psi(t) [deg]')\n", "#plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", "plt.plot(x_bar[4,:])\n", "plt.ylabel('cte(t)')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "the results seems valid:\n", "* the cte is correct\n", "* theta error is correct" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using the state space model" ] }, { "cell_type": "code", "execution_count": 83, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 2.58 ms, sys: 0 ns, total: 2.58 ms\n", "Wall time: 1.89 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 1 #m/s\n", "u_bar[1,:] = np.radians(-10) #rad/s\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = np.radians(0)\n", "x0[3] = 0\n", "x0[4] = 1\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(5,1)\n", " ut=u_bar[:,t-1].reshape(2,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": 84, "metadata": {}, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAAaIAAAEYCAYAAAAeWvJ8AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzdeVxU5f7A8c9zQEVEdvct13LLDdMsxQXLbCNLzdJy6Za5kLaZlomZaxqJW5vh1i1bjPZF9LqU18IUzfSqqKUmhgIiiChwnt8f8xMlQBBhzszwfb9evS4zc+acr8/l8J15lu+jtNYaIYQQwiKG1QEIIYQo3yQRCSGEsJQkIiGEEJaSRCSEEMJSkoiEEEJYShKREEIIS7lbHcC1On78eIHPBwYGcurUKTtH45ikLS4prC1q165tQTTOQe6xoklbXFKSe8zpE5EQwiYuLo6oqChM06RXr16EhobmeT0rK4uFCxdy6NAhqlatyrhx46hevbpF0QpxiXTNCeECTNNk6dKlTJo0iYiICH766SeOHTuW55j169dTpUoVFixYwJ133sn7779vUbRC5CWJSAgXEB8fT82aNalRowbu7u506dKF2NjYPMds27aN7t27A9C5c2d2795NSQqr6Mxz5Mx9kfM7Y4s+WIhikK45IVxAcnIyAQEBuY8DAgI4cOBAoce4ubnh6elJWloa3t7eeY6LiYkhJiYGgFmzZhEYGJjn9eyEY5xOP8Pp8KfwvGsAXoOfRFWqVBb/LKfh7u6er53Kq5K0hSQiIUQeISEhhISE5D7ON/BcwQM9aS6Vv/mIjK8+IuPX/2KMGI9q0MTOkToOmaxwSUkmK0jXnBAuwN/fn6SkpNzHSUlJ+Pv7F3pMTk4OGRkZVK1atUTXUxUr4f3YeIzxU+FcBubM5zC//gidk1Pyf4QotyQRCeECGjduTEJCAomJiWRnZ7NlyxaCgoLyHNOhQwc2bNgAwNatW2nZsiVKqWu6rmrRDiN8Aap9F3T0KszXJqITC57uLURhJBEJ4QLc3NwYPnw406dPZ/z48dx8883Uq1eP1atXs23bNgB69uxJeno6Y8eO5auvvuLhhx8ulWurKl4Yjz+HeuwZSDiK+co4zE3flWgihCiflLPvRySL7YombXGJLGi9eldzj+nkU5jL5sPendA6COPRsSgfP3uEaSm5xy6RMSIhhKWUfyDGuKmoBx+H/+3CDB+L3v5fq8MSDk4SkRCiVCnDwOh1F8bkCPCvhrlkJmbUfPS5DKtDEw5KEpEQokyoWvUwJs5B3TkA/d//YE4NQ+/fbXVYwgFJIhJClBnlXgEjdDDGhFng5oY590XMT5ahs7KsDk04EElEQogypxrfgDH5DVTX29Hfr8Gc8Qz62GGrwxIOQhKREMIulEdljCGjMMZOhjOnMac/g/n9GrQpi2DLO0lEQgi7Ujd2xAhfAK2D0J8sw5z3EvrU31aHJSwkiUgIYXeqqg/GkxNRQ5+CI4cwp4Zhblkni2DLKUlEQghLKKUwbumFMSUS6jVER83HfHMWOu2M1aEJO5NEJISwlAqsgfHsdNQDQ2FnLGb4GPRv26wOS9iRJCIhhOWU4YZxez+MF+dBVR/MyFcwVy1Gn8+0OjRhBw6zH9GpU6dYtGgRp0+fRilFSEgIffv2tTosIYQdqXoNMV6ch45+H702Gr13J8bw8ajGN1gdmihDDpOI3NzcGDJkCI0aNeLcuXO88MIL3HjjjdStW9fq0IRwaOnp6URERHDy5EmqVavG+PHj8fLyynPMH3/8wTvvvMO5c+cwDIN+/frRpUsXiyK+MlWhIqr/MPSNHTHfi8Cc/QLqzv6oOwei3B3mT5YoRQ7TNefn50ejRo0AqFy5MnXq1CE5OdniqIRwfNHR0bRu3ZrIyEhat25NdHR0vmMqVqzImDFjeP3115k0aRLLli3j7NmzFkRbfOr6VhhTIlGdu6O/Wo0563l0wjGrwxJlwCE/XiQmJnL48GGaNMm/9XBMTAwxMTEAzJo1q9C90WUP+UukLS5xxbaIjY0lPDwcgODgYMLDwxk8eHCeYy4vwe/v74+Pjw9nzpyhSpUq9gz1qinPKqjh49BtbsJctQhz2jjUA0NR3fuiDIf5HC2ukcMloszMTObNm8fQoUPx9PTM93pISAghISG5jwvbA0T2B7lE2uISV9yPKDU1FT8/254/vr6+pKamXvH4+Ph4srOzqVGjRoGvO+SHvdvvIadjF84snsmFD96mwp4deI99CbeAava5fhFc8QNOSZWkLRwqEWVnZzNv3jy6du1Kp06drA5HCIcxbdo0Tp8+ne/5Bx98MM9jpdQVt/9OSUlhwYIFjB49GqOQbxSO/GFPP/ECatP3XPhoKafCHkYNfhKjY1e7xlAQ+bB3SUk+7DlMItJa8+abb1KnTh3uuusuq8MRwqFMnjy50Nd8fHxISUnBz8+PlJQUvL29CzwuIyODWbNmMWjQIJo1a1ZWoZYppRQquA/6hhsxl76Ofvs1zLhfUA89gariVfQJhENymE7Wffv2sWnTJnbv3s1zzz3Hc889x/bt260OSwiHFxQUxMaNGwHYuHEjHTt2zHdMdnY2c+fOpVu3bnTu3NneIZY6VaM2xoTZqHsfQv/6o22vo707rQ5LlJDDfCO64YYb+Oijj6wOQwinExoaSkREBOvXr8+dvg1w8OBB1q5dy8iRI9myZQt79+4lLS2NDRs2ADB69Giuu+466wK/RsrNDXXXg+hWHTCXvo75+mRUyD2o+4agKlayOjxxFZR28iqDx48fL/B56bO9RNriElecrFDWnOEe0+fPo9csR6//CmrVw3jsaVT9xna7viO1hdWceoxICFd05swZNm3axPbt2/nzzz/JyMjA09OTBg0a0LZtW7p3717omI4oPlWpEmrQ47ZFsMvmY854FnX3INQd96MMN6vDE0WQRCREGXn//ff58ccfadeuHT179qROnTpUrlyZc+fO8ddff7Fnzx4mTJjArbfeysMPP2x1uC5BtWyHEb4AvWoJOnoV+rdtthJB1WtZHZq4AklEQpSRgIAAIiMjqVChQr7XGjZsyK233sqFCxdYv369BdG5LlWlKjz+HLTthH7/TcxXnkINGIHqetsVp7YL6zjMrDkhXE2fPn1yk1BBa4DANqW6T58+9gyrXFBKYXQKxgiPhEbXo1cuwlz4KvpMitWhiQJIIhLCDp566qkCn784w02UDeVfDWPcVNTAx2DvTswpY9E7tlodlvgHl+ya0//bxQVvb/TFUieGG1SqBJU8oFJlqOqNqlDR2iBFuVLQ5NSMjIxCqxuI0qMMAxVyD7pFW9s078UzULeEoAY+hqqcv4yYsD+XTETmG1NIycm58kGVPaGqL/gHoqrVhMAaqBq1oXYDqF4L5SYzbcS1e/LJJwG4cOFC7s8Xpaenc8stt1gRVrmkatfHmPga+ssP0d9+iv7fLttEhmYtrQ6t3HPJRGQ8/So+Pt6Xij/m5MD5TNtuj+fPQdoZSEuF1BR08kl03M+QlkruZ1b3ClCnAaphM2h0Parx9VCtlgx0iqs2duxYtNbMnDmTsWPH5nnN19dX1i/ZmXKvgLpvCLp1kG2vo7mTULf3Q93zEKqASSXCPmRB6//T5zPhxF/ov/6Av/5EHzkEhw/YEheAXyCqeRto3gbVsh2qqk8pRG8fstjuEqsWtJ4/f55KlZxztb8zLGgtCZ15Dv3RUvTmH6BuQ9si2DoNSnQuZ2+L0iQLWq+BquQBDRqjGlxaja3NHDh+FB2/F/2/nbZvTlvWoZUBTZuj2nVGteuCcpBS9MKxfPPNN/Tu3ZsKFSoUmoSysrJYu3Ytffv2tXN0QnlURj0yBt2mE+bySMxXx9vKA4XcK3sd2ZkkoitQhhvUvQ5V9zrofoctMf15CL3rF/SOrejVS9Grl0KzVqjO3VEdbkF5OvZGY8J+Tp8+TVhYGO3ataNFixbUrl0bDw8PMjMzOX78OHv27GHHjh0EBwdbHWq5ptp0xJi6EHPFQvTHUehd2zCGPYUKqG51aOWGdM1dA514HP3LZvTWDfD3X1CxIuqmYNvukQ3sV+eqKNJtcIm9u+bOnDnDhg0biIuL48iRI5w9exYvLy/q169Pu3btCA4OpmrVqmVy7dLiql1z/6S1Rv8Ug/7wXTAUatATtg+YxRgbdrW2uBYlucckEZUCrTX8cQC9+Qf0zxvhwnlodD3G7fdB206W17qSm+QSVyx6mp6eTkREBCdPnsytvu3lVfDePBkZGTz99NN07NiRESNGFOv8jnCP2ZM+eQLzvTcgfg906IIxeBTK68r1AF21LUqiJPeYdISWAqUUqmEzjEfGYLwWhXrwX5CWirlkFubLYzA3/4DOzrY6TOGioqOjad26NZGRkbRu3Zro6OhCj129ejXNmze3Y3TOR1WrifHcdNT9j0LcL5jhY9G//Wp1WC5NxohKmfL0QvW6G92jL/rX/6K/+wS9YiH6209sU0Rv6mr5NyRhfxkZGXz88cfs2bOHtLS0PAtclyxZck3njo2NJTw8HIDg4GDCw8MZPHhwvuMOHTpEamoqbdu25eDBg9d0TVenDDdUn/vRLdvbFsFGTkV1vwP1wDDbxCZRquQbURlRhhtGx1sxXorAGDMZKlVGL30d85Vx6D07rA5P2Nm7777L4cOHeeCBB0hPT2f48OEEBgZy5513XvO5U1NT8fPzA2xrk3LXz13GNE1WrFjBkCFDrvl65Ymq1xDjxXmo20LRG7+z3b+H9lkdlsuRb0RlTCkFbTpitO6A/nUL+rMVmBFToM1NGP2H26o5CJe3a9cuIiIiqFq1KoZh0LFjRxo3bszs2bO56667inz/tGnTCiyc+uCDD+Z5rJQqcHD9hx9+oF27dgQEBBR5rZiYGGJiYgCYNWsWgYGBBR7n7u5e6Gsu58nnudA1hNTIaZizX6DKA49Spf9QlLvtT2i5aosilKQtHCoRxcXFERUVhWma9OrVi9DQUKtDKjXKMFAdb0W37YRe9wX6q48ww8eg7hyA6nM/yl1WdbsyrTWenra6Zh4eHmRkZODr68uJEyeK9f7JkycX+pqPjw8pKSn4+fmRkpJS4EZ7+/fvZ+/evfzwww9kZmaSnZ2Nh4dHgfsghYSEEBISkvu4sEH4cjdAX7M+vPQG6sO3OfvRe5z9eRPGiKdRteqWv7a4gpJMVnALv9i5XAzLli3D19cXX1/fEgV4JaZpMmPGDF588UXuu+8+oqKiaNGiRZG7V6alpRX4vKenJxkZGaUe57VSbm6oJi1QXXpB8kn0+q/R2/+Lqt8Y5V82n6gctS2sUFhblPUU6p07dxIYGEiNGjWIj49nx44d7Nq1i+zs7Dx/9Evi1KlTJCQkcMMNN/D9999TrVo1brzxxjzHdOrUibvuuos777wTHx8fvL29efTRR4t1fme7x8qSqlAR1e5mVJ366K0b0P/5Gip74tmiDefOnbM6PIdQknvsqr4RmabJ9OnT8fb2pmvXrnTt2rVYX/WLIz4+npo1a1KjRg0AunTpQmxsLHXr1r3qc738sjcHDriTlVU6sZWNAGAWOj0Nth2Hz7IgIBsCqpd6TbsKFRy9LeynQwc3Jk60/3WfeOKJ3AkKw4YN44MPPuDs2bOMGTPmms8dGhpKREQE69evz52+DXDw4EHWrl3LyJEjr/kaIi/V4RaMxs0xl0eiP3ib03vj0A89ifKT+6wkrnodkWma7Nixg82bN7N9+3aaNm1Kt27d6NSpEx4eJZ9NsnXrVuLi4nJvmk2bNnHgwIF8ax3+2X994cKFfOd65hk3fvvNKLD0viPSOTnkJBzDPJ2MquyJe93rUKVYl0wp5TRtUdbatlW89lpWvucrVizbbUEOHDhA06ZN8z0fHx9PkyZNyvTa16q8rSO6Glpr9MZv0R9HgXsF1OBRGB1vtTosS9l9QevRo0eJjIzkyJEjVKxYkVtuuYUBAwbg7+9/1ecqbiL6J1e6SfSvWzBXLoKcbIyhYagOpbNFgDO2RVmxakHro48+yvLly/M9P2zYMKKiosr02tfKle6xsuJ7PoOkeS/D4f2oTsGoh55AeRa8qNjV2aXoaUZGBlu3bmXz5s38+eefdOrUiREjRhAYGMhXX33FjBkzmDt37tWeFn9/f5KSknIfJyUllSihOTPVoQtGw6aYb83BfHM2KuQe1P2XZuYI52OaJvD/n5z//7+L/v77b9xk3yuX4F6nPsaE2ehvPkZ/9SF6/++2enXN21gdmlO4qr9w8+bNY+fOnTRv3pzevXvTsWNHKly2h8cjjzzC0KFDSxRI48aNSUhIIDExEX9/f7Zs2UJYWFiJzuXMlH81jOdmoD9Zho75Av3XnxhPTEBVKZ+frpzdoEGDcn/+51RrwzC477777B2SKCPKzQ1194PoVh1si2Bfn2z7MNnvEdkRughX1TX3xRdf0K1btyvOmruWfVe2b9/O8uXLMU2THj160K9fvyLf48rdBuaWdegViyCwBsaYl1A165ToPK7QFqXF3l1zJ0+eRGtNeHg4U6dORWudu9bH29u7zMemSoMr32Ol5Z9toc+fR38ahf7PN1Crnm2vo/qOUwi5LEnR08u4yk2iD+zBXDwD0BhhU2y7xl4lV2mL0mB10VPTNPNUQnAGrn6PlYbC2kLv3o65LBLSz6DuGYTq08/lS3xJ0VMXpJq2wJg4Bzw8Mee9JOWBnNTZs2eZP38+Dz/8cG6X87Zt2/jwww8tjkyUJdWqPcbUBah2ndGfrcScMxF9sniLmMsTSUROQFWvjTFhNlSriRk5Db1jq9Uhiav0zjvv4OnpyeLFi3H//8knzZo1Y8uWLRZHJsqaqlIV9fhzqMeegeNHMaeG2SryO3dnVKmSROQklK8/xnMzoH4jzLdmSzJyMr/99hvDhg3L0yXn7e1dYIFS4XqUUhidgjHCI6FhM/SKhZiLpqPPpFgdmkOQRORElKcXxripUL+xLRnF/Wx1SKKYPD0985XKOXXqlFONFYlrp/yrYYx/BTVwBPy+AzM8TD5UIonI6SjPKpcloznofb9ZHZIohl69ejFv3jx2796N1pr9+/ezaNEievfubXVows6UYWCE3IsxOQL8AjAXz8BcFonOLF91+y4nicgJKc8qGGEv28aMFr6KPiKbnDm6e++9ly5durB06VJycnJYsmQJQUFB9O3b1+rQhEVU7foYE19D9e2P3rIec+pT6AN7rA7LEjJ924np5JOYsyeAqTGmv4mqWPD6rfLQFsVl9fRtZ1Se77Hiuta20PF7MN97A079jbq9n2035wrOuTWMXUr8CMeh/KthjHsFEhMKTULCcRw/fpw//viDzMzMPM/37NnTooiEo1BNWmC8/Ab6o/fQ332K3r3dtgi2TgOrQ7MLSUROTtWqC7WufqsMYV9r1qzh008/pUGDBvkqj1xrIkpPTyciIoKTJ0/mbgPh5ZW/JNSpU6d48803c2s6Tpw4kerVq1/TtUXpUR6eqEfGoNvchLl8Aear41H3PWIrE2S49iiKJCIh7OCbb75hxowZNGhQ+p9wo6Ojad26NaGhoURHRxMdHc3gwYPzHbdw4UL69evHjTfeSGZmZqnveyVKh2pzE0b4AsyVi9Afv4feFYsxbBwqoJrVoZUZ106zQjiIihUrUqdOyWoFFiU2Npbg4GAAgoODiY2NzXfMsWPHyMnJyd251cPDo8Q1IUXZU96+GKMmoR4dC3/EY04di7n1Py67CFa+EQlRRi5uAQEwcOBA3nvvPfr374+Pj0+e44xr7Ha5vHadr69vgYtkjx8/TpUqVZg7dy6JiYm0bt2ahx9+uMBr/3PzycDAgrewd3d3L/S18qbM2iJ0ENmdu3Em8lWylkZQcU8c3iOfx/D2Kfq9FilJW0giEqKMXL4FxEXr1q3L99zq1auLPNe0adM4ffp0vuf/ubXExcre/2SaJnv37mXOnDkEBgYSERHBhg0bChyfCgkJISQkJPdxYbPBZNbcJWXaFu6V0OPCUd9Hc/7z9zm5ZyfG0LGoVh3K5nrXSGbNCeFAFi5cCNg2xdu6dSs333xznte11vz8c/GqY0yePLnQ13x8fEhJScHPz4+UlBS8vb3zHePv7891111HjRo1ALjpppvYv3+/zNhzEspwQ91xP7plO8z3IjDnT0V174t6YCiqkofV4V0zGSMSooxUq1aNatWqUb16dT799NPcx5c/v2bNmmu+TlBQEBs3bgRg48aNdOzYMd8xTZo0ISMjgzNnzgCwe/du6taV2ZbORtVvhPHiPNRtoeiN32K+Mg59aJ/VYV0z+UYkRBnavXs3ADk5Obk/X/T3339TuXLla75GaGgoERERrF+/Pnf6NsDBgwdZu3YtI0eOxDAMhgwZwiuvvILWmkaNGuXpfhPOQ1WoiOo/HN06CDPqDczZE1B3DkD1HYByd84/6VJZoRyQtrjE3pUVRo8eDdjGWS4fwFVK4evrS2hoKEFBQWVy7dIi91jRrGoLnXEW/eHb6P/+Bxo0sS2CrWntN12nHSNauXIlv/76K+7u7tSoUYNRo0ZRpUoVq8MS4potWrQIsI0XjRkzxuJohKtRnlVQw8fbFsGuXIw5bRzqgWG28SMnWifmEGNEN954I/PmzWPu3LnUqlWLzz77zOqQhChVkoREWVIdbsEIXwDNWqH//Rbm/HD06SSrwyo2h0hEbdq0wc3Nto97s2bNSE5OtjgiIYRwLsrXHyNsCurhkXDgd9teR9t+tDqsYnGIRHS59evX07ZtW6vDEEIIp6OUwujeF2PyfNs2MW/NwVz6Ojoj3erQrshuY0RXWpB3cbrpmjVrcHNzo2vXroWeR1Z9Xz1pi0ukLUR5oGrWwZgwG/3Nx+ivV6P377bVq7vhRqtDK5DDzJrbsGEDa9eu5eWXX76qGlgyo6do0haXyH5EV0/usaI5clvow/sxl0bA33+hQu5F9RuCqlCxzK5XknvMIbrm4uLi+Pzzz5kwYYIUYhRCiFKkGjbDmPwGqkdfdMznmK8+jT5yyOqw8nCI6dtLly4lOzubadOmAdC0aVMef/xxi6MSQgjXoCpVQj00En1jR8xlCzBnPIu69yHU7fehDDerw3OMRLRgwQKrQxBCCJenWnXACI9Er1qCXrPCttfR8PGoajUtjcshuuaEEELYh/LyRj3xPGrE0/DXEcypT2Fu/sHSvY4kEQkhRDmjlMLo3B1jSiRc1wS9YiHmounoM/lnNtuDJCIhhCinVEA1jKenoQaMgN93YIaPRccVb2uS0uQQY0RCiJJLT08nIiKCkydP5lbf9vLyynfcqlWr2L59O1prWrduzbBhw5yqHpkoG8owUL3vRbdoi/nu65iLpqNu7Y0aOALl4WmXGOQbkRBOLjo6mtatWxMZGUnr1q2Jjo7Od8y+ffvYt28fc+fOZd68eRw8eJA9e/ZYEK1wVKpOA4wX59o24PspBnPqU+h4+/yOSCISwsnFxsYSHBwMQHBwMLGxsfmOUUpx4cIFsrOzycrKIicnBx8fH3uHKhyccq+A0e9RjOdmAmDOmYS5ZgU6O6tMrytdc0I4udTUVPz8/ADw9fUlNTU13zHNmjWjZcuWPP7442it6dOnT6E7tEoZravncm0R2A2zbQfS3osk89tPcPvfTnzGTcG9fqMi31qStpBEJIQTuFKtxssppQoc9zlx4gR//fUXb775Zu759u7dS/PmzfMdGxISkmf31sJK1zhyWRt7c9m2GPgvjOtvJHvFQpKeGYbq9wiq190oo/DONKfdGE8IcWWTJ08u9DUfHx9SUlLw8/MjJSUFb2/vfMf88ssvNG3aFA8PDwDatWvH/v37C0xEQlxOte2E0eh6zBUL0R8ttS2CHfoUKqBaqV1DxoiEcHJBQUFs3LgRgI0bN+ZWs79cYGAge/fuJScnh+zsbPbs2UOdOnXsHapwUsrbF2P0i6hHxsDhA5hTwzC3bii1RbCSiIRwcqGhoezatYuwsDB+++03QkNDATh48GBuV1znzp2pUaMGzz77LM899xwNGjQgKCjIyrCFk1FKYXS9DWPKfKhTH730dfTbr6HPpl37uR1lG4iSkhL1RZO2uES2gbh6co8Vrby1hTZz0N9/hv783+DljTE0DNWqPeDE20AIIYRwHspww7jjAYxJr4FnFcz54Zj/fhN9/nyJzieJSAghRImo+o0xJkfYKjP85xvMaePIOrz/qs8jiUgIIUSJqQoVMQaMwHjatp+ccq9w1eeQRCSEEOKaqeZtMF5ZiHu9hlf9XklEQgghSkVJd3uVRCSEEMJSkoiEEEJYyunXEQkhhHBuLvuN6IUXXrA6BIchbXGJtEXpkba8RNrikpK0hcsmIiGEEM5BEpEQQghLuYWHh4dbHURZadSo6E2cygtpi0ukLUqPtOUl0haXXG1byGQFIYQQlpKuOSGEEJZyuR1a4+LiiIqKwjRNevXqlbs3S3k0evRoPDw8MAwDNzc3Zs2aZXVIdrN48WK2b9+Oj48P8+bNAyA9PZ2IiAhOnjxJtWrVGD9+PF5eXhZH6nzK8z0mv1eXnDp1ikWLFnH69GmUUoSEhNC3b9+StYd2ITk5OXrMmDH6xIkTOisrSz/77LP66NGjVodlmVGjRunU1FSrw7DE77//rg8ePKiffvrp3OdWrlypP/vsM6211p999pleuXKlVeE5rfJ+j8nv1SXJycn64MGDWmutMzIydFhYmD569GiJ2sOluubi4+OpWbMmNWrUwN3dnS5duhAbG2t1WMICLVq0yPcpLDY2luDgYACCg4Pld6MEyvs9Jr9Xl/j5+eVOSqhcuTJ16tQhOTm5RO3hUl1zycnJBAQE5D4OCAjgwIEDFkZkvenTpwPQu3dvQkJCLI7GWqmpqfj5+QHg6+tLamqqxRE5H7nH8pPfK0hMTOTw4cM0adKkRO3hUolI5DVt2jT8/f1JTU3l1VdfpXbt2rRo0cLqsByCUgqllNVhCBdTHn+vMjMzmTdvHkOHDsXT0zPPa8VtD5fqmvP39ycpKSn3cVJSEv7+/hZGZK2L/3YfHx86duxIfHy8xRFZy8fHh5SUFABSUlLw9va2OCLnI/dYfuX59yo7O5t58+bRtWtXOnXqBOvNKi4AACAASURBVJSsPVwqETVu3JiEhAQSExPJzs5my5YtBAUFWR2WJTIzMzl37lzuz7t27aJ+/foWR2WtoKAgNm7cCMDGjRvp2LGjxRE5H7nH8iuvv1daa958803q1KnDXXfdlft8SdrD5Ra0bt++neXLl2OaJj169KBfv35Wh2SJv//+m7lz5wKQk5PDrbfeWq7a4o033mDPnj2kpaXh4+PDgAED6NixIxEREZw6dapcTbMtbeX5HpPfq0v+97//8fLLL1O/fv3c7rdBgwbRtGnTq24Pl0tEQgghnItLdc0JIYRwPpKIhBBCWEoSkRBCCEtJIhJCCGEpSURCCCEsJYlICCGEpSQRCSGEsJQkIiGEEJaSROSiTpw4wbBhwzh06BBgq5o8YsQIfv/9d4sjE0KIvCQRuaiaNWvy8MMPs2DBAs6fP8+SJUsIDg6mZcuWVocmhBB5SIkfFzd79mwSExNRSjFz5kwqVKhgdUhCCJGHfCNycb169eLo0aP06dNHkpAQwiFJInJhmZmZLF++nJ49e/Lxxx+Tnp5udUhCCJGPJCIXFhUVRaNGjRg5ciTt27fn7bfftjokIYTIRxKRi4qNjSUuLo5//etfADz66KMcPnyYzZs3WxyZEELkJZMVhBBCWEq+EQkhhLCUJCIhhBCWkkQkhBDCUpKIhBBCWEoSkRBCCEtJIhJCCGEpSURCCCEsJYlICCGEpSQRCSGEsJQkIiGEEJaSRCSEEMJSkoiEEEJYShKREEIIS0kiEkIIYSl3qwO4VsePHy/w+cDAQE6dOmXnaByTtMUlhbVF7dq1LYim9CxevJjt27fj4+PDvHnz8r2utSYqKoodO3ZQqVIlRo0aRaNGjYp1brnHiiZtcUlJ7jH5RiSEC+jevTuTJk0q9PUdO3Zw4sQJIiMjefzxx3n33XftGJ0QV+b034iEKA59Nh29+h2yHxwBnt5Wh1PqWrRoQWJiYqGvb9u2jW7duqGUolmzZpw9e5aUlBT8/Pyu+lo6/Qx6zQrOeHhgZmbmP8AwoIo3VK0KXt4oL2+4+J+PP6pChau+pnBtkoiEy9N74jCj5kPaabI63AxtOlsdkt0lJycTGBiY+zggIIDk5OQCE1FMTAwxMTEAzJo1K8/7AHIwSd69nfMKVAH7O+ucbHT6GTBN2+PLXzQM3KrXwq1OfdzrNMCtTgPc//9nw9f/mv+dVnF3d8/XTuVVSdpCEpFwWfrCefSaFeh1X0LNuhijJ1E56GbOSl/+FYWEhBASEpL7OH9/v4Ga816hYwEK0KYJ5zIg/UzufzotFZJPYp74i5yEY1zY9StkXbj0Rv9AVJMW0KS57X/r1EcZbmXzjyxlMkZ0SUnGiCQRCZek/4zHfPd1OHEM1etuVL9HUBUrWR2WZfz9/fP8cUhKSsLfv+y+gSjDgCpetv9q2P4AqX8co00TUk5BwjF0wlE4tA+9fzf8ssn2LaqyJzS6HtW0Jap1ENRriFL/PItwBZKIhEvROTno7z5Ff/kBVPXFGD8V1aKd1WFZLigoiO+++45bbrmFAwcO4OnpWaLxodKkDAMCqkNAdVSr9oBtdh9Jiej4vRC/Bx2/F/35++joVbbj2tyEatsJmrZEucufL1ch/08Kl6ETj2O+9wYc/B/qpm6oh0aiqnhZHZZdvPHGG+zZs4e0tDRGjhzJgAEDyM7OBuC2226jXbt2bN++nbCwMCpWrMioUaMsjrhgSikIrIEKrAGduwOgz6Sgd8aid/6C3vwDev1X4FkF1ToI1b4L3BiEcpcJEM5Maa0LGG4sfXFxcURFRWGaJr169SI0NDTP66dOnWLRokWcPXsW0zR56KGHaN++fZHnlTUORXP1ttBaozd/j/7oPXBzQz00EqNTcIHHuuo6orLkSPeYPp8Je+LQcT+jd/0C6WlQ1QfVuTvq1t6o2vXtGs9Frn6PXQ2HHSMyTZOlS5fy0ksvERAQwMSJEwkKCqJu3bq5x3z66afcfPPN3HbbbRw7doyZM2cWKxGJ8k2npmAuXwC/bYPmbTCGPoXyl9lLrkpV8oB2nVHtOqNzcmDPDswf16LXf4Ve+7ltTOmWEFTHrqjKnlaHK4rJLokoPj6emjVrUqNGDQC6dOlCbGxsnkSklCIjIwOAjIwMy/uvhePT2/+LuXIRnM9EPfg4qkdf27iDKBeUmxu0DsKtdRD6zGn01g3oH9eiVy5Cr34XdXMP1G2hqOrybdfR2SURJScnExAQkPs4ICCAAwcO5Dmmf//+vPrqq3z33XecP3+eyZMnF3iuotY4XCTz+i9xtbYwM86StvQNMtd/jXuj6/EZNwX3etcV672u1hbCRnn7om4LRfe+1zb7bvMP6J9i0Ju+h/Y3Y9x+P6phU6vDFIVwmMkKP/30E927d+fuu+9m//79LFiwgHnz5mH84xNu0WscbKTP9hJXagu9f7dtQkLyKdSdAzDvGshp9wpQzH+fjBG5NqUUNL4B1fgG9H1D0Ou+RG/4FvPXLXB9a4zb+0Gr9jIN3MHYpR/D39+fpKSk3McFrWFYv349N998MwDNmjUjKyuLtLQ0e4QnnIDOysL8ZBnm3BfBMDCen4kROlhmS4lCKR8/jH6PYMxZiuo/HBITMCOnYk4NQ2//L3aapyWKwS6JqHHjxiQkJJCYmEh2djZbtmwhKCgozzGBgYHs3r0bgGPHjpGVlYW3t+vVBBNXTx/7A3PGM+jv16C63obx8nxUk+ZWhyWchPLwxLgtFGPGW6hh4yAnG3PJTMxZz6P37bY6PIGduubc3NwYPnw406dPxzRNevToQb169Vi9ejWNGzcmKCiIRx55hLfeeouvv/4agFGjRsnX53JOmznotV+go1eCpxfGmMmoNh2tDks4KeVeAdWlJ7pTMHrLOvQXH2DOnQStgzD6DUHVbWh1iOWW3dYRlRVHWuPgqJyxLXRSom0saP9uaNcZY8hoVFWfaz6vjBFdPVe9x/SF87Zp399+AucyUJ2CUfc8hKpW86rP5extUZocdh2REMWltUb/9z/oD94CQA19CtWlp3w7FqVOVayE6nM/uuvttrJQ675Eb/sJdccDqDvuR1WoaHWI5YYkIuEwdNoZzFWLYPt/oWkLjOHjbaVehChDqooX6v5H0T3vQn8Shf7yA/TW/2A89ASqVQerwysXJBEJh6B/24a5LBLOpqMeGIrqfa/TbAEgXIPyC0D961n0rb0x//0m5vyp0L4LxsARKP9qVofn0iQRCUvpzHPoj6PQm76DOg0wxk1F1ZNBY2Ed1bwNxsuR6B8+Q3/zEebv21F3D7JtJyIVv8uEtKqwjD74P8z3IuDkCdRt96FCH5Z+eeEQVIUKqDsHoDsFY374jq3L7ucNtu7iutdZHZ7LkcJcwu50djbm5+9jzn4BsrMxnnkVo/8wSULC4ajAGriNeQlj1CQ4nYw5/WnMbz9FmzlWh+ZS5BuRsCudcAxz6evwZzzq5p6oB/+F8qxidVhCXJFq1xmjSQvM9xej1yxH7/wZY9g4VA2Z9l8aJBEJu9Cmid7wDfqTZVCpEsbIF1Adulgdlkspas+vDRs2sHLlytzyWn369KFXr15WhOqUVFVvjCcmoH/ZhP73W5ivhKHuH4rq3tfq0JyeJCJR5nRKkm1G3J4dtlXsj4xB+foX/UZRbMXZ8wtsW7CMGDHCoiidn1IK1SkYfX0rzOUL0R+8jY77mZxnXgFkrVtJyRiRKFNm7GbM8LEQvwc1eBTG2MmShMrA5Xt+ubu75+75JcqG8g3ACHsZ9cgYOLSPpKcfRe/+1eqwnJZ8IxJlQp9NR//7LfQvG6FhM4wRT0t/ehkqzp5fAD///DN79+6lVq1aPProowXuzSR7fl2F+x4i+6ZbSJ07mez5U6ly/yNUGfQYyq38/mktye9F+W0tUWb0njjMqPmQdhp170OoO/rbdtMUlurQoQO33HILFSpUYO3atSxatIgpU6bkO072/LpKlaoQMOsdTi6aydlPV3D2t18x/vUsyjeg6Pe6IIeuNVfUQCrAli1b+Pjjj1FK0aBBA5566il7hSdKgb5wHr1mBXrdl1CzDsboOajrZFdMeyjOnl9Vq1bN/blXr16sWrXKbvG5OlWpEsYjYzCbtUSvWoL5yjhbL0DLdlaH5hTskoiKM5CakJBAdHQ006ZNw8vLi9TUVHuEJkqJ/vOgbVp2wlFUz7tQ/R5FVapkdVjlxuV7fvn7+7NlyxbCwsLyHJOSkoKfnx8A27ZtyzeRQVw7o3MPdIOmmG/NxpwfjurbH3XPIClXVQS7JKLLB1KB3IHUy2+EdevWcfvtt+Pl5QWAj8+1l/wXZU/n5NgqF3/5AVT1sZXokU+BdlecPb++/fZbtm3bhpubG15eXowaNcrqsF2SqlUXY+Jc9Advob/+CH30MMZjz6Aqe1odmsOyy35EW7duJS4ujpEjRwKwadMmDhw4kGca6Zw5c6hduzb79u3DNE369+9P27Zt853rnwOpFy5cKPCa7u7uZGdnl8G/xvmUVVtkJxzjTOQ0sv73G5Vu7YX3489hVHXsXXULa4uKFaWqQ2FcdT+i0lRYW5j/+Qb94dtQow7GmJdQ1WtZEJ19OfQYUVFM0yQhIYEpU6aQnJzMlClTmDt3LlWq5F11LwOpV6+020Jrjd78A/qjpWC4oR57huxOwSSfvwDnHbvNZWM8YU9Gj77omnUw35qDOf0ZjJETUM3bWB2Ww7HLOqLiDKT6+/sTFBSEu7s71atXp1atWiQkJNgjPHEV9JkUzIWvolcugkbXY4RHYnQKtjosIRyWat4GY9Jc8PHDfGMK5n++xsk3xi51dklElw+kZmdns2XLFoKCgvIcc9NNN/H7778DcObMGRISEnLHlIRj0HFbMcPDYE8cauBjtvEg2adFiCKp6rUwJr4GrYNs6+tWLUFnZ1kdlsOwS9dccQZS27Rpw86dOxk/fjyGYTB48OA8002FdXRmBvrDd9E/xUD9RhjPTEfVqW91WEI4FVXZE2PURHT0KvS3n6L//gtj1ESUp5fVoVnOLpMVypIMpBbtWtpCH9hj2zMo6STqjgdQdw9EuVco5Qjtx4oxotTUVHbu3Mkff/xBRkYGnp6eXHfdddx44434+vqW2XVLi9xjRbvatjD/+x/08gW29XZPhaP8XGfxa5lOVjh16hR//vknZ8+epUqVKjRo0EDKe7gwnZWF/uLf6O/XQGANjOdnopo0tzosp3Ls2DFWr17N77//TqNGjahTpw6+vr6cO3eOTZs2sWzZMlq2bMnAgQNlTU85Y9zcA+3jh7lkJuas5zDCwst1L8MVE1F2djYxMTGsXbuWxMREatasiYeHB5mZmZw4cYLq1avTu3dvQkJCcJctdF2G/utPzHdfh2OHUV1vQw0YgfKobHVYTmfx4sXcc889hIWFUaFC/m+RWVlZbNu2jSVLljB9+nQLIhRWUi3aYjw3EzNyKuacCRijX0I1a2l1WJa4Ytfc+PHjadWqFbfeeitNmzbFMC7NbTBNk/j4eDZv3szvv//O66+/bpeA/0m6DYpW3LbQpomO+QL92QqoXAXj0TBUm452iNB+ZPr21ZN7rGjX1P196m/M+VPh1N8Yjz2N6nBLKUdnX6XeNRceHl5ohQPDMGjWrBnNmjXjzJkzVxmqcDQ6KdFWqHTfb9C2M8Yjo1FVpbpFaZkzZw7PP/98vufnzp3Ls88+a0FEwlGowBoYL8zGXDAN8605qIH/wuh1l9Vh2dUVp29fnoS++OKLAo/56quv8PZ27NX0onBaa8wt6zGnhsEf8aihYbaZPJKEStXFpQnFfV6UL6pKVYynp0GbTugP38b8bFW5WmtU7IGdTz/9lHvuuafA5++6q3xlb1eh085grloM27dAkxYYw8ehqtW0OiyXsnr1asA23nrx54v+/vtvqlWTdVjCRlWshPHkBNsao28+gguZMGAESrn+zq9FJqLdu3cDtjGhiz9f9Pfff1O5sgxiOyP926+YyyMhPc1WKfv2UKkQXAYuVhQxTTNPdRGw9aUPGDDAirCEg1KGGwwZDRUroWO+gPOZMPhJl783i0xES5YsAeDChQu5P4Nt73ZfX1+GDx9edtGJUqfPZ6I/iUJv+BZq18cIm4Kq38jqsFzWxQrXzZo1y1MjUYjCKKVg4GNQqfL/fzM6D8PGufTmkkUmokWLFgGwcOFCxowZU+YBibKjD+3DXBoBJxNQt4WiQgejKkjV6bJy+vTp3AWrV0pClx8nBNiSkbpvMGbFirZKDFkXbLu+OvFi8isp9hiRJCHnpbOzMT//t+3Tla8/xtPTUDfcaHVYLu+VV16hRYsWdOvWjSZNmhS4/GHTpk3s3buXefPmWRipcFTGnQMwK3mgV7+LuWgGxpMvoCq63oaTV0xEEydO5J577qFjx44FLljNzs7ml19+4auvvmLGjBllFqQoOX3iGMmzn0fH/w91cw/Ug4+jPKsU/UZxzebMmUNMTAxvvfUWiYmJVK9encqVK3Pu3LncBeK9e/dm6NChpXK9uLg4oqKiME2TXr16ERoamuf1rKwsFi5cyKFDh6hatSrjxo2jevXqpXJtUXaMkHswK1ZCr1qMGfkKxtjJqEoeVodVqq6YiEaPHs3q1at59913adiwIbVr186trJCQkMChQ4do1aqV7PTogLTW6A3f2MaDKnrY9kFx8oVyzsbd3Z0+ffrQp08fTp06xZEjR8jIyMgtkfXPrVCuhWmaLF26lJdeeomAgAAmTpxIUFBQntJB69evp0qVKixYsICffvqJ999/n/Hjx5daDKLsGN1utyWj997AXPgqxpjJqEqu883oiomobt26PPPMM5w+fZpdu3Zx5MgR0tLSqFKlCt26dWPMmDHF3tK7qE9rF23dupXXX3+dmTNn0rhx46v/Fwn06STMZZHw+w5o1Z6A8eGkmFZHVb4FBgaWaW3G+Ph4atasmbt1SpcuXYiNjc2TiLZt20b//v0B6Ny5M++99x5a63IxPdgVGJ27Y6JtyWjxdNuOry4yxlusMSJfX1+6detW4osU59MawLlz5/j2229p2rRpia9V3pmxP6JXLYbsLNTDT6KC++DmHwhSisVSWVlZfPLJJ/z000+kpaWxfPlydu7cSUJCAn369Lnm8ycnJxMQcKmCc0BAAAcOHCj0GDc3Nzw9PUlLS8u3ID0mJoaYmBgAZs2aVWgCdXd3l8LH/89ubXFXf85VrsyZhTNwf3cevi/MdLhkVJK2uGIi2rNnDy1atADIt4bocq1atbriRYrzaQ1si//uvffeQqs4iMLpjHTbhls/b4SGzTCGj0fVrGN1WOL/LV++nOTkZMLCwnLHU+vVq8fy5ctLJRGVppCQkDyz/AqroSa15i6xa1u06YwaMpoLKxeROP15W7e7A82mK/Vac0uXLs2dzXP5GqLLKaVYuHDhFQMrzqe1Q4cOcerUKdq3by+J6CrpvTttdeJSk1H3PoS6o79LrzlwRr/88guRkZF4eHjkdoX5+/uTnJxcKuf39/fPs2A2KSkp3xjUxWMCAgLIyckhIyNDNp90Uka32zFzctD/fhPznbkY/3oO5cQ7IFwx8sunlF5cT1QWTNNkxYoVxZr0IN0Gl+gL50lf9SYZX67GrU59fCbOokLTFvmOKw9tUVxWtYW7uzummXeg7syZM6WWCBo3bkxCQgKJiYn4+/uzZcsWwsLC8hzToUMHNmzYQLNmzdi6dSstW7aU8SEnZvToi5mTjV79Lnrp6/DYM077AbTEKXT37t0YhpHbdXclRX1ay8zM5OjRo0ydOhWwLfC7WK34nxMWpNvARh85aNszKOEoqsed6PuHklqpUoFjQa7eFlfDqm0gOnfuzMKFC3OnaqekpLBs2TK6dOlSKud3c3Nj+PDhTJ8+HdM06dGjB/Xq1WP16tU0btyYoKAgevbsycKFCxk7dixeXl6MGzeuVK4trGOE3GP7ZvRJFLi72yowGFesZe2Qir1V+JQpUxg0aBA33HAD0dHRfP311xiGwe23306/fv2u+N6cnByeeuopXn75Zfz9/Zk4cSJhYWHUq1evwOPDw8MZMmRIsWbNlbe9UrSZg/5uDfqLD6CqN8bQp1At213xPa7aFiVhVSLKzs5m1apVrFu3jgsXLlCxYkV69erFww8/XOCmeY6kvN1jJWF1W5hff4SOXoXqcSdq0OOWftMt063Cjx49SrNmzQBYt24dU6ZMwcPDg8mTJxeZiIrzaU0UTZ88gfleBMTvRXW4BTVkFKqK9PE7A3d3d4YOHcrQoUNzu+SkW0yUFtW3P5xNQ6/9HKr6oO5+0OqQrkqxE9HFL04nTpwAyJ3xdvbs2WK9v3379rRv3z7PcwMHDizw2PDw8OKGVS5ordE/rkWvXgqGgRrxNKpTsPwhcyLDhg0jKioKIM906ccee4x3333XqrCEi1BKwQPDID0N/cW/Mb2qYvS40+qwiq3Yiej666/nvffeIyUlhY4dbdtHnzhxQmbdlDF9JgVzxSLY+Qtc39q2Z5C/7GHjbHJycvI9l52dnW8CgxAlpQwDHh1rW8rxwduYnl4YnYKtDqtYip2IRo8ezZdffom3t3fuBnnHjx+nb9++ZRZceafjttqS0LkM1MARqJ53O+VAZHn28ssvo5QiKyuLKVOm5HktKSkpt7tbiNKg3NwwHn8Oc/5UdNQbaE8vVOsOVodVpGInosqVK2MYBps3b+bLL7/Ez8+PLl26FDk+JK6ezsxAf/gu+qcYqN8I45npqDr1rQ5LlEDPnj0B26LuHj165D6vlMLHx4fWrVtbFZpwUapiJYzRL2LOexHzzZkY46ehmjS3OqwrKnYieuedd0hISGD48OFUq1aNkydP8tlnn5GcnCxFT0uRPrDHNiEh6SSqb3/U3Q861KppcXW6d+8OwObNm6lVqxbXX3997mv79u1j5cqVpVZ9W4iLlGcVjKfCMWe/gLngFYznZqLqXmd1WIUqdj9PbGwsEyZMoF27dtStW5d27drx/PPPExsbW5bxlRs6OwtzzXLM1yaCUhjPz8C4b4gkIRfxxx9/5FuO0KhRI3788UeLIhKuTnn7Yjz9ClSshBn5Cjolqeg3WaTYicjX15fz58/nee7ChQv4+fmVelDljf7rT8zpz6K//RR1a2+Ml99ANSl6obBwHkqpfBMTTNOkmMv4hCgRFVAdI2wKnDtrS0bnMqwOqUBu4cWcK33u3Dnef/99DMMgNTWV33//nWXLltG1a1eys7NJTEzM3fzLntLS0gp83tPTk4wMx2z0i7Rpotd+jn57DuTkYDz+HMbt95X6tyBnaAt7Kawtynr25/79+zl8+DCtW7fOTUqrV6/Gw8ODW25x7H2inPkesxdHbgvl44dq0AQd8zn6j3hUx65lOumpJPdYsSsrjB49ushjilMAtbQ566pvnXQSM+oN2PcbtO2EMWQ0ytu3TK7l6G1hT1ZVVkhKSmLWrFmcPn06NwY/Pz8mTJiQpyCwI3LWe8yenKEtzB/XopcvQN3aG/XImDJbh1imlRXKsuhpeaK1Rm/dgP7gLTA16tGxqFtCZHGqiwsICGD27NnEx8fnVsBu0qQJhkzHF3Zi3NobMykR/dVqCKiOuqvgggJWcN664U5Ip5/BXLUYft0CTZrb9gyqVtPqsISdGIYh64aEpdQ9D8GpRPTn72P6V8Po0tPqkABJRHajd/+KuWwBpJ9B9XsEdft9KMM5S7YLIZyTUgoeHYM+nYResQDtF4Bq3sbqsIo/a06UjD6fifn+m5jzp0IVL4xJczHueECSkBDCEsq9AsaTL0CNOphLZqKPH7E6JPlGVJb04f2YSyMg8Tiq972o+4Y43P7ywvmlp6cTERHByZMnqVatGuPHj8fLyyvfcQMHDqR+fVuFjsDAQCZMmGDvUIWDUJ5eGGFTMGc8g7nwVYxJc1Fe3kW/sYzYLRHFxcURFRWFaZr06tWL0NDQPK9/9dVXrFu3Djc3N7y9vXnyySepVs05i3vq7Gz0Nx+hv/4IfP0xnp6GuuFGq8MSLio6OprWrVsTGhpKdHQ00dHRDB48ON9xFStW5LXXXrMgQuGIVEA1jFGTMOe+iPnmbIxx4ZYtoLdL15xpmixdupRJkyYRERHBTz/9xLFjx/Icc9111zFr1izmzp1L586dWbVqlT1CK3X6xDHM2RPQX36IuqkbxpRISUKiTMXGxhIcbKuyHBwcLNVORLGpxjegHh0L+35Df/C2ZQus7fKNKD4+npo1a1KjRg0AunTpQmxsbO6eRgCtWrXK/blp06Zs3rzZHqGVGq01esM3ti17K1TCGDkB1cGxFyoK15Camppb4cTX15fU1NQCj8vKyuKFF17Azc2Ne++9l5tuusmeYQoHZXTujnn8CPrbT6B2fVSvu+0eg10SUXJycp5FewEBARw4cKDQ49evX0/btm0LfC0mJoaYmBgAZs2aRWBgYIHHubu7F/paactJPsmZhTO4sONnKrbrjPeYSbj52+faxWHPtnB0ztoW06ZN4/Tp0/mef/DBvDtxKqUKXZO2ePFi/P39+fvvv3nllVeoX78+NWvmXz7giPeYo3P2ttCPjSM1OZHzHy2larPmVGrXucTnKklbONxkhU2bNnHo0KFCd2kNCQkhJCQk93Fhq5nttdJZb/sRc9USyDqPengk2cF3kGICDrTK2hlWfduLVZUVrtXkyZMLfc3Hx4eUlBT8/PxISUnJswPs5fz9/QGoUaMGLVq04I8//igwETnaPeYMXKEt9ODR8NcRTr82GWPia6hadYt+UwFKco/ZZYzI39+fpKRLlV+TkpJyb4rL7dq1i88++4znn3+eChUcu+q0zkjHXPo65ltzoHotjMnzMbr3lQoJwu6CgoLYuHEjABs3bszdQfly6enpZGVlAXDmzBn27duXp2tcCOVRGWPMS+DujrlwGvpswTUGy4JdvhE1btyYhIQEEhMT8ff3Z8uWFMn+uQAACV9JREFULYSFheU55vDhw7zzzjtMmjQJHx8fe4RVYnrvTsxl8+F0MuruQag7B6DcZF2QsEZoaCgRERGsX78+d/o2wMGDB1m7di0jR47kr7/+4u2338YwDEzTJDQ0VBKRyEcFVLfNpJt3cSbdVLv8bSt20dNrtX37dpYvX45pmvTo0YN+/fqxevVqGjduTFBQENOmTePIkSP4+toKfxZ3nYM9CzLqrAvoNSvRMZ9DjToYI55GNWxaqtcoC67QbVBanLVrzkpS9LRortYW5k/r0Mvmo24Lxeg//KreW6ZFT69V+/btad++fZ7nBg68VHTvSn3gjkAfOWhbnHr8CKpHX9T9w1CVKlkdlhBClDrjll6Yfx5A/xCN2aAJxk3dyvR6DjdZwdFoMwf9/Wfoz/8NXt4YT4WjWrUv+o1CCOHE1IAR6KN/oJdHomvXQ9VtWGbXklpzV6BPnsB8bRJ6zQpU204Y4ZGShIQQ5YJyr4AxcgJ4emEunlmmkxckERVAa425+QfMqU/BX0dQI55GPfG8pbWYhBDC3pSPH8bIFyD5FOa789BmTplcRxLRP+gzpzEXTUevWAgNm2KER2J07i7TsoUQ5ZJqfANq0OOwe7ttiKIMyBjRZXTcz5grFsK5DNTAEaied5fp3u5CCOEMVLfb4c949Dcfoxs0QbW/uVTPL4kI0JkZ6NVL0T+uhXoNMZ6ZjqpT3+qwhBDCISilYNAT6GN/YL73Bkatuqha9Urt/OX+476O34M59Sn0T+tQdzxg25dDkpAQQuShKlSwjRdVrIi5ZBY681ypnbvcJiKdnYW5ZjnmnEmgFMbzMzD6PWLZfhxCCOHolH8gxr+ehRN/oVcuLrVtI8pl15z+6wjm0nlw9DCq622oAcNRHp5WhyWEEA5PNW+DumcQ+vP3oWkLVPc7rvmc5SoRadNEr/sSvWYFVPbEGP0iqm0nq8MSQginovr2Rx/ci179DrphU1SDJtd0vnLTNaeTTmK+Phn90VJo2c62OFWSkBBCXDVlGBjDn4aqvphvzkafTb+m87l8ItJaY279D+bUMPjjAOqRMbZvQt5+VocmhBBOS1X1xnjieUg5hbls/jWNF9mtay4uLo6oqChM06RXr16EhobmeT0rK4uFCxdy6NAhqv5fe3cX09QZxgH8f1oytYCVFsGl1mRWjdE7LKgkBnVNlqDZotlUlpARLhYubPkIWYAJaWYIzbKuJn6MkHlBmEu8GVwvxMgFSMQxkikhYmMWTDQIpQzSaaB9d6G0NpUJ7bEHzvn/7nrO4Zw3T87Th/NweN/sbNTU1CAvLy+la4r5fyB++Qnij37Athe6ylpIeR+mdE4iInpFsu2F9HnFq39/+b0H0ienkjpPWp6IIpEIrl+/jqamJvh8PvT39+PJkydxx9y6dQuZmZm4fPkyTpw4gRs3bqR0zZd/DiLidkGMDEI6VQ7dN20sQkREMpM+/hQoKIb4rRNifDSpc6SlED169Ajbtm1Dfn4+MjIyUFxcjKGhobhj7t27h6NHjwIADh06hPv37yf1qCdevkTk13YEv6sDDJnQNf0AXekXkHRcuI7U6c6dO6irq8PZs2fh9/uXPW5kZATV1dVwOp3o6elJ4whJzSRJgu4rJ5Cbj0jH94gEA6s+R1pac4FAAGazOfrZbDZjfHx82WP0ej0MBgPm5uaweXP8RKO9vb3o7e0FAHg8HuTm5sbtj/wbQmDsL2z8rAyZX34N6QOuGZSRkZEQJ61SYyysVivq6+vR0dGx7DFLXYkLFy7AbDajsbERdrudq7SSLCRDJnRVDYj82IyFv/2AZXVLRqy717cdDgccDkf089tWAhTfepFl2f56X/rWXV+r1LZ6ZCrUuELrSorJm10JANGuBAsRyUWyfgSd52dssGzH3Cq/b9LSmjOZTJieno5+np6ehslkWvaYcDiMUCiE7OzspK4nbdiY/GCJVOhtXYlAYPUtFKL/k+x3b1qeiGw2G54+fYrJyUmYTCYMDAzA5XLFHXPgwAHcvn0be/bsweDgIPbv38+lF4heu3jxIoLBYML2c+fOobCwUNZrvav9vUSNbc5kMRYxycQiLYVIr9ejsrISra2tiEQiOHbsGKxWK27evAmbzQa73Y7jx4/jypUrcDqdyMrKQk1NTTqGRrQuNDc3p/TzK+lKLFlJ+xtgy/dNjEVMMu1vScg1ax0RKcrtdqO8vBw2my1hXzgcRnV1NVpaWmAymdDY2AiXywWrVb6p/ImSpdqZFRoaGpQewprBWMSoMRZ3795FVVUVHj58CI/Hg9bWVgCv/i7U1tYGIL4rUVtbi8OHD6dchNQYy2QxFjHJxGLdvTVHRPGKiopQVFSUsH3pyWdJQUEBCgoK0jk0ohVR7RMRERGtD3q32+1WehDvy86dO5UewprBWMQwFvJhLGMYi5jVxoIvKxARkaLYmiMiIkWxEBERkaJU99bcu9Y9Urtr165heHgYRqMRXq8XADA/Pw+fz4fnz59j69atqK2tRVZWlsIjfb+mpqZw9epVBINBSJIEh8OB0tJSTcZCblrOMeZXjKw5JlQkHA6L8+fPi2fPnomFhQVRX18vJiYmlB5WWj148ED4/X5RV1cX3dbV1SW6u7uFEEJ0d3eLrq4upYaXNoFAQPj9fiGEEKFQSLhcLjExMaHJWMhJ6znG/IqRM8dU1ZpbybpHardv376E3z6GhoZQUlICACgpKdFETHJycqJv7mzatAkWiwWBQECTsZCT1nOM+RUjZ46pqhBxhuG3m52dRU5ODgBgy5YtmJ2dVXhE6TU5OYnHjx9j165dmo9FqphjiXhPpZ5jqipE9G6SJGlqVvMXL17A6/WioqICBoMhbp/WYkHvnxbvKTlyTFWFaDUzDGuJ0WjEzMwMAGBmZiZh1Vu1WlxchNfrxZEjR3Dw4EEA2o2FXJhjibR8T8mVY6oqRG+ue7S4uIiBgQHY7Xalh6U4u92Ovr4+AEBfX5/s69esRUIItLe3w2Kx4OTJk9HtWoyFnJhjibR6T8mZY6qbWWF4eBidnZ3RdY9Onz6t9JDS6tKlSxgdHcXc3ByMRiPOnDmDwsJC+Hw+TE1Naeb10rGxMbS0tGDHjh3R1kBZWRl2796tuVjITcs5xvyKkTPHVFeIiIhofVFVa46IiNYfFiIiIlIUCxERESmKhYiIiBTFQkRERIpiISIiIkWxEBERkaL+A1AZp+TS5iHfAAAAAElFTkSuQmCC\n", "text/plain": [ "
" ] }, "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(x_bar[2,:])\n", "plt.ylabel('theta(t)')\n", "#plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 3)\n", "plt.plot(x_bar[3,:])\n", "plt.ylabel('psi(t)')\n", "#plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", "plt.plot(x_bar[4,:])\n", "plt.ylabel('cte(t)')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The results are the same as expected" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "------------------\n", "\n", "the kinematics model predictits psi and cte for constant heading references. So, for non-constant paths appropriate functions have to be developed.\n", "\n", "-----------------" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## PRELIMINARIES" ] }, { "cell_type": "code", "execution_count": 85, "metadata": {}, "outputs": [], "source": [ "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", " final_xp=[]\n", " final_yp=[]\n", " delta = step #[m]\n", "\n", " for idx in range(len(start_xp)-1):\n", " section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n", "\n", " interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n", " \n", " fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n", " fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n", " \n", " final_xp=np.append(final_xp,fx(interp_range))\n", " final_yp=np.append(final_yp,fy(interp_range))\n", "\n", " return np.vstack((final_xp,final_yp))\n", "\n", "def get_nn_idx(state,path):\n", "\n", " dx = state[0]-path[0,:]\n", " dy = state[1]-path[1,:]\n", " dist = np.sqrt(dx**2 + dy**2)\n", " nn_idx = np.argmin(dist)\n", "\n", " try:\n", " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " path[1,nn_idx+1] - path[1,nn_idx]] \n", " v /= np.linalg.norm(v)\n", "\n", " d = [path[0,nn_idx] - state[0],\n", " path[1,nn_idx] - state[1]]\n", "\n", " if np.dot(d,v) > 0:\n", " target_idx = nn_idx\n", " else:\n", " target_idx = nn_idx+1\n", "\n", " except IndexError as e:\n", " target_idx = nn_idx\n", "\n", " return target_idx\n", "\n", "def road_curve(state,track):\n", " \n", " #given vehicle pos find lookahead waypoints\n", " nn_idx=get_nn_idx(state,track)\n", " LOOKAHED=6\n", " lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n", "\n", " #trasform lookahead waypoints to vehicle ref frame\n", " dx = lk_wp[0,:] - state[0]\n", " dy = lk_wp[1,:] - state[1]\n", "\n", " wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n", " dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n", "\n", " #fit poly\n", " return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n", "\n", "def f(x,coeff):\n", " return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n", "\n", "def df(x,coeff):\n", " return 3*coeff[0]*x**2+2*coeff[1]*x+coeff[2]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "test it" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### MPC Problem formulation\n", "\n", "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n", "\n", "The controller generates a control signal over a fixed lenght T (Horizon) at each time step." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "![mpc](img/mpc_block_diagram.png)\n", "\n", "![mpc](img/mpc_t.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Linear MPC Formulation\n", "\n", "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n", "\n", "$x_{t+1} = Ax_t + Bu_t$\n", "\n", "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n", "\n", "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n", "\n", "The objective function used minimize (drive the state to 0) is:\n", "\n", "$\n", "\\begin{equation}\n", "\\begin{aligned}\n", "\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n", "\\textrm{s.t.} \\quad & x(0) = x0\\\\\n", " & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t= MIN_SPEED]\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", "\n", "prob = cp.Problem(cp.Minimize(cost), constr)\n", "solution = prob.solve(solver=cp.OSQP, verbose=True)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Print Results:" ] }, { "cell_type": "code", "execution_count": 112, "metadata": {}, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ "No handles with labels found to put in legend.\n" ] }, { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "x_mpc=np.array(x.value[0, :]).flatten()\n", "y_mpc=np.array(x.value[1, :]).flatten()\n", "theta_mpc=np.array(x.value[2, :]).flatten()\n", "psi_mpc=np.array(x.value[3, :]).flatten()\n", "cte_mpc=np.array(x.value[4, :]).flatten()\n", "v_mpc=np.array(u.value[0, :]).flatten()\n", "w_mpc=np.array(u.value[1, :]).flatten()\n", "\n", "#simulate robot state trajectory for optimized U\n", "x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n", "\n", "#plt.figure(figsize=(15,10))\n", "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(track[0,:],track[1,:],\"b*\")\n", "plt.plot(x_traj[0,:],x_traj[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "#plot v(t)\n", "plt.subplot(2, 2, 2)\n", "plt.plot(v_mpc)\n", "plt.ylabel('v(t)')\n", "#plt.xlabel('time')\n", "\n", "#plot w(t)\n", "plt.subplot(2, 2, 3)\n", "plt.plot(w_mpc)\n", "plt.ylabel('w(t)')\n", "#plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", "plt.plot(cte_mpc)\n", "plt.ylabel('cte(t)')\n", "plt.legend(loc='best')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "full track demo" ] }, { "cell_type": "code", "execution_count": 93, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CVXPY Optimization Time: Avrg: 0.1988s Max: 0.2729s Min: 0.1579s\n" ] } ], "source": [ "track = compute_path_from_wp([0,3,4,6],\n", " [0,0,2,1])\n", "\n", "sim_duration = 20\n", "opt_time=[]\n", "\n", "x_sim = np.zeros((N,sim_duration))\n", "u_sim = np.zeros((M,sim_duration-1))\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER_SPEED = 1.57/2\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = np.radians(-0)\n", "\n", "K=road_curve(x0,track)\n", "\n", "x0[3]=-np.arctan(K[2])\n", "x0[4]=K[3]\n", "\n", "x_sim[:,0]=x0\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", "u_bar[1,:]=0.00\n", "\n", "for sim_time in range(sim_duration-1):\n", " \n", " iter_start=time.time()\n", " \n", " K=road_curve(x_sim[:,sim_time],track)\n", " \n", " # dynamics\n", " x_bar=np.zeros((N,T+1))\n", " x_bar[:,0]=x_sim[:,sim_time]\n", " for t in range (1,T+1):\n", " xt=x_bar[:,t].reshape(5,1)\n", " ut=u_bar[:,t-1].reshape(2,1)\n", "\n", " A,B,C=get_linear_model(xt,ut)\n", "\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", "\n", " xt_plus_one[3]=-np.arctan(df(x_bar[0,t],K))\n", " xt_plus_one[4]=f(x_bar[0,t],K)\n", " x_bar[:,t]= xt_plus_one\n", " \n", " #CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", " x = cp.Variable((N, T+1))\n", " u = cp.Variable((M, T))\n", " \n", " for t in range(T):\n", "\n", " # Tracking\n", " cost += 50*cp.sum_squares( x[3, t]) # psi\n", " cost += 1000*cp.sum_squares( x[4, t]) # cte\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(M))\n", " \n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],10*np.eye(M))\n", " \n", " # Constrains\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]\n", "\n", " # sums problem objectives and concatenates constraints.\n", " constr += [x[:,0] == x_sim[:,sim_time]] # starting condition\n", " constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " \n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(solver=cp.OSQP, verbose=False)\n", " \n", " #retrieved optimized U and assign to u_bar to linearize in next step\n", " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", " (np.array(u.value[1, :]).flatten())))\n", " \n", " u_sim[:,sim_time] = u_bar[:,0]\n", " \n", " # Measure elpased time to get results from cvxpy\n", " opt_time.append(time.time()-iter_start)\n", " \n", " # move simulation to t+1\n", " tspan = [0,dt]\n", " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:,sim_time],\n", " tspan,\n", " args=(u_bar[:,1],))[1]\n", " \n", "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " np.max(opt_time),\n", " np.min(opt_time))) " ] }, { "cell_type": "code", "execution_count": 92, "metadata": {}, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzde3hU1b3/8c+aDAFCuCXDxQQRuQqxSBAEURQlosdai1RROdVa68/DD6uHwvERPfZyjtrDqQb5WaXao9VWe8G2ap9j1doUEQuGi0lQEpSAKHInCWAgCSSZ9fsDSYm5TZKZWXtm3q/n4WFm9p7ZH/eaicw3a323sdZaAQAAAAAAeJjPdQAAAAAAAIC2UMAAAAAAAACeRwEDAAAAAAB4HgUMAAAAAADgeRQwAAAAAACA51HAAAAAAAAAnud3HaAzdu/eHfVjBgIBlZWVRf24aBlj4i2Mh7cwHt7DmHgL4+E9rsYkIyMj6seMpPb+O9nLnwWytZ9Xc0lk6wiv5pIil62ln8nMwAAAAAAAAJ5HAQMAAAAAAHgeBQwAAAAAAOB5FDAAAAAAAIDnUcAAAAAAAACeRwEDAAAAAAB4HgUMAAAAAADgeRQwAAAAAACA51HAAAAAAAAAnkcBAwAAAAAAeB4FDAAAAAAA4HkUMAAAAAAAgOf5XQcAAAAAgEQSzF8p+8oLkrUdfxFjWtx0wOdTMBjs+GtHkLNsIZzrA0k+Beu9d95OnLN6hwlaea+1cc7MVdfLN3VG2JJQwAAAAACAKLG1tbIv/UrqkiwzfHQHX6T1L+PJ3brpWE1Nx147wpxma6XoI0nJ3brqWM2xKIUJXXK3rjp2zFGuNuo+bZ0z0zcQ1jieKmAEg0EtWrRIaWlpWrRokes4AAAAABBW9t0V0sEy+eb/h0xWdkSO0TsQUFlZWUReu7PI1n5ezSVFP5unemC89tpryszMdB0DAAAAAMLO1tfLvvFHacgIacw413GAmOOZAkZ5ebkKCgo0ffp011EAAAAAIOzs+nekA3vl++p1Mm0sZwDQlGeWkDz33HP65je/qerq6hb3ycvLU15eniRp8eLFCgTCu54mFH6/38lx0TLGxFsYD29hPLyHMfEWxsN7GBPEKxsMyr72eynzDGnsea7jADHJEwWM9957T71799bQoUNVXFzc4n45OTnKyclpuO9iHVDAw+uPEhVj4i2Mh7cwHt7DmHgL4+E9rsYkIyMj6sdEginKl/Z8JnPbQhmfZybCAzHFEwWMjz76SBs2bFBhYaGOHz+u6upqPfbYY7rrrrtcRwMAAACATrHWKvjn30v9M2QmXug6DhCzPFHAmDNnjubMmSNJKi4u1v/+7/9SvAAAAAAQH4oLpB3bZL51p4wvyXUaIGYxdwkAAAAAIuTE7IsXpbSAzORpruMAMc0TMzBOlZWVpaysLNcxAAAAAKDzthRLWzfL3Hi7jL+L6zRATGMGBgAAAABESPC1F6VefWQuvMx1FCDmUcAAAAAAgAiw27dIJUUyM2bKJHd1HQeIeRQwAAAAACACgn9+UUpJlbn4CtdRgLhAAQMAAAAAwszu/ETauE5m+tdkuqW4jgPEBQoYAAAAABBm9rXfS926y0y/ynUUIG5QwAAAAACAMLJ7d8luWC0z7UqZHj1dxwHiBgUMAAAAAAgj+8YfJL9f5rKrXUcB4goFDAAAAAAIE1u+XzZ/pczUGTK9+rqOA8QVChgAAAAAECb2Ly9JMjKXX+M6ChB3KGAAAAAAQBjYwwdl3/mrzJRLZdL6uY4DxB0KGAAAAAAQBvbNV6T6epkrZrmOAsQlChgAAAAA0En2yOeyb78uM3GqTP8M13GAuEQBAwAAAAA6yf7tVelYjcyV17qOAsQtChgAAAAA0Am2ukp2xf9K2ZNlMs9wHQeIWxQwAAAAAKAT7MrXpKqj8n11tusoQFyjgAEAAAAAHWSPHZP965+krGyZM4a7jgPENb/rAAAAAEAiWrZsmQoKCtS7d2/l5uY22f7OO+/oT3/6k6y16t69u2677TYNGTIk+kHRKvv3N6XKw/JdyewLINKYgQEAAAA4MG3aNN13330tbu/fv79+9KMfKTc3V9/4xjf085//PIrpEApbe1z2jZekkVkyI7NcxwHiHgUMAAAAwIExY8YoNTW1xe2jRo1q2D5ixAiVl5dHKxpCVP3W69KhcmZfAFHCEhIAAADA41asWKHs7OwWt+fl5SkvL0+StHjxYgUCgXa9vt/vb/dzosWr2Wx9ncpffkH+4Wcp7aIcGWNcR2rg1XMmka0jvJpLin42ChgAAACAh23atElvvfWW/vM//7PFfXJycpSTk9Nwv6ysrF3HCAQC7X5OtHg1WzB/pezeXfLNu89zs2O8es4ksnWEV3NJkcuWkZHR7OMsIQEAAAA86tNPP9VTTz2lu+++Wz179nQdB1+w1sq+/gf5Bw+VzjnPdRwgYVDAAAAAADyorKxMjzzyiL773e+2+NtIOFK2T9q9Q90vnynj4ysVEC0sIQEAAAAcWLp0qUpKSlRZWam5c+dq9uzZqqurkyTNmDFDf/jDH3TkyBE9/fTTkqSkpCQtXrzYZWR8wZYWS5K6ZLXclwRA+FHAAAAAAByYP39+q9vnzp2ruXPnRikN2qW0REpJlf/0M6WKCtdpgITBfCcAAAAAaAdbWiINH83yESDK+MQBAAAAQIjs5welfbtkRoxxHQVIOBQwAAAAACBUpZslSWZEluMgQOKhgAEAAAAAIbKlxVJysnTGMNdRgIRDAQMAAAAAQmRLS6QzR8n4u7iOAiQcChgAAAAAEAJbXSV9tp3lI4AjFDAAAAAAIBTbNks2SANPwBEKGAAAAAAQAltaIvl80tBRrqMACYkCBgAAAACEwJYWS4OHyXTr7joKkJAoYAAAAABAG2xtrbS9lOUjgEMUMAAAAACgLZ+USnW1NPAEHKKAAQAAAABtsKXFJ24MZwYG4AoFDAAAAABogy0tkU47XaZnL9dRgIRFAQMAAAAAWmGD9dK2zSwfARyjgAEAAAAArdn5iVRdJdHAE3CKAgYAAAAAtMKWlkgSMzAAxyhgAAAAAEArbGmxlNZPJr2f6yhAQqOAAQAAAAAtsNZKpSUyLB8BnKOAAQAAAAAt2b9H+vyQNJLlI4BrFDAAAAAAoAW2tFgS/S8AL6CAAQAAAAAtKS2RUntJAwe5TgIkPAoYAAAAANACW1osDR8jY4zrKEDCo4ABAAAAAM2wh8qlA3tp4Al4BAUMAAAAAGiGLS2RRP8LwCsoYAAAAABAc0qLpa7dpMFDXScBIAoYAAAAANAsW1oiDR0lk5TkOgoAUcAAAAAAgCZs1RFp16cyI1k+AngFBQwAAAAA+LKtmyVr6X8BeAgFDAAAAAD4EltaIiX5pTNHuo4C4AsUMAAAAADgS2xpsTRkuExyV9dRAHyBAgYAAAAAnMIePyZ9slVm+BjXUQCcggIGAAAAAJxq+xapvo7+F4DHUMAAAAAAgFPY0mLJGGn4aNdRAJyCAgYAAAAAnMKWlkiZZ8j0SHUdBcApKGAAAAAAwBdsfb207SOZEfS/ALyGAgYAAAAAnPTZx9Kxaon+F4DnUMAAAAAAgC/Y0hJJYgYG4EEUMAAAAADgC7a0WOo3UKZPuusoAL6EAgYAAAAASLLWSls3ywxn9gXgRRQwAAAAAECS9u6SKg9LLB8BPIkCBgAAAABIsqWbJEmGBp6AJ1HAAAAAAABJKi2RevWRBmS4TgKgGRQwAAAAAEBfXIFkxBgZY1xHAdAMChgAAAAAEp6tOCCV72f5COBhFDAAAAAAJDxbWiJJMjTwBDyLAgYAAAAAlBZL3VOkQUNcJwHQAgoYAAAAABKeLS2Rhp0l40tyHQVACyhgAAAAAEho9sjn0u4dMsNZPgJ4md91AAAAACARLVu2TAUFBerdu7dyc3ObbN+1a5eWLVum7du364YbbtDVV1/tIGWC2Hqy/wUNPAEvYwYGAAAA4MC0adN03333tbg9NTVV3/72t/W1r30tiqkSky0tkfxdpDNHuI4CoBUUMAAAAAAHxowZo9TU1Ba39+7dW8OHD1dSEj0ZIs2WlkhnjpDpkuw6CoBWUMAAAAAAkLDssRppxzaWjwAxgB4YAAAAQIzLy8tTXl6eJGnx4sUKBALter7f72/3c6Il0tmOvb9Bh+rr1fvc89U1Ts6bV3NJZOsIr+aSop+NAgYAAAAQ43JycpSTk9Nwv6ysrF3PDwQC7X5OtEQ6W3DDGsn49Hm/DJk4OW9ezSWRrSO8mkuKXLaMjIxmH2cJCQAAAICEZUtLpNOHyHRPcR0FQBuYgQEAAAA4sHTpUpWUlKiyslJz587V7NmzVVdXJ0maMWOGDh06pEWLFqm6ulrGGL322mtasmSJUlL4oh0utq5O+vgjmakzXEcBEAIKGAAAAIAD8+fPb3V7nz599OSTT0YpTYLasU06fowGnkCMYAkJAAAAgIRkS4tP3Bgx2m0QACGhgAEAAAAgIdnSEmlApkyvvq6jAAgBBQwAAAAACccGg1JpicyIMa6jAAgRBQwAAAAAiWfPZ1LVEYkCBhAzKGAAAAAASDh2y4n+FzTwBGIHBQwAAAAACceWFEqBASf+AIgJFDAAAAAAJBRbVyttfl8mK1vGGNdxAISIAgYAAACAxLLtI+lYtczZ410nAdAOFDAAAAAAJBRbXCAlJUmjxrqOAqAd/K4DSNLx48f1wx/+UHV1daqvr9fkyZM1e/Zs17EAAAAAxCFbXCANO0ume4rrKADawRMFjC5duuiHP/yhunXrprq6Ov3gBz/QuHHjNHLkSNfRAAAAAMQR+/lBacfHMtfc5DoKgHbyxBISY4y6desmSaqvr1d9fT3NdAAAAACEnS0ukiSZLPpfALHGEzMwJCkYDOqee+7R3r17dfnll2vEiBFN9snLy1NeXp4kafHixQoEAtGOKb/f7+S4aBlj4i2Mh7cwHt7DmHgL4+E9jAkirrhA6tlbOv1M10kAtJNnChg+n08PP/ywjh49qkceeUQ7duzQ4MGDG+2Tk5OjnJychvtlZWXRjqlAIODkuGgZY+ItjIe3MB7ew5h4C+PhPa7GJCMjI+rHRPTZYFC2pOjE5VN9npiMDqAdPPep7dGjh7KyslRUVOQ6CgAAAIB48tnHUuVhKSvbdRIAHeCJAsbnn3+uo0ePSjpxRZL3339fmZmZjlMBAAAAiCd2U4EkyYyhgAHEIk8sITl48KCeeOIJBYNBWWt1/vnn69xzz3UdCwAAAEAcscUF0uBhMr36uI4CoAM8UcA444wz9JOf/MR1DAAAAABxylZXSR9/JHP5LNdRAHSQJ5aQAAAAAEBEffi+VF8vQ/8LIGZRwAAAAAAQ9+ymAqlbd2noWa6jAOggChgAAAAA4pq19kT/i7POkfF7YhU9gA6ggAEAAAAgvu3bJZXvZ/kIEOMoYAAAAACIa7a4UJIoYAAxjgIGAAAAgLhmNxVIAzJl+g10HQVAJ1DAAAAAABC3bO1xacsHzL4A4gAFDAAAAADxq7REOn5c5uzxrpMA6CQKGAAAAADili0ukPx+aeTZrqMA6CSuIQQAAACE6PHHHw9pP7/fr7lz50Y4DUJhiwulEVkyXbu5jgKgk5iBAQAAAIRozZo1GjBgQJt/3n33XddRIclWlEm7PpXJYvkIEA+YgQEAAACEKD09Xdddd12b+61evToKadAWW8LlU4F4wgwMAAAAIEQ//elPQ9pv6dKlEU6CkBQXSn3SpMwzXCcBEAYUMAAAAIAw2Ldvn/bv3+86Br5gg/WyJUUyWdkyxriOAyAMKGAAAAAAHbB06VJ99NFHkqS33npLCxYs0MKFC7VixQrHySBJ2l4qVR2Rss51nQRAmFDAAAAAADpg06ZNGjZsmCTp1Vdf1fe//339+Mc/1iuvvOI4GaQvLp9qfDJjznEdBUCY0MQTAAAA6IC6ujr5/X5VVFToyJEjOuussyRJhw8fdpwM0heXTx0yXKZHT9dRAIQJBQwAAACgA4YMGaKXX35ZBw4c0PjxJy7TWVFRoe7duztOBnu0UtpeKnPVbNdRAIQRS0gAAACADpg7d6527Nih48eP64YbbpAkbdmyRRdeeKHjZLAlGyUblMka7zoKgDBiBgYAAADQDn/729+UnZ2tgQMH6l//9V8bbZs8ebImT57sKBkaFBdIKanSkBGukwAIIwoYAAAAQDts27ZNf/zjH9WjRw+NHz9e2dnZGjVqFJfq9AhrrWxxgczoc2SSklzHARBGFDAAAACAdrj99tslSTt27FBBQYF++9vfavfu3crKytL48eM1btw49erVy3HKBLZ7h3SoQsrKdp0EQJhRwAAAAAA6YPDgwRo8eLBmzpypqqoqFRUVqbCwUC+88IL69eun6667TuPGjXMdM+HYTQWSRP8LIA5RwAAAAAA6KSUlRVOmTNGUKVMkSVu3bnWcKHHZ4gIpY7BMWsB1FABhRgEDAAAA6KDNmzdr+/btqqmpafT4rFmzHCVKbPZYjVRaLHPpVa6jAIgAChgAAABAB/ziF7/Qu+++q7POOkvJyckNj9PM06Etm6S6Ohn6XwBxiQIGAAAA0AHvvPOOcnNzlZaW5joKvmCLC6XkZGlElusoACLA5zoAAAAAEIsCgYC6dOniOgZOYTcVSCO/ItMlue2dAcQcZmAAAAAAHTB37lw99dRTuuCCC9S7d+9G28aMGeMoVeKyZfukfbtkpv2T6ygAIoQCBgAAANABH3/8sQoLC7V58+ZGPTAk6Wc/+5mjVInLFhdKkszZXD4ViFcUMAAAAIAO+O1vf6t77rlHY8eOdR0F+mL5SHp/aUCm6ygAIoQCBgAAANABXbt27dRSkWXLlqmgoEC9e/dWbm5uk+3WWj377LMqLCxU165dNW/ePA0dOrQzkeOWrauTPtwoc95FXAUGiGM08QQAAAA64Prrr9dzzz2nQ4cOKRgMNvoTimnTpum+++5rcXthYaH27t2rxx57TLfffruefvrpcEWPPx9/KNVUy2SxfASIZ8zAAAAAADrgZJ+Lv/71r022LV++vM3njxkzRvv3729x+4YNG3TRRSdmFIwcOVJHjx7VwYMH1bdv346HjlO2uFDy+aSzWM4DxDMKGAAAAEAHPP744xF9/YqKCgUCgYb76enpqqiooIDRDFtcKA07Syalh+soACKIAgYAAADQAf369XMdoUFeXp7y8vIkSYsXL25U+AiF3+9v93Oipa1swUMVOvDpVvWYc7tSo/zf4NXz5tVcEtk6wqu5pOhno4ABAAAAhOh3v/udbrjhhjb3e/HFFzV79uxOHSstLU1lZWUN98vLy5WWltbsvjk5OcrJyWm4f+rzQhEIBNr9nGhpK1swf6UkqfrMUaqJ8n+DV8+bV3NJZOsIr+aSIpctIyOj2cdp4gkAAACE6LXXXtP+/fu1b9++Vv+8/vrrnT7WhAkTtGrVKllrtWXLFqWkpLB8pDnFBVJqL2nwMNdJAEQYMzAAAACAEB07dkx33nlnm/t16dKlzX2WLl2qkpISVVZWau7cuZo9e7bq6uokSTNmzFB2drYKCgp01113KTk5WfPmzet0/nhjg0HZ4kKZMdkyPn43C8Q7ChgAAABAiEK5ukio5s+f3+p2Y4xuu+22sB0vLn22Xao8LJ3N5VOBRECZEgAAAEBMssUFkiSTNc5xEgDRQAEDAAAAQEyyxYXS6WfK9KI3CJAIKGAAAAAAiDn2aKW0tUTm7HNdRwEQJRQwAAAAAMQcu3G9FAzKZJ/vOgqAKKGAAQAAAHRQTU2NysvLVVNT4zpKwrFF+VKfdGnIcNdRAEQJVyEBAAAA2mHHjh3Ky8tTQUGBDhw40PB4//79NW7cOF122WUaPHiww4Txzx47JhUXyFyQI2OM6zgAooQCBgAAABCipUuXaufOnZoyZYruvPNOZWZmqnv37qqurtauXbtUUlKixx57TIMGDWrzMqnohJJC6fhxlo8ACYYCBgAAABCiqVOn6txzmzaNTE1N1ahRozRq1Chdc801eu+99xykSxy2MF9KSZVGZLmOAiCK6IEBAAAAhOjU4kVpaWmz+2zdurXZIgfCw9bXy76/XmbsRBk/v48FEgkFDAAAAKADHnzwwWYff+ihh6KcJMFs2SQdrZTJnuw6CYAoo2QJAAAAtEMwGJQkWWsb/py0b98+JSUluYqWEGzRWqlLspSV7ToKgCijgAEAAAC0w4033thw+4Ybbmi0zefz6Zprrol2pIRhrT1x+dSsbJmu3VzHARBlFDAAAACAdnj88cdlrdWPfvQj/cd//EfD48YY9erVS8nJyQ7Txbkd26SKMpmr/9l1EgAOUMAAAAAA2qFfv36SpGXLljlOknhsQb5kfDLnTHQdBYADNPEEAAAAQvTLX/5Shw4danWfQ4cO6Ze//GWUEiUWW/iuNDJLJrWX6ygAHGAGBgAAABCijIwM3XvvvRo0aJBGjx6tjIwMde/eXdXV1dqzZ49KSkq0e/duzZo1y3XUuGP37pL2fCZz8RWuowBwhAIGAAAAEKLLLrtMl1xyiTZs2KDCwkKtX79eVVVV6tGjhwYPHqzLLrtM5557LlciiQBblC9JMuO4fCqQqChgAAAAAO3g9/s1efJkTZ7MF+loskVrpcHDZNL7uY4CwBF6YAAAAAAd8Nxzz2nr1q2uYyQEe6hC2vahTDZFIyCRMQMDAAAA6ABrrR5++GF17dpVF154oS688EJlZGS4jhWXbNFaSaKAASQ4ChgAAABAB3z729/Wt771LW3atEl///vf9e///u/q37+/pk6dqquuusp1vLhii/Kl/qdJGYNdRwHgEEtIAAAAgA7y+XwaO3as5s2bp9zcXPXs2VPPP/+861hxJXj0iPThBzLZk2WMcR0HgEPMwAAAAAA6qKamRuvWrdPq1atVUlKiMWPG6I477nAdK64cK1gj1ddx9REAFDAAAACAjliyZIkKCws1dOhQXXDBBbrjjjvUq1cv17HizrH8VVKvPtLQUa6jAHCMAgYAAADQAcOGDdPNN9+sQCDgOkrcsrXHdbwgX+a8qTI+Vr8DiY4CBgAAANABX//6111HiH+bN8rWVMnH1UcAiCaeAAAAADzKFubLpPSQzhrrOgoAD6CAAQAAAMBzbLBeduM6JY8/X8bfxXUcAB5AAQMAAACA92z9UKo8rG6TL3adBIBHUMAAAAAA4Dm2MF/y+5VM/wsAX6CAAQAAAMBTrLWyRfnS6HHypfRwHQeAR1DAAAAAAOAtOz+RyvbJMPsCwCkoYAAAAADwFFuYLxkjc85E11EAeAgFDAAAAACeYgvzpWGjZXr1dR0FgIdQwAAAAADgGfbAXmnndpnsSa6jAPAYChgAAAAAPMMWrZUkmXH0vwDQGAUMAAAAAJ5hC9+VMs+Q6X+a6ygAPIYCBgAAAABPsJ8fkrZ+KJN9vusoADyIAgYAAAAAT7Ab10k2yOVTATSLAgYAAAAAT7CF+VJ6f+n0M11HAeBBFDAAAAAAOGdrqqTNG2WyJ8sY4zoOAA+igAEAAADAvU0FUl0tVx8B0CIKGAAAAACcs4VrpdRe0ojRrqMA8CgKGAAAAACcsnW1sh9skDnnPBlfkus4ADyKAgYAAAAAtz78QKo+ytVHALSKAgYAAAAAp2xRvtS1mzT6HNdRAHgYBQwAAAAAzthgULZonZQ1Xia5q+s4ADyMAgYAAAAAd7ZvkQ5XsHwEQJsoYAAAAABwxhbmS0lJMmMnuI4CwOP8rgMAAAAAiaioqEjPPvusgsGgpk+frpkzZzbafuDAAf3sZz/T559/rtTUVN15551KT093lDYyrLUnChijviKTkuo6DgCPYwYGAAAAEGXBYFDPPPOM7rvvPj366KNavXq1du7c2Wif559/XhdddJEeeeQRXXvttfrNb37jKG0E7flM2r+b5SMAQkIBAwAAAIiyrVu3auDAgRowYID8fr+mTJmi9evXN9pn586dOvvssyVJWVlZ2rBhg4uoEWXz35KMT2bcJNdRAMQATywhKSsr0xNPPKFDhw7JGKOcnBxdeeWVrmMBABB2OTl+/e53rlMAcK2ioqLRcpD09HSVlpY22ueMM87QunXrdOWVV2rdunWqrq5WZWWlevbs2eT18vLylJeXJ0lavHixAoFAu/L4/f52P6ezbG2tytasUPKEKeozfFSL+7nIFiqvZvNqLolsHeHVXFL0s3migJGUlKSbbrpJQ4cOVXV1tRYtWqSxY8dq0KBBrqMBABBW77zD5EcAobnpppv0i1/8QitXrtTo0aOVlpYmn6/5nyE5OTnKyclpuF9WVtauYwUCgXY/p7Pshr8rePigas+/tNVju8gWKq9m82ouiWwd4dVcUuSyZWRkNPu4JwoYffv2Vd++fSVJ3bt3V2ZmpioqKihgAAAAIC6lpaWpvLy84X55ebnS0tKa7PNv//ZvkqSamhqtXbtWPXr0iGrOSAqu+ouU1k/KynYdBUCM8EQB41T79+/X9u3bNXz48CbbOjs1Lhy8PH0nUTEm3sJ4eAvj4Q05Of5GMy8yM0/8VmHq1KDy8upcxYL4jHhRoozJsGHDtGfPHu3fv19paWlas2aN7rrrrkb7nLz6iM/n08svv6xLLrnEUdrws/t3S5s3ynx9jowvyXUcADHCUwWMmpoa5ebm6pZbblFKSkqT7Z2dGhcOXp6+k6gYE29hPLyF8fCGU3teZGZmaNeu3Q33GR63+Ix4j6sxaWm6cqQkJSXp1ltv1UMPPaRgMKhLLrlEp59+upYvX65hw4ZpwoQJKikp0W9+8xsZYzR69Gh95zvfiWrGSLKr3pR8PpkLL3MdBUAM8UwBo66uTrm5uZo6daomTaILMQAgceXm9tTChZWuYwCIsPHjx2v8+PGNHrv++usbbk+ePFmTJ8ff5UVtXa3smr9JY8+T6ZPe9hMA4Aue6CRmrdWTTz6pzMxMXXXVVa7jAAAQMVOnBtvcZ8mSplcYAIB4YQvzpcrD8l18uesoAGKMJ2ZgfPTRR1q1apUGDx6su+++W5J04403NqlIAwAQ6/Ly6lg2AiCh2bffkNL7S2No3gmgfTxRwDjrrLP04osvuo4BAIAzubk9G828ONnoczUVhZAAACAASURBVMGCSpaTAIgbdu8u6aMPZGZ+U6aFS8ICQEv4qQEAQITk5oa+FGThwkrt2rW7ocHnydutFS/a8/oA4AX2nTelpCSZC3La3hkAvoQCBgAAERLpXhb0ygAQS2ztF807zzlPpk+a6zgAYhAFDAAAPGbBApaMAIg/tmCNdORz+S66wnUUADGKAgYAAGGUm9tTmZkZDT0sTt5u73KSSL4+ALhgV/1F6jdQGn2O6ygAYpQnmngCABAvFi78R9PNzMyMhp4WsfL6ABAJds9OacsmmVk307wTQIfx0wMAgDjGzAwAXmDf+cuJ5p1TpruOAiCGUcAAACBCIt3LIpTXp9EnANds7XHZNSukcZNkevd1HQdADKOAAQBAhLTWyyIWXh8AwsG+t0Y6WknzTgCdRgEDAIA4Q6NPAF5iV71xonnnWWNdRwEQ42jiCQBAnKHRJwCvsLt3SKUlMt/4Fs07AXQaP0UAAAiDWJ/dEOv5AXiTfedNKclP804AYUEBAwCAMPBqs8xQG4l6NT+A2GWPH5Nds0Ime7JMrz6u4wCIAxQwAACIYzT6BOCKfW+NVHVE5qLLXUcBECcoYAAA0EGx3iwz1vMD8Da76g2pfwbNOwGEDU08AQDooFhvlhnr+QF4l921Q9q6Webab8sY4zoOgDjBDAwAABAyZmcACIVd9Ybkp3kngPCigAEAQBiE2izTq2j2CSBc7LFjsvlvyYyfItOzl+s4AOIIBQwAAMIg1ptlxnp+AN5h3/u7VHWU5p0Awo4CBgAAaBXNPgG0h131F2lgpjTybNdRAMQZChgAALRTon1xX7iwUrt27W5o8nnydkuzNhLt/AD4B7vzE2nbhzJTL6d5J4Cwo4ABAEA70QeidZwfIHGdaN7ZRWbKpa6jAIhDFDAAAEDIYr1ZKYDIOdG8c6XMuVNkUmneCSD8KGAAABAC+kCc0NqyEc4PkNjshnek6iqZi65wHQVAnPK7DgAAQCxYuLCy4ct7ZmZGQz8InMD5AWDffkM67XRpxBjXUQDEKWZgAAAAJ5idAcQP+9l2afsWmYtm0LwTQMRQwAAAoJ3oA9G6UM8PzT6B+NHQvPN8mncCiBwKGAAAtFNLfSBwAucHSCy2pvpE884JF8r0oDAJIHIoYAAAgKih2ScQf+z6d6SaapmLL3cdBUCco4knAACIGpp9AvHHrs6TMgZLw0a7jgIgzjEDAwCANjA7wB3OPeBt9lC5tO1DmfMuonkngIijgAEAQBtoNhkZoTT75NwD3maL1kmSTPZkx0kAJAIKGAAAwAmafQKxzxblS/1Pk0473XUUAAmAAgYAAM2g2aQ7nHsgNtiqo9KHH8iMm8zyEQBRQRNPAACaQbNJdzj3QGywxQVSfZ1M9iTXUQAkCGZgAACAmMfsDMCBwnypZ29p6CjXSQAkCAoYAAC0IZRmk4iMUM89zT6B6LK1tbIfbJAZN0nGl+Q6DoAEQQEDAIA20GzSHc494FEffSDVVMuMY/kIgOihgAEAAGISzT4Bd2xRvtS1mzT6HNdRACQQmngCAICYRLNPwA0bDMoWrZOyxst0SXYdB0ACYQYGAACn4Lf38YuxBcLkk1LpcAVXHwEQdRQwAAA4Bc0gY1MozT4ZWyA8bNFayeeT+cpE11EAJBgKGAAAIObR7BOIHlu0Vhr1FZkeqa6jAEgwFDAAAAmPZpDxi7EFwsvu3Snt+YyrjwBwgiaeAICERzPI+NWRsc3N7cmMDqAFtmitJFHAAOAEMzAAAABOQa8MoGW2aK10xnCZtH6uowBIQBQwAAA4RSjNIBGbGFugc+yhCunjj2TGnec6CoAExRISAABOwdKB+NXa2Obm9mw08+Jkz4wFCyp5TyBiioqK9OyzzyoYDGr69OmaOXNmo+1lZWV64okndPToUQWDQc2ZM0fjx493lFay76+TrJUZN9lZBgCJjQIGAABIePRBQbQFg0E988wzuv/++5Wenq57771XEyZM0KBBgxr2+eMf/6jzzz9fM2bM0M6dO/Vf//VfbgsYReukfgOlzDOcZQCQ2FhCAgBIWFyJAp3B+wedsXXrVg0cOFADBgyQ3+/XlClTtH79+kb7GGNUVVUlSaqqqlLfvn1dRJUk2ZoqaXORzLhJMsY4ywEgsTEDAwCQsJYs4WoTaCrUXhm8f9AZFRUVSk9Pb7ifnp6u0tLSRvtcd911evDBB/XGG2/o2LFj+v73v9/i6+Xl5SkvL0+StHjxYgUCgXbl8fv9rT6nZvUKHa6rU59plyu5na/dWW1lc8mr2byaSyJbR3g1lxT9bBQwAAAATkFRAl6xevVqTZs2TV/72te0ZcsW/fSnP1Vubq58vqaTqHNycpSTk9Nwv6ysrF3HCgQCrT4nuOqvUmovHQ6cJtPO1+6strK55NVsXs0lka0jvJpLily2jIyMZh9nCQkAIKHk5vZUZmZGQ5PGk7dZDoBQ8P5BuKSlpam8vLzhfnl5udLS0hrts2LFCp1//vmSpJEjR6q2tlaVldEvsNm6WtkPNsicc56MLynqxweAkyhgAAASysKFldq1a3dDk8aTt/mtO0LB+wfhMmzYMO3Zs0f79+9XXV2d1qxZowkTJjTaJxAIaNOmTZKknTt3qra2Vr169Yp+2C2bpOqjMuMmRf/YAHAKlpAAAABESG4ufTLQvKSkJN1666166KGHFAwGdckll+j000/X8uXLNWzYME2YMEE333yznnrqKf35z3+WJM2bN89JA01btFZK7iqNGRf1YwPAqShgAAASVqjNGoHmhPL+odEnWjN+/Pgml0W9/vrrG24PGjRIDzzwQLRjNWKtlS1cK2VlyyR3dZoFAFhCAgBIWHyxRGfw/kFC+HSrdKhcZtxk10kAgAIGAABAONHoE/HEFq6VfD6ZsRPa3hkAIowlJAAAAGG0cGFlw+yMzMyMhoafQCyyRfnSiCyZVAfNQwHgS5iBAQBICPz2G17G+xNeZPftlnbvkMlm+QgAb6CAAQBICEuW8AUR0Rdqo1jen/AiW7RWkrh8KgDPoIABAAAQITT6RCyzRWul08+USe/vOgoASKKAAQCIYzRThJfx/oSX2c8PSds2c/URAJ5CE08AQNyimSK8rL3vz9zcnszoQNTYjeska+l/AcBTmIEBAAAQA+iTgWiyRWul9P7SoCGuowBAAwoYAICEEGozRcAF3p/wEltTLZUUyWRPljHGdRwAaEABAwCQEJh6Dy9r6f1Jnww4UVwo1dXS/wKA59ADAwAAwKPo4wIXbFG+1KOnNHy06ygA0AgzMAAAcYnfUCNR8d5HZ9i6Otn3N8iMnSiTlOQ6DgA0QgEDABCXaHiIeBNqnwze++iU0mKp6ghXHwHgSRQwAAAAYgB9XBANtmitlJwsjcl2HQUAmqCAAQCIGzQ8RKLivY9wsNae6H8xJluma1fXcQCgCZp4AgDiBg0Pkah47yMsdnwsVZTJXP3PrpMAQLOYgQEAAJBgmJmB5tiifMn4ZMZOdB0FAJpFAQMAEJdCbXgIxJtQ3vs0+kRzbGG+NGK0TM9erqMAQLMoYAAA4hIND5GoeO+jI+r27pJ2fSozjquPAPAuChgAAAAJgEafaM2xde9Iksy4SY6TAEDLaOIJAACQAGj0idYcW7tKGjREpt9A11EAoEXMwAAAxDx+gwxEBp+txGArD6v2w/dZPgLA8yhgAABiHg0JgfYJtcktn60EseNjyZckk83yEQDexhISAACABEOjT5zKZGWr369eU/mRKtdRAKBVzMAAAMQkGhICkcFnKzH5uveQMcZ1DABoFTMwAAAxiYaEQGTw2QIAeBUzMAAAANApzM4AAEQDBQwAQMwLtSEhgPah2ScAwEsoYAAAYl5HGxLawnwFX/u9bG1tmBMB8YFmnwAAL6GAAQBISDYYVPDl52XzV0pJSa7jADGHZp8AgGijgAEAiClh+3JU+K605zOZq66X8fG/Q6C9Fi6s1K5duxuafJ683dKsDQobAIDO4l9sAICYEo619jYYVPDV5dLATJkJF4QhFYC20CcDANBZFDAAAIln4zpp5ycyV86W8bF8BOgsGukCAKKBAgYAwPPCudbeWqvgq7+T+g2UOe+icEcFElJry0bokwEACBe/6wAAALRl4cLKhi9ImZkZDWvuO+SDDdKOj2VuuUuG5p1ARIX1swsASHiemYGxbNky3XbbbVq4cKHrKACAOHVi9sVyKb2/zKRpruMAaMYDD1BYBAA0zzMFjGnTpum+++5zHQMA4HGdWmtfXCht3yJz5bUyfiYhAtEU6mf3wQcpYAAAmueZf72NGTNG+/fvdx0DAOBxLa21b0tD74u0gMz508OcCkBbOvrZRcdkZGRE5TnRQrb282ouiWwd4dVcUnSzeaaAEYq8vDzl5eVJkhYvXqxAIBD1DH6/38lx0TLGxFsYD29hPP7h+PsbdHDbh+p5+0KlnHaasxyMibcwHt7wwANJjWZenGz6ef/99fr+9+tdxUooixYt0uLFi13HaBbZ2s+ruSSydYRXc0nRzxZTBYycnBzl5OQ03C8rK4t6hkAg4OS4aBlj4i2Mh7cwHv9Q/+ufS33SdHTc+apyeE4YE29hPLzh//7fE3+kps0+ozU8Xv7tJgDgBM/0wAAA4MvCdalF+9EmacsmmctnyXRJDstrAgAAILooYAAAPGvJkvAUMIJ/Xi716iMz9fKwvB6AyLn/fpaMuHDqLGevIVv7eTWXRLaO8GouKfrZjLXWRvWILVi6dKlKSkpUWVmp3r17a/bs2br00ktbfc7u3dG/ljhTTb2HMfEWxsNbYn08vjyVvCPs1s0K/vc9Mtd9W74Z14QpWcfF+pjEG8bDe1yNCUtIAMD7PNMDY/78+a4jAAA8IDe3Z6OZFyeb+S1YUNmhqxgE/7xcSu0lc/E/hS0jAAAAos8zBQwAAKQTl1o8Wajo7AwMu32LtKlAZta3ZLp2C1dEAAAAOEABAwAQt4KvLpd69JS5hNkXACBJRUVFevbZZxUMBjV9+nTNnDmz0fba2lo9/vjj+vjjj9WzZ0/Nnz9f/fv3j3iusrIyPfHEEzp06JCMMcrJydGVV17ZaJ/i4mL95Cc/acgzadIkXXvttRHPJkl33HGHunXrJp/Pp6SkpCaXjbTW6tlnn1VhYaG6du2qefPmaejQoRHNtHv3bj366KMN9/fv36/Zs2frq1/9asNj0Txny5YtU0FBgXr37q3c3FxJ0pEjR/Too4/qwIED6tevn773ve8pNTW1yXNXrlypl156SZI0a9YsTZs2LeLZnn/+eb333nvy+/0aMGCA5s2bpx49ejR5bltjH+5cL774ov72t7+pV69ekqQbb7xR48ePb/Lctj7Lkcj26KOPNrRxqKqqUkpKih5++OEmz43kOaOAAQDwrAUL2r9k5CS7Y5v0/nqZr/+zTLeUMKYCgNgUDAb1zDPP6P7771d6erruvfdeTZgwQYMGDWrYZ8WKFerRo4d++tOfavXq1fr1r3+t733vexHPlpSUpJtuuklDhw5VdXW1Fi1apLFjxzbKJkmjR4/WokWLIp6nOT/84Q8bvlR+WWFhofbu3avHHntMpaWlevrpp/XjH/84onkyMjIavjwGg0H9y7/8i84777wm+0XrnE2bNk1XXHGFnnjiiYbHXnnlFX3lK1/RzJkz9corr+iVV17RN7/5zUbPO3LkiP7whz80fMldtGiRJkyY0GyhI5zZxo4dqzlz5igpKUkvvPCCXn755SbZTmpt7MOdS5K++tWv6uqrr27xeaF8liOR7dSfBb/61a+UktLyv68idc64CgkAwLM60vPipOCry6XuPWQuvSqMiQAgdm3dulUDBw7UgAED5Pf7NWXKFK1fv77RPhs2bGj47ffkyZO1adMmRaPnf9++fRtmLHTv3l2ZmZmqqKiI+HHDZcOGDbroootkjNHIkSN19OhRHTx4MGrH/+CDDzRw4ED169cvasf8sjFjxjQpOqxfv14XX3yxJOniiy9u8n6TTswkGDt2rFJTU5WamqqxY8eqqKgo4tnOOeccJSUlSZJGjhzp5P3WXK5QhPJZjmQ2a63effddXXDBBWE9ZiiYgQEAiDt253apMF/mazfIpDSdDgoAiaiiokLp6ekN99PT01VaWtriPklJSUpJSVFlZWVEfpPakv3792v79u0aPnx4k21btmzR3Xffrb59++qmm27S6aefHrVcDz30kCTpsssua3LpyIqKCgUCgYb76enpqqioUN++faOSbfXq1S1+mXR5zg4fPtxwDvr06aPDhw832efL78u0tLSoFxNWrFihKVOmtLi9tbGPhL/85S9atWqVhg4dqptvvrlJISGUz3Ikbd68Wb1799Zpp53W4j6ROmchFzCee+45TZs2TUOGDAnbwQEAOCk3t2enZlycyr76otStu8z0lqdfAgC8p6amRrm5ubrllluaTE8/88wztWzZMnXr1k0FBQV6+OGH9dhjj0Ul1wMPPKC0tDQdPnxYDz74oDIyMjRmzJioHLstdXV1eu+99zRnzpwm21yesy8zxsgY4+TYrXnppZeUlJSkqVOnNrs92mM/Y8aMhj4ly5cv169+9SvNmzcvYsfriNYKZlJkz1nIS0iCwaAeeughLVy4UK+88orKy8vDEgAAAEmNLp3aGXb3DtmCNTKXXiXTI3zrZwEg1qWlpTX6N3x5ebnS0tJa3Ke+vl5VVVXq2TM8P5/bUldXp9zcXE2dOlWTJk1qsj0lJUXdup24otT48eNVX1+vzz//PCrZTp6n3r17a+LEidq6dWuT7WVlZQ33mzu3kVJYWKgzzzxTffr0abLN5TmTTpyvk0tpDh482OxMni+/LysqKqJ27lauXKn33ntPd911V4vFlbbGPtz69Okjn88nn8+n6dOna9u2bc1mauuzHCn19fVat25dqzNWInnOQi5g3HrrrXrqqac0Z84cffLJJ/re976nBx54QG+//bZqamrCFggAgM6wf/69lNxVJufrrqMAgKcMGzZMe/bs0f79+1VXV6c1a9ZowoQJjfY599xztXLlSklSfn6+srKyovJbc2utnnzySWVmZuqqq5rvXXTo0KGGfhxbt25VMBiMSnGlpqZG1dXVDbfff/99DR48uNE+EyZM0KpVq2St1ZYtW5SSkuKJ5SOuztlJEyZM0Ntvvy1JevvttzVx4sQm+4wbN04bN27UkSNHdOTIEW3cuFHjxo2LeLaioiL96U9/0j333KOuXbs2u08oYx9up/ZOWbduXbNLfkL5LEfKBx98oIyMjEZLWE4V6XNmbAe78nz22Wd67LHHtGPHDiUnJ+uCCy7Q7Nmzo1b5kdRwCZdoCgQCjaqrcI8x8RbGw1u8Ph65uT2bnXmxYEFlh5aT2L07FfzBd2VmzJTv2lvCkDD8vD4miYbx8B5XY5KRkRH1Y7pQUFCgX/7ylwoGg7rkkks0a9YsLV++XMOGDdOECRN0/PhxPf7449q+fbtSU1M1f/58DRgwIOK5PvzwQ/3gBz/Q4MGDGwomN954Y8N7YcaMGXrjjTf05ptvKikpScnJybr55ps1atSoiGfbt2+fHnnkEUknfvt84YUXatasWXrzzTcbsllr9cwzz2jjxo1KTk7WvHnzNGzYsIhnq6mp0bx58/T44483LLk5NVc0z9nSpUtVUlKiyspK9e7dW7Nnz9bEiRP16KOPqqysrNFlVLdt26a//vWvmjt3rqQTPShefvllSScuo3rJJZdEPNvLL7+surq6hv4SI0aM0O23366Kigo99dRTuvfee1sc+0jmKi4u1ieffCJjjPr166fbb79dffv2bZRLav6zHE7NZbv00kv1xBNPaMSIEZoxY0bDvtE8Z+0qYFRVVSk/P1/vvPOOPv30U02aNEkXX3yxAoGAXn31VW3atKkhbDRQwIDEmHgN4+EtsTQemZkZ2rWrcz/Xg794VPa91fL919MyvZpOpfWCWBqTRMB4eA8FDABAS0Ju4pmbm6uNGzdq9OjRuuyyyzRx4kR16dKlYfvNN9+sW265JRIZAQBok92/R3bt2zKXfs2zxQsAAAB0XMgFjBEjRug73/lOs81hJMnn8+l//ud/whYMAJBYFizo3BVI7Ot/kHxJMpdfE6ZEAAAA8JKQCxhXX932pehaan4CAEBbOnMJVVu2T/bdFTIX/5NMn+j1YgIAAED0hHwVEgAAvMjW1yv47P+TkpJkLg9vAysAAAB4BwUMAEBMs396QdqySeabd8ikBVzHAQAAQIRQwAAAOJGb2/nr0NuN62Vf/6PMRZfLd354L7kGAAAAb6GAAQBwYsmSzhUw7IG9Cv5iiTR4qMwN/ydMqQAAAOBVFDAAADHH1tYq+NRPJCv55i6S6ZLsOhIAAAAijAIGACBqcnN7KjMzQ5mZGZLUcLu9y0nsi09Ln26V79Z/lek3MBJRAQAA4DEhX0YVAIDOWriwsuFyqZmZGdq1a3e7XyOYv1J25esyl18jM25yuCMCAADAo5iBAQCIGXb3Dtnnn5BGjJGZeZPrOAAAAIgiChgAACcWLKhs1/62plrBJ/9b6tpNvtvvlvEziRAAACCR8K8/AIATJ5eShMJae2Lmxd5d8i34T5k+6RFMBgCxb/fu9i3RCwQCKisri1CaziFb+3k1l0S2jvBqLily2TIyMpp9nBkYAADPs2+/LrtulczX58icNdZ1HAAAADhAAQMAEHHtvcrIqez2UtnlT0tfmSDzT9eGMRUAAABiCQUMAEDELVnSsQKGPVqp4FP/LfXqK9+t82V8/G8LAAAgUfEvQQCAJ9lgUMFnHpUOVcg39x6Z1F6uIwEAAMAhChgAgIjIze2pzMwMZWaeaMJ08naoy0nsX16SPtggc/13ZM4cGcmoAAAAiAFchQQAEBELF1Y2XGkkMzNDu3aF3hHffvSB7MsvyEycKjPtykhFBAAAQAxhBgYAwFPsoQoFf/6wNCBD5uY7ZIxxHQkAAAAeQAEDABBxCxZUhrSfra9X8H8elmqq5Zu7SKZbSoSTAQAAIFZQwAAARNzJpSRtsa+8IG0plrnpDpnMwRFOBQAAgFhCAQMA4Am2aK3sG3+UufgK+SZPcx0HAAAAHkMBAwDgnD2wV8Fnl0qDh8lcf5vrOAAAAPAgrkICAAir3NyeIS8ZkSRbe1zBJ/9bkuSbe49Ml+RIRQMANMN+UqqKR78ve913ZAYNcR0HbbC1x2V//6xs+f4W9zmYnKz648cjFMB26ukHu3TpRDbbOINtZVvDwy083oyKLl1UX1vbwr6dOHYnVfj9qq+ra3mHUI5zsil6o7+NZPSl+83tp6a3v3jiweQvzlmTfU787bvocpnsyW3nCxEFDABAWC1Z0s4CxvKnpR3b5Lvj32X6DYxgMgBAs2prVVuyUb7PD0oa4joN2mD/9GvZt/4snX6mZJqfUB/0+6XWvvB2VieuEBbs4pfq6sN37Ba/YKvJl2m1FPvkfsGgZINqfucvvtybZp4XyrE7wXTxS7VtjGdrh7Gn3Di1oHNq4cPaE9uDweb3b/R6/3jM1vmlUwsYX95+/FgYzsA/UMAAADgTzH9L9u03ZK74hsy4Sa7jAEBi6t79xN/V1W5zoE22tET2zVdkps6Q7+bvtrhfeiCgsrKyKCYLnZezpXk0W1+P5pKif87ogQEA6LTc3J7KzMxQZmaGJDXczs3t2eJz7I5tss8vk0Zmycz8ZrSiAgC+rOuJAoatqXIcBK2xNdUn+kWl95eZfavrOIATzMAAAHTawoWVDctGMjMztGvX7lb3tyVF/5+9O4+Puj7wP/7+TBIyMyEkhHCYIFHCHUEwcnnUK4Wqu21tXWtZtPrQtvZQW6wVty20pV3xgG61tttHbX9eu1vrbi+PXtEeXihHwn1IVJAjQEIgkEwSZubz++ObBJAAmWRmvt+ZvJ6Ph4+ZTOb7/b6HMZC88zkU/cl9Uk6ufJ+9WyYjIxkxAQBdCQSd2xZGYHiZffb/SXV75Pva92X8QbfjAK6gwAAAJFX09Zdln3xEGjZcvjsWyuQXuB0JAPo2f8cUEkZgeJVdt1L2H3+UmfVxmTHnuB0HcA0FBgAgrubN63oBT2ut7Au/kv3df0njJsn3hXtlgjlJTgcA+CCTmSVl9ZOYQuJJtumwok88Ip1xJlMu0edRYAAA4qqrHUhsJCL7Xz+RfeXPMjMulfnM7c43zAAATzCBIFNIPMr+90+lQwfl+/I32WocfR4FBgAgoWxLSNGfPiCtWylz1b/IfHyuTC+2XwMAxJ8vmKMou5B4jl3xquxbf5f56ByZklFuxwFcR4EBAEgYe7BB0Ye/K73/rswNX5TvQx9xOxIAoAsmEGQXEo+xBxsU/a+fSCWjZK681u04gCewjSoAoMdOuU3q7h2K3ne3VLtDvi9/g/ICADzMBHKYQuIh1lpFn/yR1Noq3y1flcnk986ARIEBAOiFpUu7LjDslvWKLv661NYq393/LjNpapKTAQBiYYI5LOLpIfbVv0hrlst84gaZM850Ow7gGVR5AIC4siteVfTnP5AKhzjbpA4e5nYkAMBp+AJBtlH1CFu3R/aZn0tjJ8pc/s9uxwE8hREYAICYLFmSq+LiIhUXF0lS5/0lD/VX9M+/dRbsPGuUfPfcT3kBACmCKSTeYKNRRf/fDyUj+W66Q8bHj2vAsRiBAQCIyV13HercKrW4uEg7d+6SjUZkf/UL2Wefk8ovkO+WeWz1BgApxJlCQoHhNvvSc9KWdc5244VD3Y4DeA4FBgCgV2xbq6KPLZGqlslUfEzmX27mN0YAkGJMICgdaZMNh1kw0iV29/uyv35SmjRV5sIKt+MAnsTfTgCAHvu3O3YouuSb0rtbZD51q3wVH3U7EgCgB3yBoHOnNSRlnnyHKSSGDYed9aP8fvlu/LKMMW5HAjyJX5EBAHrE7t2t2xq/KL3/rnyfv4fyAgDiIBqN6utf/7oWL1583OO/jjpE9QAAIABJREFU+MUvdMMNNyTsuiaY49xhIU9X2BeflbZtlW/uF2XyBrodB/AsRmAAAGJm392i6COLpGhUvnmLZEaNdzsSAKSFF198UcXFxQqFjq5HUVNTo6ampoRe13SMwGAr1aSz27bKvvgrmWmXyJRf6HYcwNMYgQEA6JYlS5whxbb6TUUf+jcp2y/f/PspLwAgTurr67Vq1SpdccUVnY9Fo1E9/fTTmjt3bkKv3TkCg4U8k8q2tTpTR3LzZOZ83u04gOdRYAAAumXp0lxF//qioj++TzpjhHz3PiAzbLjbsQAgbTz++OOaO3fucesf/PGPf1R5ebkGDkzstAIT6JhCQoGRTPa3T0u735fvM3fI5PR3Ow7geUwhAQCclo1GNX/sI7L//YQ0aap8n7tbJtvvdiwASBsrV65UXl6eRo4cqfXr10uS9u/frzfeeEPf/va3T3t8ZWWlKisrJUmLFy9WYWFhTNe3rc4UldysDPljPDbRMjMzY349ydKbbG3rqtRQ+XsFZl+jAZfO8kyuRCNb7LyaS0p+NgoMAMBJLVmSqx/9R7YemvQdfbH0T3pq2ye14A936862kO6665Db8QAgbWzevFkrVqxQVVWV2traFAqFdNdddykzM1N33HGHJKmtrU233367HnnkkROOr6ioUEXF0a036+rqYrr+wPZSunHfXh2O8dhEKywsjPn1JEtPs9mWZkX/4ztS4VC1/tP1cX996fhnlgxezebVXFLishUVFXX5OAUGAOCk5n1ht75i75M2r9XiTV/Wv/3jw7rJ7HU7FgCknTlz5mjOnDmSpPXr1+u5557T/Pnzj3vODTfc0GV5EQ9Hp5CwiGcy2L/8Xtq/T76v3yfjD7gdB0gZrIEBAOiSXV+l6Pe/Jm3dKHPLPP34nZvYlx4A0lTnD9HsQpIUdvNaaUSpzKgJbkcBUgojMAAAx7F7dyn6q19Iq9+SBg+T76vfkRk7UfPmMWUEAJKhrKxMZWVlJzz+1FNPJeyaJiNDyvaziGcS2HBYeneLzMXxXfcC6AsoMAAAkpz5uPb5X8lW/l7KzJL55GdkrvioTFaWJLHmBQCkO39QaqXASLgd70ptrVIp25ADsaLAAIA+zkajsm/8VfY3T0oHG2QuuELmmhtk8gvcjgYASKZAgDUwksDWbJIkmdJxLicBUg8FBgD0YbZmk6K//Jn03tvSyLHyfembMmePdjsWAMAN2QFZ1sBIvK0bpYJCmQJvbosJeBkFBgD0QbahXvbXT8gu+5uUXyBzy1dlpl0i4zu6tvOSJblMGwGAviQQlFqYQpJotmaTzCimjwA9QYEBAH2IPdIm++ffyv7hf6VIROaqf5G58tout3BbupQCAwD6FH9Q2rfb7RRpzdbvkxrqWP8C6CEKDADoA6y1UtUyRZ/9hVS3RzpvpnzX3iwzeJjb0QAAHmH8AVlGYCSUrdkoSYzAAHqIAgMA0pzduc1Z52LTGqm4RL55i2TGn9vlc5csydXSpbmdHxcXF0mS5s07xGgMAEh3gQBTSBKtZpOzXe3ws9xOAqQkCgwASFO26ZAaf/2Eon/8jRQIysz5vMyHPiKTkXHSY+6662hRUVxcpJ07dyUrLgDAbf6g1NIsa62MMW6nSUt260bp7DGn/LcYwMlRYABAmrGRiOw//ij7u/9WKNQsc+mVMh/9tEz/AW5HAwB4mT8gRSLSkTapX7bbadKObQlJO96VufJat6MAKYsCAwDSiN24WtFnHpN2bpPGTdKg2+7WgZy8Hp1r3jymjABAnxIIOrctIQqMRHh3ixSNsv4F0AsUGACQBuy+WmeBzqplUuFQ+b5wrzRlhjIHD5bq6np0Tta8AIA+xt9RYDRLA/LdzZKGbM0myRhp5Fi3owApiwIDAFKYbQnJ/uF/Zf/8WykjQ+aaG2Q+/DGZrH5uRwMApBjjD8hKUoiFPBPB1myUikbIBPu7HQVIWRQYAJCCbDQq+9bfZf/vCenAfpkZl8l84kaZgYN6dL4lS3IZcQEAfZ0/4NyyE0nc2WhUqtksM/Uit6MAKY0CAwBSiLVWemezor/6ufTOZums0fLdNl+mdFyvzrt0KQUGAPR5gWOmkCC+dr8vhZqkUta/AHqDAgMAUoCt3SH71iuyK151vgnKGyhz853OyAufz+14AIB00D4Cw4aaxSaq8WVrNkqSzKje/cIB6OsoMADAo+y+WtkVr8q+9Yq0411n4a/RZTKXXy0z41KZjsXWemjJklwtXZrb+XFxcZEkZ/cRRmMAQB/kP2YXEsTX1o1Sbp40+Ay3kwApjQIDADzE7q9zSosVrzrbrUlS6TiZT90qc/6FMvk9W+OiK3fddbSoKC4u0s6du+J2bgBACmIKScLYmk1S6XgZw9gWoDcoMADAZfZgg+zK12SXvypt3eA8OKJU5tqbZM6/SGbQEHcDAgD6hn7ZkvGxC0mc2cYGae9umQ/NdjsKkPIoMADABfZwo+yqN2SXvyJtXifZqFRcIvOxf5WZerHM0KKk5pk3jykjANDXGWOcdTBaKTDiqmazJMmwgCfQaxQYAJAktrlJtnqZM9JiY7UUiUhDimSu/heZ8y+WKR7hWjbWvAAASJICASnEFJJ4sls3SpmZUkmp21GAlEeBAQAJZFtCsqvfcta0WLdSCoelQUNkPvxxZy/4M0cyHxYA4B3ZAVnWwIgrW7NRKhklk9XP7ShAyqPAAIA4s22t0rqVzrana5dLbW1SfoHMpVfJTL1YOnsMpQUAwJsCQXYhiSN75Ii0bavM5f/sdhQgLVBgAEAc2PARaX217IpXZKvedOYP5+bJXFDhjLQYNUHG53M7piRn+1SmjAAAuuQPSqEmt1Okj21bpXBYZhTrXwDxQIEBAD1kw2Fpy1pnpEXVG1JzkxTsLzP1ImekxdiJMhkZbsc8wdKlFBgAgJPwB6SGOrdTpA1bs8m5UzrW3SBAmqDAAIBusocbpXc2y9Zscr4heXeL1NYq+QMyk2c4Iy0mTJbJzHI7KgAAPWICAVmmkMSN3bpRGnKGzICBbkcB0gIFBgB0wUajUu0Op6joKCxqdzif9PmcxTcvniUzdqJ0znmeX5hryZJcLV2a2/lxcbGzTeu8eYcYjQEAOMoflFjEMy6stVLNRplzznM7CpA2KDAAQM5uIXp3y9HRFe9scqaESFJOrlQ6TmbmZc4e7meNksn2uxs4RnfddbSoKC4u0s6du1xOBADwJH9AagnJWsuC0721b7d06KDE+hdA3FBgAOhzrLVS3Z5jRldslHZsk2zUeULRCJnyC6XS8TKl46ShRXwTBwDoGwJByVqptcUpM9Bjdquz/oUppcAA4oUCA0Das4cbpffeln3vbdn3tjprVzQecD6ZHZBGjpG5+l+csuLssTI5/d0NnGDz5jFlBABwEv6gc9vSTIHRWzUbpUCOdMaZbicB0gYFBoC0YltC0vYa2ffelt7b6tzuqz36hGHDZSZMkUaOdQqL4hJP7hSSSKx5AQA4qY7SIhSS8t2NkupszSapdKxntlEH0gEFBoCUZcNHpJ3bZN/d0j7CYqu06/2jU0EKBktnjZa5eLbMWaOkklEywRxXMwMA4GXGH5SVJHYi6RXbfFjatV3m/AvdjgKkFc/UgdXV1brzzjt1++2367e//a3bcU5q0aLu/aZ2yZLc0z8pxue6eU5ej7fP2ReubaMR2d3vK/r6y4r+938q8u9fU/T2Tyn6vXmy//WfsquXSwMLZa6+Tr7bvyXfkieUcf/PlfGF+fJd+UktffGibpUX6fb/RkUFPTUApIpoNKqvf/3rWrx4sSTp4Ycf1p133qm77rpLP/7xjxUOhxMfItA+AoOdSHrnnc2Stax/AcSZsdZat0NEo1Hdeeed+uY3v6lBgwbp3nvv1Z133qnhw4ef8rhdu5K/in53V++PZZX/VDinl19PYWGh6urqXLm2V87ppWsf+3709Jwjzxykmn8sk33/Hen9d2Tff1fa8Z6zoJjkrFtRUipz1mjdtni6fvqnQdKgIadcaDMV/iwTcU52HPGeU32NIPl4P7zHrfekqKgo6df8oOeff141NTUKhUKaP3++Vq1apSlTpkiSfvjDH2rChAmaNWtWt84V6/fJHX/udnuNoou+Kt8X7pU5b2bMryERvPx1erJs0d8+LfuH/5Xvh/8j48JaIqn4Z+YFXs3m1VxS4rKd7O9kT/xqbuvWrRo2bJiGDh0qSbrgggu0fPny0xYYAFKfbTokbX+nvax4V3b7O9o4a6ei/x5xnhAISmeeLXPRh6URI2XOGi0NK5bxOaOhXvhKkUwhP6ADAFJbfX29Vq1apU984hN6/vnnJUnnnXde5+dHjRql+vr6xAdpX8TTtoTE/ls9Z2s2ScPPdqW8ANKZJ0ZgLFu2TNXV1brtttskSf/4xz/09ttv65ZbbjnueZWVlaqsrJQkLV68WG1tbUnJt2hRhr73vROnjnzzmxF961uRmJ+XKudMldfz/e9n6bvfPfGf2FR9Pal+7e98xxw3xPXoc62K/bWaMGCzzsnbrGvO36SR/bYoum9P53N3h4Zow6ExWndwrNY3jtWGxjG6cd4wfWtB1LXXk4r/b1RUZOqVV06cIXjxxVFVViZh+DFOKTMzMznDwNEtvB/e49Z70q9fv6Rf81hLlizRNddco1AopOeee07z58/v/Fw4HNY3vvEN3XTTTRo/vuspCb39Prnjzz16sEH7brpauZ+dp+BV1/b8BcWRl79Ou8pmI2Htmztb/suv1oDPzvNMLq8gW+y8mktKXLaT/Z2cUgXGBzGFJHnn9PLrYQqJt65dWFiofTt3SLu2y+7cJu3c3jm6Qs2HnScZIw0tlhkx0hldMWKkdOZImdw8z72eVD8nU0i8x8vDQPsi3g/v6YtTSFauXKmqqirdeuutWr9+/QkFxn/+53/K7/frpptu6vY5ezyF5Eibol+8Vubjc+W7+rqYzpEoXv467Sqb3Vaj6Pe+KvPZr8k37UOeyeUVZIudV3NJfXQKSUFBwXFD4urr61VQUOBiIgDdYcNHNDZ3q6JvLu8sLOpqdyi655hvmrL6OVuVnn+h7v3xFC1+ukAafpZMtt+94AAAeMjmzZu1YsUKVVVVqa2tTaFQSA8//LDuuOMOPfvss2psbNTnPve55ITJzJIyMqVWdiHpKbt1oySxgCeQAJ4oMEpLS7V7927t3btXBQUFev3113XHHXe4HatL3/xm5PRPkjRv3qFun7O7z3XznLweb58z0de20YhUt8cZTbFzm7N16c5t0t5d+svFEdnHJPl80tBiZY4ar+iMy2SKS6TiEVLh0M71KobszZUpPf310/nPMtnnvPji6OmfBABw1Zw5czRnzhxJ6hyBcccdd+ill17S6tWrtWDBAvl8ydk80Bjj7EQSosDosZqNzs5ogwa7nQRIO56YQiJJq1at0hNPPKFoNKrLLrtMn/jEJ057jBtTSLw8fKev4j2JHxuNSPX7pNodsrved4qKXdul3dulY+fSDh4mFY1wSoqO26HFMllZvB8ew/vhPbwn3sL74T19cQrJsY6dQnL99ddr8ODB8vudUYvTp0/Xtdd2b12Knk4hkaTI/FtlRk+Q7xZ31m/4IC9/nXaVLXLPLTIjx8r3+a+7lCr1/sy8wqvZvJpL6qNTSCRnleVjV1oGkDi2tUXas1N29w6pdqdTWOx+X9qzSwofOfrEvAKpeITMh650bovPks4YzoraAAAkSFlZmcrKyiRJv/zlL90JEQjKtjACoyfs/jpp/z5p1sfdjgKkJc8UGADiy1orNR44Wk7UdhQWO5x/WDsYnzR4qDRsuEzZec4WpWcMdz7uP8C9FwAAANzhD0qhZrdTpCRbs0mSZErHuZwESE8UGECKs60t0t7d0t7dsnt3Sbt3yNa2j6wINR19YrbfKSXGlDm3w5ySQkPOkMnKcu8FAAAAbwkEnV+CIHY1G6V+2dLws91OAqQlCgwgBdiWkLSvVtq7S3bvbmnPLtl9u6U9u6WD+49/cv4gZ5rHjEuOLyoGDnIW5gIAADgF4w84328gZnbrRunsMTKZ/JgFJAJfWYBH2JaQM5Ji327ZPbuOjqjYW3tiSZE3UBp8hsw5U5zboUXSkCJpyDAZf9CdFwAAANKDPyC1MIUkVra1RXr/HZmPfNLtKEDaosAAksRGI1LDfqluj2zdHmdb0rpa5/6+Wulgw/EHUFIAAAA3BFgDo0fee1uKRmVGjXc7CZC2KDCAOLHWSocPHVNQ1B5fVtTvkyLhowcYIw0cJBUOdRbPHEJJAQAAPCA7ILW1ykYiMhkZbqdJGXbrRufOyLHuBgHSGAUG0E3WWqn5sFNE7N8nW7/3+IJi3x6p9QNbjvUf4BQUI0ql8y5w7hcOdXb9KBgsk8nimQAAwGMC7b9EaQ1Jwf7uZkkhtmaTdMaZMjm5bkcB0hYFBtDOhsPSgXqnnNi/72hR0Xm/7sSCol+2VDjUKSbGTpQKhzgFRcdjjKIAAACpxh9wbkMUGN1lo1GpZqNM+YVuRwHSGgUG+gRn9EST1LBPqq9zSon9x4yk2F8nHdgv2ejxB+bmSQWDpWHFMmVTnFETBYOdxwYNlnLz2NkDAACkl45fwLCQZ/fV7nC+1yxl/QsgkSgwkPKstbKHG6WGeml/nWxDnXO/oU72gPOYGuqkttbjD8zIlAoKnVJi3CSnkCgYLNN+q4GDZbKz3XlRAAAALjGBgKwktYRO91S061j/wpSOczkJkN4oMOBpNhqVDje2T+2ok20vJtRQ315U1GnvgXqpre34A30+Kb9AGlgoM/wsaeL5zoKZAwtlCgqlQUOkAfkyPp8rrwsAAMCzOkZgsBNJ99VsctY+G1rkdhIgrVFgwBXWWucfxYP7pQP7ZQ/sd0qKD9zXwYbjd+6QpIwMKX+QNHCQTMkoBWZeppA/KDOwsLOkUF6+jI9VswEAAGLWsQYGU0i6zW7dKJWOY2oxkGAUGIg729rqFBMHG44vIw7sd6Z0HNjvfL615cSDAznOyIn8AmdRzI77x5YTuXnHjZzILSxUa11dEl8hAABAGmvfhcS2hMSP46dnDx2U9u6SufjDbkcB0h4FBrrFWisdPiQ1NkgH98sePNBZUuhgg2z7rRobuh5umNXvaBlRUirlTZUGFkh5BTL5g47ez/Yn/8UBAADgKBbxjE3NJkmSYQFPIOEoMPo429oiNR7o/M82dhQTB2Q7CorGBunggROnckhSdkDKy5fyBjprTeSfJ+UNlAYMlMkb6IyayC+Qgv0ZUgcAAJAKjt1GFadlt250FocvKXU7CpD2KDDSkG0JOaXDsaVEF/fVeFBqPck/TLl5R4uIM86U8p37yitwion2/0zHP3AAAABICyYjQ+rXj11IusnWbJJKSmX6sXsdkGgUGCnARiJSU6NTOBw66JQQhzs+PuDMuzu2mPjgdqGSZIyUkysNyHd23zh7TOd9DciXGTDw6Me5eTKZ/K8BAADQZ/mDTCHpBnukTXrvbZnLrnI7CtAn8FNqktlo1CkY2lqlpsPSoQNOKXHo4NGC4tAB6VCjdMgpKNR0WLL2xJP5fM5Iif4DnBKidNwxpcRAmWMKCuXmOW06AAAAcDr+ANuodsORd7ZI4SMyo1j/AkgGCowY2Hc2q2VjlaL1dVJrewnR1uLsptHWKrW2ynY81v5x5+c6HmtrO/VF+udK/fOkAXlS0ZkyuROdkiI3T2ZAXvv9fCl3gLOuxDG7cQAAAABx4Q8605JxSkc2rXHusIAnkBQUGDGI/uk3Orjq9RM/0S9byvY7tx33s/3OqIjjHss+/rnB/s4oidwBTimRk8vUDQAAALgvwBSS7jiyaZ00eJizRhyAhOOn5Rj4rr1JA2/6khqaQ1K/jsKiH7trAAAAIL34A1L9PrdTeJq1Vkc2rZEZN8ntKECfQYERAzN4mDILC2Xq6tyOAgAAACSM8QdkT7ZbHRx1exQ9sF+G6SNA0rCAAgAAAIDjBYIs4nkadutGSZIZNc7lJEDfQYEBAAAA4HjZAdbAOJ2ajTLBHKlohNtJgD6DAgMAAADA8QJBKRyWPXLE7SSeZbduVNaYMhlfhttRgD6DAgMAAADA8fxB55atVLtkDx2Udm1X1vhz3Y4C9CkUGAAAAACO5w84t0wj6ZJdu1KyVtnlM92OAvQpFBgAAAAAjmMCHQUGIzC6tGa5lDdQmWePcTsJ0KdQYAAAAAA4XscUEnYiOYENh2U3VMlMPF/Gx49TQDLxFQcAAADgeEwhObmajVKoWWbi+W4nAfocCgwAAAAAxws4IzAsU0hOYNcslzIzpQks4AkkW6bbAQAAAAA4otGo5s+fr4KCAs2fP1979+7Vf/zHf+jQoUMaOXKkbr/9dmVmJuFb+M5dSBiB8UF2zQppzDkyHX9GAJKGERgAAACAR7z44osqLi7u/Pjpp5/W1VdfrUceeUQ5OTl6+eWXkxOkYwpJiBEYx7J7d0u1O2QmTXU7CtAnUWAAAAAAHlBfX69Vq1bpiiuukCRZa7V+/XrNmDFDknTppZdq+fLlyQmT7XdumUJyHLt2hSSx/gXgEqaQAAAAAB7w+OOPa+7cuQq1j3o4dOiQgsGgMjIyJEkFBQXav39/l8dWVlaqsrJSkrR48WIVFhbGdO3MzMwTjtkbCCpgrHJjPFe8dZXNLQ0bqxUpLlHhhImSvJXtWF7NJZGtJ7yaS0p+NgoMAAAAwGUrV65UXl6eRo4cqfXr18d8fEVFhSoqKjo/rquri+n4wsLCE46x2X6F9terNcZzxVtX2dxgW5oVXV8lc/k/debxSrYP8mouiWw94dVcUuKyFRUVdfk4BQYAAADgss2bN2vFihWqqqpSW1ubQqGQHn/8cTU3NysSiSgjI0P79+9XQUFB8kL5g0whOdaG1VI4zPoXgIsoMAAAAACXzZkzR3PmzJEkrV+/Xs8995zuuOMOLV26VMuWLdOFF16ov/3tbzr//CSuvRAIyrILSSe7doUUyJFKx7sdBeizWMQTAAAA8Kh//dd/1fPPP6/bb79dhw8f1uWXX568i/sDjMBoZ6NR2bUrZMqmyCRjG1sAXeKrDwAAAPCQsrIylZWVSZKGDh2q++67z50g/oDUeMCda3vN++9IBxskdh8BXMUIDAAAAAAnMP6gFGIKiSTZNSskY2QmlrsdBejTKDAAAAAAnCgQlFgDQ5Jk1yyXzh4jk5vndhSgT6PAAAAAAHCi9jUwrLVuJ3GVbWyQ3nub3UcAD6DAAAAAAHAif1CKRqW2NreTuMquXSlJMqx/AbiOAgMAAADAiQIB57aPTyOxa1ZI+YOkM892OwrQ51FgAAAAADiRv6PA6LtbqdrwEWlDlcyk82WMcTsO0OdRYAAAAAA4gfEHnTt9eQTG2xuklhDTRwCPyHQ7AAAAAJCK1q1b163n+Xw+TZgwIcFpEiDQXmD04a1U7ZrlUmaWNP5ct6MAEAUGAAAA0COLFi3S4MGDT7tLR2Njo5566qkkpYojppA461+MmyiT7Xc7CgBRYAAAAAA9kp2drR/96Eenfd7NN9+chDQJ0D6FxLY0qy+u/mBrd0p7d8lU/LPbUQC0Yw0MAAAAoAfuvvvubj3vrrvuSnCSBOnYhSTUN0dg2LUrJLF9KuAlFBgAAABAD0ycOLFbzzvnnHMSnCRBsvv2Ip527QqpaIRM4VC3owBoxxQSAAAAoJdeffVVnXXWWRo+fLh27dqln/70p/L5fLr11ltVXFzsdrye6ddP8vn65BoYNtQsbVknU/Ext6MAOAYjMAAAAIBeeuaZZ9S/f39J0pNPPqnS0lKNHz9ejz32mMvJes4Y46yD0Rd3IdlQLUUiMpOmup0EwDEoMAAAAIBeamxsVH5+vtra2rR582Z9+tOf1rXXXqv33nvP7Wi94w/0ySkkds1yKdhfKh3ndhQAx2AKCQAAANBLAwYMUG1trbZv367S0lJlZWWptbXV7Vi9FwjK9rEpJDYalV27Quac82QyMtyOA+AYFBgAAABAL33yk5/UPffcI5/Pp69+9auSpLVr16qkpMTlZL3kD/S9NTC2bZUOHZTYfQTwHAoMAAAAoIdaW1uVnZ2tSy+9VDNnzpQkZWdnS5JGjx6tr3zlK27G6z1/QGo67HaKpLJrVkjGJ3POeW5HAfABFBgAAABAD33xi1/U2WefrSlTpqi8vFzDhg3r/FxeXp6LyeLD+IOy9fvcjpFUds1yqXSsTP8BbkcB8AEUGAAAAEAP/fSnP9XGjRtVVVWl+++/X9FoVJMnT9aUKVN0zjnnKDMzxb/dDgT71CKe9kC9tL1G5hM3uh0FQBdS/G9UAAAAwD2ZmZmaOHGiJk6cqBtvvFF79uxRVVWV/vCHP+iRRx7RmDFjNGXKFE2bNk35+flux42dPyCF+s4aGHbtSkmSYf0LwJMoMAAAAIA4GTp0qD7ykY/oIx/5iNra2rR27VpVVVUpIyNDV1xxhdvxYucPSq0h2WhUxudzO03C2TUrpIJCqTjFF18F0hQFBgAAANBL0Wj0hMcyMzNVXl6u8vJyFxLFSSDg3La2ONNJ0pg9ckTaWC0z8zIZY9yOA6ALFBgAAABAL33605/u8vGMjAwNHDhQ06dP13XXXSe/35/kZL3kby8wQs1pX2BoyzqptYXpI4CHUWAAAAAAvXTzzTdr+fLl+vjHP65Bgwaprq5Ov//973XeeeepqKhIzz77rB5//HHddtttbkeNjb+9tGhN/3Uw7NoVUr9+0rhJbkcBcBIUGAAAAEAvvfDCC7r//vsVDDo/8BcVFam0tFTz58/XI488ohEjRuiee+5xOWXsTCAoKzkjMNKYtdbZPnXsJJl+2W7HAXAS6b8SDwAAAJBgzc3Nam1tPe6x1tZWNTc7P/jn5+erra3NjWi9k90+hSTdt1Kt3Sntq5WZNNXtJABOgREYAAAAQC9dcskl+t73vqcrr7xShYWFqq+v14v10tmTAAAgAElEQVQvvqhLLrlEkrR69WoVFRW5nLIHOta9aEnvKSR2zXJJbJ8KeB0FBgAAANBLc+fO1bBhw/T666+roaFB+fn5mj17tioqKiRJZWVl+s53vuNyyh5oX8TThkJK53057NoV0vCzZAYNdjsKgFOgwAAAAAB6yefzadasWZo1a1aXn+/Xr1+SE8VJxyKeaTyFxDYflt5eLzP7E25HAXAaFBgAAABAL1lr9dJLL+n1119XY2OjHnroIW3YsEEHDhzQBRdc4Ha8nuvYRjWNp5DY9dVSNMr6F0AKoMAAAAAAeumZZ57R2rVrddVVV+lnP/uZJGnQoEF64oknulVgtLW1aeHChQqHw4pEIpoxY4auu+46rV27Vk8//bSi0aj8fr++9KUvadiwYYl+OZ1MVpaUmZXeu5CsWS71z5VGjnE7CYDToMAAAAAAeunvf/+77r//fg0YMECPPfaYJGnIkCHau3dvt47PysrSwoUL5ff7FQ6HtWDBAk2ePFmPPfaY7r77bg0fPlx/+tOf9H//93/60pe+lMiXciJ/IG2nkNhoRHbdSplzymV8GW7HAXAabKMKAAAA9FLHCIljtbS0nPDYyRhjOp8biUQUiURkjLNsZijkTN9obm7WwIED45i6mwLB9J1C8u7b0uFGid1HgJTACAwAAACgl6ZMmaInn3xSn/nMZyQ5a2I888wzKi8v7/Y5otGo7rnnHtXW1mr27NkaPXq0brvtNt13333q16+fAoGAvv/973d5bGVlpSorKyVJixcvVmFhYUz5MzMzT3pMff9c+aIRDYzxnPFyqmy9dfhP69Xky1Dhhyrk6z8g5uMTma03vJpLIltPeDWXlPxsFBgAAABAL91444169NFHddNNNykcDuvGG2/UpEmT9OUvf7nb5/D5fHrwwQfV1NSkhx56SNu3b9cLL7yge++9V6NHj9bvf/97Pfnkk7rttttOOLaioqJzy1ZJqquriyl/YWHhSY+JZGZJBw/EfM54OVW23oq8+Q9p1Djtb2mTWmK/RiKz9YZXc0lk6wmv5pISl62oqKjLxykwAAAAgF4KBoO6++67deCA84N+YWGh8vPze3SunJwclZWVqbq6Wtu2bdPo0aMlSRdccMFJR2AklD8oHWxI/nUTzO6vk95/V+bam9yOAqCbWAMDAAAA6IFoNHrCfwMGDNDIkSM1YMCAzse6o7GxUU1NTZKcHUnWrFmj4uJiNTc3a9euXZLU+ViymUAwLRfxtGtXSJIM618AKYMRGAAAAEAPfPrTn+7W85555pnTPqehoUGPPvqootGorLWaOXOmysvL9fnPf15LliyRz+dTTk6OvvCFL/Q2duz8wbTcRtVWvykNHiadcabbUQB0EwUGAAAA0AM/+tGPOu+vWrVKy5Yt0zXXXNM5J/x3v/udpk+f3q1zlZSU6IEHHjjh8WnTpmnatGlxy9wj/oDUml67kNhQs7Rptczl/9S52wsA76PAAAAAAHpg8ODBnfeff/55LV68WDk5OZKcBehGjhype++9V7NmzXIrYnwEAlJbm2w4LJOZHj8+2HWrpHBYZvIMt6MAiAFrYAAAAAC91NzcrNbW1uMea2trU3NzGky98Aed23QahVG9TMrNk0rHup0EQAzSo0IFAAAAXHTJJZdo0aJFuvrqqzVo0CDV19frD3/4gy655BK3o/WeP+DctoSknFx3s8SBDR+RXbtSpvwCGV+G23EAxIACAwAAAOiluXPnatiwYXr99dfV0NCg/Px8zZ49WxUVFW5H6zUTCMpK6bOQ55Z1UqiJ6SNACqLAAAAAAHrJ5/Np1qxZqb/eRVc6ppCkyVaqtupNKdsvjZ/kdhQAMWINDAAAAKAHVq9e3a3nrVmzJsFJEuzYKSQpzkajzvapZVNk+mW7HQdAjCgwAAAAgB5YunRpt573gx/8IMFJEqx9BIYNpX6BoW010oF6po8AKYopJAAAAEAPtLS06Atf+MJpnxcOh5OQJoECHSMwUn8Kia1eJvl8MpPOdzsKgB6gwAAAAAB6YOHChd16njEmwUkSLJ2mkFQtk8acI5MGu6kAfREFBgAAANADEyZMcDtCcnQUGCm+C4mt3Sntfl/mkivdjgKgh1gDAwAAAMBJGV+Gs2tHik8hsavflCSZydNdTgKgp1wvMN544w3NmzdPn/rUp1RTU+N2HAAAAAAf5A+k/BQSW7VMGlEqM2iw21EA9JDrBcaZZ56pr33taxo/frzbUQAAAAB0xR9M6QLDNjZI72yWmcLoCyCVub4GxvDhw92OAAAAAOBU/AHZFF4Dw65eLlnL9qlAinO9wAAAAABSVSQS0YoVK7Rq1Spt27ZNTU1NysnJUUlJiaZMmaKpU6cqIyPD7Zi9l+JTSGzVMmnwMKm4xO0oAHohKQXGokWLdODAgRMev/766zV16tRun6eyslKVlZWSpMWLF6uwsDBuGbsrMzPTlevi5HhPvIX3w1t4P7yH98RbeD+8J5Xekz//+c/6zW9+o+HDh2v8+PEqLy+X3+9XS0uLduzYoZdeeklPPPGErrnmGs2aNcvtuL0TCEp1e9xO0SO2pVnauFrmsqtSf0tboI9LSoHxrW99Ky7nqaioUEVFRefHdXV1cTlvLAoLC125Lk6O98RbeD+8hffDe3hPvIX3w3vcek+KiopiPqa2tlb33Xef8vPzT/jctGnTJEkNDQ167rnnep3PbcYfTN0pJOurpPARdh8B0gBTSAAAAIAeuPHGG0/7nIEDB3breZ6XwlNIbNUyqf8AqZRNA4BU5/ouJG+99ZZuu+02bdmyRYsXL9b3v/99tyMBAAAAMbn55pu7fPzWW29NcpIECTgFhrXW7SQxseGw7JoVMudOlUmHtUiAPs71ERjTpk3rHGIHAAAApKJIJHLCY+FwWNFo1IU0CeAPSpGwFD4iZfVzO033bVknhZrYfQRIE64XGAAAAECqWrBggYwxOnLkiBYuXHjc5+rr6zVmzBiXksWZP+DchppTqsCw1cukftnShMluRwEQBxQYAAAAQA9dfvnlkqStW7fqsssu63zcGKO8vDydc845bkWLL3/QuW0JSQNOXLTUi6y1stVvSWVTZPplux0HQBxQYAAAAAA9dOmll0qSRo8ereLiYnfDJJAJBGUlqSWFdiLZXiM11Ml8fK7bSQDEieuLeAIAAACpaMWKFZ33T1VeHPu8lNU5hSR1diKxVcskn09m0vluRwEQJ4zAAAAAAHrgtdde0//8z//ooosu0oQJE1RUVKRAIKBQKKTdu3drw4YNeuWVV1RSUqLzz0/xH6KPnUKSImz1m9LoMpn+A9yOAiBOKDAAAACAHrjzzju1fft2/eUvf9GPfvQj7d27t/Nzw4YN05QpU/SVr3xFZ555posp4yTgjMCwLc0yLkfpDrt3l7Rzm8z1n3U7CoA4osAAAAAAemjEiBG65ZZbJEmtra1qampSTk6OsrPTbNHIY3chSQG2+k1Jkpk83eUkAOKJNTAAAACAXtq2bZuys7NVUFCQfuWFdHQKSWtqTCGxVW9KZ54tM2iI21EAxBEjMAAAAIBeWrx4sVpbWzVu3DhNmDBBEyZM0Nlnny1jUmHCRTdk+yVjUmIEhm08INVslPmn692OAiDOKDAAAACAXvrJT36iPXv2aOPGjdqwYYP+9Kc/6dChQxo3bpzmz5/vdrxeM8Y400hSYBFPu/otyVqZKTPcjgIgzigwAAAAgDgYOnSoIpGIwuGwwuGwqqurdfDgQbdjxY8/KLWkwAiM6jelQUOk4We5HQVAnFFgAAAAAL30gx/8QFu2bFFBQYEmTJigiy66SJ/97GcVaN+943Ta2tq0cOFChcNhRSIRzZgxQ9ddd52stfrlL3+pZcuWyefz6cMf/rCuuuqqBL+ak/AHZEPeHoFhW0LShmqZS69Mn+k7ADpRYAAAAAC99O6778rn86mkpEQlJSU666yzul1eSFJWVpYWLlwov9+vcDisBQsWaPLkydq5c6fq6+v1gx/8QD6fz90RHf6A90dgbKiSwkdkJjN9BEhHFBgAAABALz388MNqaGjoXAPjd7/7ndra2jR+/Hjddtttpz3eGCO/3y9JikQiikQiMsboz3/+s+688075fM7mgXl5eQl9HacUCHp+DQxb9abUP1caNd7tKAASgAIDAAAAiIOBAweqqKhI+/fv1/79+7V+/XpVVVV1+/hoNKp77rlHtbW1mj17tkaPHq09e/bo9ddf11tvvaUBAwbo5ptv1hlnnHHCsZWVlaqsrJTk7IhSWFgYU/bMzMzTHnMgL1/hQwdjPndvdSebJNlwWPvWrZB/2sXKGzo0Ccm6ny3ZvJpLIltPeDWXlPxsFBgAAABAL91///3atGmTAoGAJkyYoPLyct1www1dlg0n4/P59OCDD6qpqUkPPfSQtm/friNHjigrK0uLFy/Wm2++qZ/85Cf67ne/e8KxFRUVqqio6Py4rq4upvyFhYWnPSZqMmQPH4r53L3VnWySZDeulj18SG3jJyctY3ezJZtXc0lk6wmv5pISl62oqKjLxykwAAAAgF6aPn26br75Zg0ZMqTX58rJyVFZWZmqq6s1aNAgTZ8+XZI0bdo0/fjHP+71+XvM41NIbPWbUr9+0vgpbkcBkCA+twMAAAAAqe7SSy/tVXnR2NiopqYmSc6OJGvWrFFxcbGmTp2qdevWSZI2bNhw0t9KJkX7Ip7WWvcynIS1VrZ6mTRhikx2tttxACQIIzAAAAAAlzU0NOjRRx9VNBqVtVYzZ85UeXm5xo0bp4cfflgvvPCC/H6/Pv/5z7sXMhCUrJVaW5wyw0u2vyPtr5P56L+6nQRAAlFgAAAAAC4rKSnRAw88cMLjOTk5uvfee11I1IXs9tKiJeS5AsNWL5OMT2bSVLejAEggppAAAAAAOL1A0LltaXY3Rxds1TJp9ASZ3AFuRwGQQBQYAAAAAE7L+NsLjJC3FvK0+2qlndtkpkx3OwqABKPAAAAAAHB6HdNGPDYCw1a/KUky51JgAOmOAgMAAADA6QWOWQPDQ2z1Mmn42TKDh7kdBUCCUWAAAAAAOL32KSQ25J0RGPbQQentjUwfAfoICgwAAAAAp9cxhaTVOyMw7Jrlko3KTJ7hdhQASUCBAQAAAOD0OnYh8dIIjKpl0qAh0plnux0FQBJQYAAAAAA4vcwsKSPTM4t42qZD0vpVMlNmyBjjdhwASUCBAQAAAOC0jDHONBKPLOJp3/y7FA7LXHCF21EAJAkFBgAAAIDu8QekkEcKjNcqpZJRMkwfAfoMCgwAAAAA3RMIynpgCondXiNtf0fmwgq3owBIIgoMAAAAAN3jkSkk9rWXpMwsmWkfcjsKgCSiwAAAAADQPf6g67uQ2CNtssv+JnPeTJmc/q5mAZBcFBgAAAAAusUEgq6PwLDVb0nNh5k+AvRBFBgAAAAAuscDU0jsa3+RCgZL4ya5mgNA8lFgAAAAAOgef0BycRFPu3+ftKFa5sIrZHz8KAP0NXzVAwAAAOgef1BqbZGNRly5vH39ZclamQuucOX6ANxFgQEAAACge/wB57alJemXttGo7OsvSePPlSkcmvTrA3AfBQYAAACA7gkEnVs3ppG8vV7aV8vinUAfRoEBAAAAoHv87QVGKPkLedpXK6VAjsyUGUm/NgBvoMAAAAAA0C2mcwpJckdg2OYm2VWvyUz/kEy/7KReG4B3UGAAAAAA6J5AR4GR3BEYdvkrUlsb00eAPo4CAwAAAED3+N1ZA8O+VikVl0glo5J6XQDeQoEBAAAAoHvap5DYUPIKjPC2GundLTIXVcgYk7TrAvAeCgwAAAAA3dO5C0nyppCEXn5BysiUmX5p0q4JwJsoMAAAAAB0T3ZyF/G04SMK/e2P0rnTZHLzknJNAN5FgQEAAACgW0xmppTVL3nbqK5ZIdt4QL6LWLwTAAUGAAAAgFj4A0mbQhJ9rVK+gkJpwpSkXA+At1FgAAAAAOi+QDApU0jsgXpp7Ur5L7tKJiMj4dcD4H0UGAAAAAC6zx9Iyi4k9o2/STaqwOVXJ/xaAFJDptsBAAAAgL6ura1NCxcuVDgcViQS0YwZM3Tdddd1fv4Xv/iF/vrXv+qpp55yMWU7f1BqTewUEmut7GuV0ugJyiw6U6qrS+j1AKQGCgwAAADAZVlZWVq4cKH8fr/C4bAWLFigyZMna8yYMaqpqVFTU5PbEY8KBKX9+xJ7jZqN0p6dMldem9jrAEgpTCEBAAAAXGaMkd/vlyRFIhFFIhEZYxSNRvX0009r7ty5Lic8ymQnfhFP+2qllB2QKb8godcBkFoYgQEAAAB4QDQa1T333KPa2lrNnj1bo0eP1osvvqjy8nINHDjQ7XhHBRJbYNiWkOyKV2WmXizjDyTsOgBSDwUGAAAA4AE+n08PPvigmpqa9NBDD2nDhg1644039O1vf/u0x1ZWVqqyslKStHjxYhUWFsZ07czMzG4fc6hgkJpbQjFfo7tCLz2vxtYW5V99rfoVFsaULdm8ms2ruSSy9YRXc0nJz0aBAQAAAHhITk6OysrKtH79etXW1uqOO+6Q5Cz0efvtt+uRRx454ZiKigpVVFR0flwX46KXhYWF3T4mao10pE37anfLZGbFdJ3uiPzxt9KwYh0cNEymri6mbMnm1WxezSWRrSe8mktKXLaioqIuH6fAAAAAAFzW2NiojIwM5eTkqK2tTWvWrNHHPvYx/exnP+t8zg033NBleZF0HdM6WkJS//gWGLZ2p7R1g8wnPyNjTFzPDSD1UWAAAAAALmtoaNCjjz6qaDQqa61mzpyp8vJyt2N1zR90bkPNUv8BcT21fb1S8vlkZlwW1/MCSA8UGAAAAIDLSkpK9MADD5zyOU899VSS0pyaCQRkpbgv5GkjEdnX/ypNPF8mvyCu5waQHthGFQAAAED3HTuFJJ7Wr5IO7pfvoorTPxdAn0SBAQAAAKD7OqaQtDTH9bTR1yql3DzpnPPjel4A6YMCAwAAAED3BZwCw4biV2DYxgPS6rdkZl4uk8ksdwBdo8AAAAAA0H3Z8Z9CYpf9TYpEZC68Im7nBJB+KDAAAAAAdF8gvlNIrLWyr/5FGjlWpmhEXM4JID1RYAAAAADovmy/cxuK0wiM996Wdr8vcyGLdwI4NQoMAAAAAN1mfD5nGkmcppDYVyulfv1kpl4cl/MBSF8UGAAAAABiEwjEZQqJbW2VXf4PmfILZTqmpgDASVBgAAAAAIiNPyjFYRcSu+YtKdTM9BEA3UKBAQAAACA2/oBsaxymkKxfJQX7S6Mn9P5cANIeBQYAAACA2AR6PwLDWiu7vlpm/Lkyvow4BQOQzigwAAAAAMTGH4dFPHe/Lx2ol8qmxCcTgLRHgQEAAAAgJiYOBYbdUOWca8LkeEQC0AdQYAAAAACITRwW8bQbVktDi2UGDYlTKADpjgIDAAAAQGwCQamlWdbaHh1ujxyRNq9l9AWAmFBgAAAAAIiNPyBFo9KRtp4dX7NRamuVYf0LADGgwAAAAAAQG3/QuW3p2TQSu6FKysiQxp4Tx1AA0h0FBgAAAIDYBALObahnC3naDaulkWNlOooQAOgGCgwAAAAAMTH+9gKjBzuR2EMHpe01MhOYPgIgNhQYAAAAAGLTiykkduNqyVrWvwAQMwoMAAAAALEJtBcYPdlKdUOVFOwvlZTGNxOAtEeBAQAAACA22c4UEhvjFBJrrbP+xfhJMr6MRCQDkMYoMAAAAADEJtDDKSS1O6SGOta/ANAjFBgAAAAAYtOxBkaMu5DY9VWSJDNhcrwTAegDKDAAAAAAxKZfP8n4Yt6FxG6oloYUyRQOTVAwAOmMAgMAAABATIwxUiAQ0xQSe+SItHmtTBmjLwD0DAUGAAAAgNj5g7HtQvLOJqmtlfUvAPQYBQYAAACA2PkDsrGMwFhfJWVkSGMnJjAUgHRGgQEAAAAgdoFgTGtg2A3V0sixMh07mABAjCgwAAAAAMTOH+h2gWEPNUrba9h9BECvUGAAAAAAiJ0/0O01MOym1ZK1rH8BoFcy3Q4AAAAA9HVtbW1auHChwuGwIpGIZsyYoeuuu04PP/ywampqlJmZqdLSUn3uc59TZqY3voU3/qBsd6eQrK+SgjnSWaMSGwpAWvPG334AAABAH5aVlaWFCxfK7/crHA5rwYIFmjx5si666CLdfvvtkqQf/vCHevnllzVr1iyX07YLBLu1jaq11ln/Yvy5Mr6MJAQDkK5cLzCeeuoprVy5UpmZmRo6dKi++MUvKicnx+1YAAAAQNIYY+T3+yVJkUhEkUhExhidd955nc8ZNWqU6uvr3Yp4ovY1MGw0KuM7xcz02p1SQ53MhOuSlw1AWnK9wJg0aZLmzJmjjIwMPf300/rNb36juXPnuh0LAAAASKpoNKp77rlHtbW1mj17tkaPHt35uXA4rFdeeUU33XRTl8dWVlaqsrJSkrR48WIVFhbGdO3MzMyYj2kaNFiHJQ3KzZEvcPJfQDYve1mHJA268HJlxHiNnmZLFq9m82ouiWw94dVcUvKzuV5gnHvuuZ33x4wZo2XLlrmYBgAAAHCHz+fTgw8+qKamJj300EPavn27RowYIUl67LHHNH78eI0fP77LYysqKlRRUdH5cV1dXUzXLiwsjPmYaDQqSarfsUNm4KCTPi/y1qvSkCI1ZPSTYrxGT7Mli1ezeTWXRLae8GouKXHZioqKunzc9QLjWC+//LIuuOCCk36+t81yPHi5/eqreE+8hffDW3g/vIf3xFt4P7yH90TKyclRWVmZqqurNWLECD377LNqbGzU5z73ObejHc8fdG5bmiV1XWDY8BFpyzqZCy5PXi4AaSspBcaiRYt04MCBEx6//vrrNXXqVEnSr3/9a2VkZOjiiy8+6Xl62yzHg5fbr76K98RbeD+8hffDe3hPvIX3w3vcek9O9tu+ZGlsbFRGRoZycnLU1tamNWvW6GMf+5heeuklrV69WgsWLJDvVOtMuMD4A7KSdKqdSGo2S60tMhMmJysWgDSWlALjW9/61ik//7e//U0rV67UggULZIxJRiQAAADAMxoaGvToo48qGo3KWquZM2eqvLxc119/vQYPHqxvfOMbkqTp06fr2muvdTltu44RGKGT70RiN1RJPp80dlKSQgFIZ65PIamurtbvfvc7fec731F2drbbcQAAAICkKykp0QMPPHDC47/85S9dSNNNgWOnkHTNrq+SRo6T6XguAPSC6wXGz3/+c4XDYS1atEiSNHr0aO/N7wMAAABwPH9AkmRbQupqDLU93Chtr5H56KeTmwtA2nK9wHjkkUfcjgAAAAAgVp1TSLpeA8NuXCNZKzOe9S8AxIe3VgICAAAAkBraR2CcdArJhiopmCOdNTp5mQCkNQoMAAAAADEzWVlSZmaXu5BYa50FPMedK5OR4UI6AOmIAgMAAABAz/iDXY/AqN0p7a+TKWP6CID4ocAAAAAA0DOBYJfbqNoN/7+9e42Nqur3OP7bbYG2FFqmlCIgPhSLOdQHOVBCAZGrYMQXhhiiqIg3gkUJGg1wXoCJEpCLNA8UC4lB5I1IQiX4AhEJ4I0AhXIpWKQUJAdrhYHSCxVmZp0XhTlIBygjM2vTfj9v2rnszC9rt3uv/Z+11t4vSax/AeCuooABAAAAIDxtEmRCTSE5Uix1uk9OWmcLoQA0VxQwAAAAAIQnIaHRGhjGd0UqPSSn939bCgWguaKAAQAAACA88SGmkJSVSn/Vs/4FgLuOAgYAAACAsDjxIUZgHNkvxcRIvf5tKRWA5ooCBgAAAIDwJDS+C4k5UixlPCQnsa2lUACaKwoYAAAAAMJzw21UTc1F6dRx1r8AEBEUMAAAAACEJz5BunxZxu+XJJmjByVj5PRm/QsAdx8FDAAAAADhSUho+HltHYwj+6WEttK/Mu1lAtBsUcAAAAAAEJ74xIaf9XUyxjSsf/FffeTExtrNBaBZooABAAAAICxO/HUjMP74X8n7J+tfAIiYONsBAAAAANyjro3AuFQnc6pMklj/AkDEMAIDAAAAQHgSrptCcmS/lNZZTlpnu5kANFsUMAAAAACE5+oUElNTLZUelpPF9BEAkUMBAwAAAEB4rk0hObJf+usS618AiCgKGAAAAADCc/U2qqZ4txQTIz30b8uBADRnFDAAAAAAhKfN1buQXKqVevSSk9jWbh4AzRoFDAAAAABhcWJjpdZtGn5n+giACKOAAQAAACB8V+9EwgKeACItznYAAAAAAPewNglSwmXpX5m2kwBo5ihgAAAAAAhfl/vlJGU1TCcBgAiigAEAAAAgbDG5/yMZYzsGgBaAAgYAAACAsDmOIzmO7RgAWgAW8QQAAAAAAK5HAQMAAAAAALgeBQwAAAAAAOB6rIEBAAAAWHb58mXNnTtXPp9Pfr9fOTk5mjBhgiorK5WXl6fq6mplZGTorbfeUlwcXXgALRNHPwAAAMCyVq1aae7cuYqPj5fP59OcOXPUt29fff311xo3bpyGDBmiVatWadu2bRozZoztuABgBVNIAAAAAMscx1F8fLwkye/3y+/3y3EclZSUKCcnR5I0fPhw7dmzx2ZMALCKERgAAACACwQCAc2cOVMVFRUaO3as0tPTlZiYqNjYWEmSx+OR1+sNue3WrVu1detWSdKCBQvUsWPHO/rsuLi4O94mWsh259yaSyJbONyaS4p+NgoYAAAAgAvExMRo0aJFqq2t1eLFi3XmzJkmbzt69GiNHj06+Pjs2bN39NkdO3a8422ihWx3zq25JLKFw625pMhl69KlS8jnmUICAAAAuEjbtm2VlZWlY8eOqa6uTn6/X5Lk9Xrl8XgspwMAeyhgAAAAAJZdvHhRtbW1khruSHLw4EF17dpVWVlZ2rVrlyRp+/btys7OthkTAKxiCgkAAABg2fnz55Wfn69AICBjjAYNGqT+/furW7duysvL0xdffHxVOw8AAAuWSURBVKEePXpo5MiRtqMCgDUUMAAAAADLHnjgAS1cuLDR8+np6Zo/f76FRADgPo4xxtgOAQAAAAAAcCusgXGHZs2aZTsCbsA+cRf2h7uwP9yHfeIu7A/3YZ/Y4eZ2J9udc2suiWzhcGsuKfrZKGAAAAAAAADXo4ABAAAAAABcL/b9999/33aIe01GRobtCLgB+8Rd2B/uwv5wH/aJu7A/3Id9Yoeb251sd86tuSSyhcOtuaToZmMRTwAAAAAA4HpMIQEAAAAAAK5HAQMAAAAAALhenO0A95Li4mKtXr1agUBAo0aN0tNPP207Uou1YsUK7du3T8nJyVqyZIntOC3e2bNnlZ+frwsXLshxHI0ePVpPPvmk7Vgt2uXLlzV37lz5fD75/X7l5ORowoQJtmO1eIFAQLNmzZLH43H1LdFaimnTpik+Pl4xMTGKjY3VggULbEdq0Wpra1VQUKDTp0/LcRy98cYb6tWrl+1Yzc7t+rNXrlzR8uXLdeLECbVr104zZsxQp06dIp6rKX2JkpISLVy4MJhn4MCBeuaZZyKeTbr98cIYo9WrV2v//v1q06aNcnNzI74uwJkzZ7R06dLg48rKSk2YMEHjxo0LPhfNNgvVP6+pqdHSpUv1559/Ki0tTW+//baSkpIabbt9+3Zt2LBBkjR+/HgNHz484tnWrl2roqIixcXFKT09Xbm5uWrbtm2jbSN5rgiV68svv9R3332n9u3bS5Kee+459evXr9G2kb42DZVt6dKlOnPmjCSprq5OiYmJWrRoUaNtI3p+NWgSv99v3nzzTVNRUWGuXLli3n33XXP69GnbsVqskpISU1ZWZt555x3bUWCM8Xq9pqyszBhjTF1dnZk+fTr/H5YFAgFz6dIlY4wxV65cMbNnzzalpaWWU2HTpk0mLy/PzJ8/33YUGGNyc3NNVVWV7Ri4atmyZWbr1q3GmIbjVk1NjeVEzU9T+rObN282K1euNMYY88MPP5iPP/44Ktma0pc4fPiwtePn7Y4XRUVFZt68eSYQCJjS0lIze/bsKKZr2Levvfaaqays/Nvz0WyzUP3ztWvXmsLCQmOMMYWFhWbt2rWNtquurjbTpk0z1dXVf/s90tmKi4uNz+cL5gyVzZjInitC5Vq3bp3ZuHHjLbeLxrXp7a631qxZY9avXx/ytUi2GVNImuj48ePq3Lmz0tPTFRcXp8GDB2vPnj22Y7VYvXv3Dlm9hR0dOnQIfsuQkJCgrl27yuv1Wk7VsjmOo/j4eEmS3++X3++X4ziWU7Vs586d0759+zRq1CjbUQDXqaur09GjRzVy5EhJUlxcXMhvQvHPNKU/u3fv3uC33zk5OTp8+LBMFNb8v9f7Env37tVjjz0mx3HUq1cv1dbW6vz581H7/EOHDqlz585KS0uL2mfeKFT/fM+ePRo2bJgkadiwYSGvn4qLi9WnTx8lJSUpKSlJffr0UXFxccSzPfLII4qNjZUk9erVy8rfW7jXNNG4Nr1VNmOMfv75Zw0ZMuSufmZTMIWkibxer1JTU4OPU1NT9euvv1pMBLhTZWWlysvL9eCDD9qO0uIFAgHNnDlTFRUVGjt2rDIzM21HatE+++wzvfDCC7p06ZLtKLjOvHnzJEmPP/64Ro8ebTlNy1VZWan27dtrxYoVOnXqlDIyMjR58uRgIRZ3R1P6s9e/JzY2VomJiaqurg4OZ4+GW/Uljh07pvfee08dOnTQiy++qPvvvz9quW51vPB6verYsWPwcWpqqrxerzp06BCVbD/++ONNLyZttllVVVWwDVJSUlRVVdXoPTf+XXo8nqgXE7Zt26bBgwff9PVonyu++eYb7dy5UxkZGZo0aVKjQoLta9OjR48qOTlZ9913303fE6k2o4AB4K6pr6/XkiVLNHnyZCUmJtqO0+LFxMRo0aJFqq2t1eLFi/Xbb7+pe/futmO1SEVFRUpOTlZGRoZKSkpsx8FVH3zwgTwej6qqqvThhx+qS5cu6t27t+1YLZLf71d5ebleeeUVZWZmavXq1frqq6/07LPP2o6GKLtVX6JHjx5asWKF4uPjtW/fPi1atEj/+c9/opLLzccLn8+noqIiTZw4sdFrNtvsRo7juHI06IYNGxQbG6uhQ4eGfD3a+37MmDHBdUrWrVunzz//XLm5uRH7vHDcqmAmRbbNmELSRB6PR+fOnQs+PnfunDwej8VEgLv4fD4tWbJEQ4cO1cCBA23HwXXatm2rrKysuz4cE01XWlqqvXv3atq0acrLy9Phw4etdSDx/66dx5OTkzVgwAAdP37ccqKWKzU1VampqcGRYjk5OSovL7ecqvlpSn/2+vf4/X7V1dWpXbt2Ucl3u75EYmJicFROv3795Pf7dfHixahku93xwuPx6OzZs8HH0bxW2L9/v3r06KGUlJRGr9lsM6mhva5NpTl//nzIkTw3/l16vd6otd327dtVVFSk6dOn37S4Eu1zRUpKimJiYhQTE6NRo0aprKwsZCZb16Z+v1+7d+++5YiVSLYZBYwm6tmzp37//XdVVlbK5/Ppp59+UnZ2tu1YgCsYY1RQUKCuXbvqqaeesh0Hki5evKja2lpJDXckOXjwoLp27Wo5Vcs1ceJEFRQUKD8/XzNmzNDDDz+s6dOn247VotXX1wen89TX1+vgwYOMULIoJSVFqampwdXtDx06pG7dullO1fw0pT/bv39/bd++XZK0a9cuZWVlReVb86b0JS5cuBBcj+P48eMKBAJRKa405XiRnZ2tnTt3yhijY8eOKTEx0RXTR2y12TXZ2dnasWOHJGnHjh0aMGBAo/f07dtXBw4cUE1NjWpqanTgwAH17ds34tmKi4u1ceNGzZw5U23atAn5HhvniuvXTtm9e3fIKT82r00PHTqkLl26/G0Ky/Ui3WaOicaqPM3Evn37tGbNGgUCAY0YMULjx4+3HanFysvL05EjR1RdXa3k5GRNmDAhuPAXou+XX37RnDlz1L1792An52a3fEJ0nDp1Svn5+QoEAjLGaNCgQVG71RxuraSkRJs2beI2qpb98ccfWrx4saSGb5MeffRRzuuWnTx5UgUFBfL5fOrUqZNyc3NZsDsCQvVn161bp549eyo7O1uXL1/W8uXLVV5erqSkJM2YMUPp6ekRz3WzvsS1UQ1jxozR5s2btWXLFsXGxqp169aaNGmSHnrooYhnu9nxYsuWLcFsxhh9+umnOnDggFq3bq3c3Fz17Nkz4tnq6+uVm5ur5cuXB6fcXJ8rmm0Wqn8+YMAALV26VGfPnv3bbVTLysr07bffaurUqZIa1qAoLCyU1HAb1REjRkQ8W2FhoXw+X/A4k5mZqSlTpsjr9WrlypWaPXt2xM8VoXKVlJTo5MmTchxHaWlpmjJlijp06PC3XFLkr01vdr2Vn5+vzMxMjRkzJvjeaLYZBQwAAAAAAOB6TCEBAAAAAACuRwEDAAAAAAC4HgUMAAAAAADgehQwAAAAAACA61HAAAAAAAAArkcBAwAAAAAAuB4FDAAAAAAA4HoUMAAAAAAAgOtRwACAZqaiokIvv/yyTpw4IUnyer169dVXVVJSYjkZAAAAED4KGADQzHTu3FnPP/+8li1bpr/++kuffPKJhg0bpqysLNvRAAAAgLA5xhhjOwQA4O776KOPVFlZKcdxNH/+fLVq1cp2JAAAACBsjMAAgGZq1KhROn36tJ544gmKFwAAALjnUcAAgGaovr5ea9as0ciRI7V+/XrV1NTYjgQAAAD8IxQwAKAZWr16tTIyMjR16lT169dPq1atsh0JAAAA+EcoYABAM7Nnzx4VFxfr9ddflyS99NJLKi8v1/fff285GQAAABA+FvEEAAAAAACuxwgMAAAAAADgehQwAAAAAACA61HAAAAAAAAArkcBAwAAAAAAuB4FDAAAAAAA4HoUMAAAAAAAgOtRwAAAAAAAAK5HAQMAAAAAALje/wFcNImG5lJ0qwAAAABJRU5ErkJggg==\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "#plot trajectory\n", "grid = plt.GridSpec(2, 3)\n", "\n", "plt.figure(figsize=(15,10))\n", "\n", "plt.subplot(grid[0:2, 0:2])\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_sim[0,:],x_sim[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(grid[0, 2])\n", "plt.plot(u_sim[0,:])\n", "plt.ylabel('v(t) [m/s]')\n", "\n", "plt.subplot(grid[1, 2])\n", "plt.plot(np.degrees(u_sim[1,:]))\n", "plt.ylabel('w(t) [deg/s]')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "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.6.9" } }, "nbformat": 4, "nbformat_minor": 4 }