{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "source: --> https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/common/motion_model" ] }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "%matplotlib inline\n", "from IPython.display import Markdown, display\n", "import numpy as np\n", "import matplotlib.dates as mdates\n", "import matplotlib.pyplot as plt\n", "import time\n", "from scipy.stats import norm\n", "from sympy import (\n", " Symbol,\n", " symbols,\n", " Matrix,\n", " sin,\n", " cos,\n", " integrate,\n", " diff,\n", " Q,\n", " refine,\n", " simplify,\n", " factor,\n", " expand_trig,\n", " exp,\n", " latex,\n", " Integral,\n", ")\n", "from sympy import init_printing\n", "from sympy.utilities.codegen import codegen\n", "\n", "init_printing(use_latex=True)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Motion models\n", "In tracking we can use different motion models. Here, we will derive a couple of those:" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## CATR - constant acceleration and turn rate model\n", "This model assumes the state $[x, y, \\theta, v, \\omega, a]^\\top$, where $x$ and $y$ are coordinates of the object, $\\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\\theta$, $\\omega$ is the angular velocity (aka turn rate), and $a$ is the acceleration along the vector spawned by $\\theta$. Furthermore, we assume that the object has constant acceleration $a$ and constant turn rate $\\omega$. This is a useful model to model differential-drive systems.\n", "\n", "### Derivation:" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CART state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\theta\\\\v\\\\\\omega\\\\a\\end{matrix}\\right]$" ], "text/plain": [ "⎡ x ⎤\n", "⎢ ⎥\n", "⎢ y ⎥\n", "⎢ ⎥\n", "⎢\\theta⎥\n", "⎢ ⎥\n", "⎢ v ⎥\n", "⎢ ⎥\n", "⎢\\omega⎥\n", "⎢ ⎥\n", "⎣ a ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Assuming a non-zero turn rate, the next state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\frac{\\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + x - \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\- \\frac{\\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + y + \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\\\Delta{t} \\hat{\\omega} + \\theta\\\\\\Delta{t} a + v\\\\\\hat{\\omega}\\\\a\\end{matrix}\\right]$" ], "text/plain": [ "⎡ \\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅sin(\\theta) v⋅sin\n", "⎢ ──────────────────────────────────────────────── + x - ───────────── + ─────\n", "⎢ \\hat{\\omega} \\hat{\\omega} \n", "⎢ \n", "⎢ \n", "⎢ \\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅cos(\\theta) v⋅co\n", "⎢- ──────────────────────────────────────────────── + y + ───────────── - ────\n", "⎢ \\hat{\\omega} \\hat{\\omega} \n", "⎢ \n", "⎢ \n", "⎢ \\Delta{t}\n", "⎢ \n", "⎢ \\\n", "⎢ \n", "⎢ \n", "⎢ \n", "⎣ \n", "\n", "(\\Delta{t}⋅\\hat{\\omega} + \\theta) a⋅cos(\\theta) a⋅cos(\\Delta{t}⋅\\hat{\\omeg\n", "───────────────────────────────── - ───────────── + ──────────────────────────\n", " \\hat{\\omega} 2 2 \n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", "s(\\Delta{t}⋅\\hat{\\omega} + \\theta) a⋅sin(\\theta) a⋅sin(\\Delta{t}⋅\\hat{\\ome\n", "────────────────────────────────── - ───────────── + ─────────────────────────\n", " \\hat{\\omega} 2 2\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", "⋅\\hat{\\omega} + \\theta \n", " \n", "Delta{t}⋅a + v \n", " \n", " \\hat{\\omega} \n", " \n", " a \n", "\n", "a} + \\theta) ⎤\n", "──────────── ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", "ga} + \\theta)⎥\n", "─────────────⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/plain": [ "'Plugging in the numbers:'" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\hat{\\omega} : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ a : 2, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n", "y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}42.1728437300543\\\\23.1186522301942\\\\0.7\\\\2.2\\\\2.0\\\\2.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡42.1728437300543⎤\n", "⎢ ⎥\n", "⎢23.1186522301942⎥\n", "⎢ ⎥\n", "⎢ 0.7 ⎥\n", "⎢ ⎥\n", "⎢ 2.2 ⎥\n", "⎢ ⎥\n", "⎢ 2.0 ⎥\n", "⎢ ⎥\n", "⎣ 2.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Jacobian of the next state with respect to the previous state:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1 & 0 & \\frac{\\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} & - \\frac{\\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{\\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} & \\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{\\Delta{t} v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{2 \\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{2 a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{3}} - \\frac{2 a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{3}} & \\frac{\\Delta{t} \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{\\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{\\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\0 & 1 & \\frac{\\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} & \\frac{\\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} - \\frac{\\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} & \\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{\\Delta{t} v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{2 \\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{2 a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{3}} - \\frac{2 a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{3}} & - \\frac{\\Delta{t} \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{\\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{\\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\0 & 0 & 1 & 0 & \\Delta{t} & 0\\\\0 & 0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡ \n", "⎢ \\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅cos(\\theta) v⋅co\n", "⎢1 0 ──────────────────────────────────────────────── - ───────────── + ────\n", "⎢ \\hat{\\omega} \\hat{\\omega} \n", "⎢ \n", "⎢ \n", "⎢ \n", "⎢ \\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅sin(\\theta) v⋅si\n", "⎢0 1 ──────────────────────────────────────────────── - ───────────── + ────\n", "⎢ \\hat{\\omega} \\hat{\\omega} \n", "⎢ \n", "⎢ \n", "⎢0 0 \n", "⎢ \n", "⎢0 0 \n", "⎢ \n", "⎢0 0 \n", "⎢ \n", "⎣0 0 \n", "\n", " \n", "s(\\Delta{t}⋅\\hat{\\omega} + \\theta) a⋅sin(\\theta) a⋅sin(\\Delta{t}⋅\\hat{\\ome\n", "────────────────────────────────── + ───────────── - ─────────────────────────\n", " \\hat{\\omega} 2 2\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", "n(\\Delta{t}⋅\\hat{\\omega} + \\theta) a⋅cos(\\theta) a⋅cos(\\Delta{t}⋅\\hat{\\ome\n", "────────────────────────────────── - ───────────── + ─────────────────────────\n", " \\hat{\\omega} 2 2\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " 1 \n", " \n", " 0 \n", " \n", " 0 \n", " \n", " 0 \n", "\n", " \n", "ga} + \\theta) sin(\\theta) sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t\n", "───────────── - ──────────── + ──────────────────────────────────── ────────\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", " \n", "ga} + \\theta) cos(\\theta) cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t\n", "───────────── ──────────── - ──────────────────────────────────── ────────\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", " 0 \n", " \n", " 1 \n", " \n", " 0 \n", " \n", " 0 \n", "\n", " 2 \n", "} ⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\hat{\\om\n", "───────────────────────────────────────── + ──────────────────────────────────\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", " 2 \n", "} ⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\hat{\\om\n", "───────────────────────────────────────── + ──────────────────────────────────\n", " \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", "\n", " \n", "ega} + \\theta) 2⋅\\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅sin(\\t\n", "────────────── - ────────────────────────────────────────────────── + ────────\n", " 2 \n", " \\hat{\\omega} \\hat{\\om\n", " \n", " \n", "ega} + \\theta) 2⋅\\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) v⋅cos(\\t\n", "────────────── + ────────────────────────────────────────────────── - ────────\n", " 2 \n", " \\hat{\\omega} \\hat{\\om\n", " \n", " \\Delta{t} \n", " \n", " 0 \n", " \n", " 1 \n", " \n", " 0 \n", "\n", " \n", "heta) v⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta) 2⋅a⋅cos(\\theta) 2⋅a⋅cos(\\De\n", "───── - ────────────────────────────────────── + ─────────────── - ───────────\n", " 2 2 3 \n", "ega} \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", "heta) v⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) 2⋅a⋅sin(\\theta) 2⋅a⋅sin(\\De\n", "───── + ────────────────────────────────────── + ─────────────── - ───────────\n", " 2 2 3 \n", "ega} \\hat{\\omega} \\hat{\\omega} \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", "\n", " \n", "lta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t}⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)\n", "───────────────────────────── ──────────────────────────────────────────────\n", " 3 \\hat{\\omega} \n", " \\hat{\\omega} \n", " \n", " \n", "lta{t}⋅\\hat{\\omega} + \\theta) \\Delta{t}⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta\n", "───────────────────────────── - ─────────────────────────────────────────────\n", " 3 \\hat{\\omega} \n", " \\hat{\\omega} \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", "\n", " ⎤\n", " cos(\\theta) cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) ⎥\n", " - ───────────── + ──────────────────────────────────── ⎥\n", " 2 2 ⎥\n", " \\hat{\\omega} \\hat{\\omega} ⎥\n", " ⎥\n", " ⎥\n", ") sin(\\theta) sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)⎥\n", "─ - ───────────── + ────────────────────────────────────⎥\n", " 2 2 ⎥\n", " \\hat{\\omega} \\hat{\\omega} ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", "\\Delta{t} ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/plain": [ "'Plugging in the numbers:'" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\hat{\\omega} : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ a : 2, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n", "y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.118652230194179 & 0.082396074316744 & -0.00631501513627726 & 0.00402579071041348\\\\0 & 1.0 & 0.172843730054315 & 0.0563701873029421 & 0.00858190270490869 & 0.00295592779414759\\\\0 & 0 & 1.0 & 0 & 0.1 & 0\\\\0 & 0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡1.0 0 -0.118652230194179 0.082396074316744 -0.00631501513627726 0.004\n", "⎢ \n", "⎢ 0 1.0 0.172843730054315 0.0563701873029421 0.00858190270490869 0.002\n", "⎢ \n", "⎢ 0 0 1.0 0 0.1 \n", "⎢ \n", "⎢ 0 0 0 1.0 0 \n", "⎢ \n", "⎢ 0 0 0 0 1.0 \n", "⎢ \n", "⎣ 0 0 0 0 0 \n", "\n", "02579071041348⎤\n", " ⎥\n", "95592779414759⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 0.1 ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 1.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Assuming a zero turn rate, state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + x\\\\\\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + y\\\\\\theta\\\\\\Delta{t} a + v\\\\\\omega_{0}\\\\a\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 \n", "⎢\\Delta{t} ⋅a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) \n", "⎢──────────────────────────────────────────── + \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\ome\n", "⎢ 2 \n", "⎢ \n", "⎢ 2 \n", "⎢\\Delta{t} ⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) \n", "⎢──────────────────────────────────────────── + \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\ome\n", "⎢ 2 \n", "⎢ \n", "⎢ \\theta \n", "⎢ \n", "⎢ \\Delta{t}⋅a + v \n", "⎢ \n", "⎢ \\omega₀ \n", "⎢ \n", "⎣ a \n", "\n", " ⎤\n", " ⎥\n", "ga₀ + \\theta) + x⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", "ga₀ + \\theta) + y⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/plain": [ "'Plugging in the numbers:'" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\hat{\\omega} : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ a : 2, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n", "y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}42.184292337997\\\\23.1006793631069\\\\0.5\\\\2.2\\\\0\\\\2.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡42.184292337997 ⎤\n", "⎢ ⎥\n", "⎢23.1006793631069⎥\n", "⎢ ⎥\n", "⎢ 0.5 ⎥\n", "⎢ ⎥\n", "⎢ 2.2 ⎥\n", "⎢ ⎥\n", "⎢ 0 ⎥\n", "⎢ ⎥\n", "⎣ 2.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Jacobian of the next state with respect to the previous state with 0 turn rate:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} - \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & - \\frac{\\Delta{t}^{3} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} - \\Delta{t}^{2} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{2} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2}\\\\0 & 1 & \\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{3} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t}^{2} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{2} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2}\\\\0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 \n", "⎢ \\Delta{t} ⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) \n", "⎢1 0 - ──────────────────────────────────────────── - \\Delta{t}⋅v⋅sin(\\Delta\n", "⎢ 2 \n", "⎢ \n", "⎢ 2 \n", "⎢ \\Delta{t} ⋅a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) \n", "⎢0 1 ──────────────────────────────────────────── + \\Delta{t}⋅v⋅cos(\\Delta{\n", "⎢ 2 \n", "⎢ \n", "⎢0 0 1 \n", "⎢ \n", "⎢0 0 0 \n", "⎢ \n", "⎢0 0 0 \n", "⎢ \n", "⎣0 0 0 \n", "\n", " 3\n", " \\Delta{t} \n", "{t}⋅\\omega₀ + \\theta) \\Delta{t}⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) - ──────────\n", " \n", " \n", " 3 \n", " \\Delta{t} ⋅\n", "t}⋅\\omega₀ + \\theta) \\Delta{t}⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) ───────────\n", " \n", " \n", " 0 \n", " \n", " 1 \n", " \n", " 0 \n", " \n", " 0 \n", "\n", " \n", "⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) 2 \n", "────────────────────────────────── - \\Delta{t} ⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\the\n", " 2 \n", " \n", " \n", "a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) 2 \n", "───────────────────────────────── + \\Delta{t} ⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\thet\n", " 2 \n", " \n", " 0 \n", " \n", " 0 \n", " \n", " 1 \n", " \n", " 0 \n", "\n", " 2 ⎤\n", " \\Delta{t} ⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n", "ta) ──────────────────────────────────────────⎥\n", " 2 ⎥\n", " ⎥\n", " 2 ⎥\n", " \\Delta{t} ⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n", "a) ──────────────────────────────────────────⎥\n", " 2 ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " \\Delta{t} ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/plain": [ "'Plugging in the numbers:'" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\hat{\\omega} : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ a : 2, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n", "y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.100679363106883 & 0.0877582561890373 & -0.0100679363106883 & 0.00438791280945186\\\\0 & 1.0 & 0.184292337996978 & 0.0479425538604203 & 0.0184292337996978 & 0.00239712769302102\\\\0 & 0 & 1.0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡1.0 0 -0.100679363106883 0.0877582561890373 -0.0100679363106883 0.0043\n", "⎢ \n", "⎢ 0 1.0 0.184292337996978 0.0479425538604203 0.0184292337996978 0.0023\n", "⎢ \n", "⎢ 0 0 1.0 0 0 \n", "⎢ \n", "⎢ 0 0 0 1.0 0 \n", "⎢ \n", "⎢ 0 0 0 0 1.0 \n", "⎢ \n", "⎣ 0 0 0 0 0 \n", "\n", "8791280945186⎤\n", " ⎥\n", "9712769302102⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 0.1 ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 1.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols(\n", " \"v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}\"\n", ")\n", "turn_rate_non_zero = Symbol(\"\\hat{\\omega}\", nonzero=True, finite=True)\n", "zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n", "turn_rate = Symbol(\"\\omega\")\n", "\n", "state_catr = Matrix([x, y, yaw, speed, turn_rate, acceleration])\n", "print(\"CART state is:\")\n", "display(state_catr)\n", "\n", "\n", "def get_next_state(turn_rate):\n", " # Specify the functions for yaw, speed, x, and y.\n", " yaw_func = yaw + turn_rate * t\n", " speed_func = speed + acceleration * t\n", " x_speed_func = speed_func * cos(yaw_func)\n", " y_speed_func = speed_func * sin(yaw_func)\n", "\n", " # Get next state by integrating the functions.\n", " next_speed = speed + integrate(acceleration, (t, 0, dt))\n", " next_yaw = yaw + integrate(turn_rate, (t, 0, dt))\n", " next_x = x + integrate(x_speed_func, (t, 0, dt))\n", " next_y = y + integrate(y_speed_func, (t, 0, dt))\n", "\n", " return Matrix([next_x, next_y, next_yaw, next_speed, turn_rate, acceleration])\n", "\n", "\n", "# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n", "print(\"Assuming a non-zero turn rate, the next state is:\")\n", "next_state = get_next_state(turn_rate_non_zero)\n", "display(next_state)\n", "\n", "substitutes = {\n", " x: 42,\n", " y: 23,\n", " yaw: 0.5,\n", " speed: 2,\n", " acceleration: 2,\n", " dt: 0.1,\n", " turn_rate_non_zero: 2,\n", " zero_turn_rate: 0,\n", "}\n", "display(\"Plugging in the numbers:\", substitutes)\n", "display(next_state.evalf(subs=substitutes))\n", "\n", "state = Matrix([x, y, yaw, speed, turn_rate_non_zero, acceleration])\n", "print(\"Jacobian of the next state with respect to the previous state:\")\n", "J = next_state.jacobian(state)\n", "display(J)\n", "display(\"Plugging in the numbers:\", substitutes)\n", "display(J.evalf(subs=substitutes))\n", "\n", "print(\"Assuming a zero turn rate, state is:\")\n", "next_state = get_next_state(zero_turn_rate)\n", "display(next_state)\n", "display(\"Plugging in the numbers:\", substitutes)\n", "display(next_state.evalf(subs=substitutes))\n", "\n", "state = Matrix([x, y, yaw, speed, zero_turn_rate, acceleration])\n", "print(\"Jacobian of the next state with respect to the previous state with 0 turn rate:\")\n", "J = next_state.jacobian(state)\n", "display(J)\n", "\n", "display(\"Plugging in the numbers:\", substitutes)\n", "display(J.evalf(subs=substitutes))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## CVTR - constant velocity and turn rate model\n", "This model assumes the state $[x, y, \\theta, v, \\omega]^\\top$, where $x$ and $y$ are coordinates of the object, $\\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\\theta$, $\\omega$ is the angular velocity (aka turn rate). Furthermore, we assume that the object has constant speed $v$ and constant turn rate $\\omega$. This is also a useful model to model differential-drive systems. It is a bit simpler than the CATR one.\n", "\n", "### Derivation:" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CVRT state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\theta\\\\v\\\\\\omega\\end{matrix}\\right]$" ], "text/plain": [ "⎡ x ⎤\n", "⎢ ⎥\n", "⎢ y ⎥\n", "⎢ ⎥\n", "⎢\\theta⎥\n", "⎢ ⎥\n", "⎢ v ⎥\n", "⎢ ⎥\n", "⎣\\omega⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Assuming a non-zero turn rate, next state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}x - \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega}\\\\y + \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega} - \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega}\\\\\\Delta{t} \\omega + \\theta\\\\v\\\\\\omega\\end{matrix}\\right]$" ], "text/plain": [ "⎡ v⋅sin(\\theta) v⋅sin(\\Delta{t}⋅\\omega + \\theta)⎤\n", "⎢x - ───────────── + ────────────────────────────────⎥\n", "⎢ \\omega \\omega ⎥\n", "⎢ ⎥\n", "⎢ v⋅cos(\\theta) v⋅cos(\\Delta{t}⋅\\omega + \\theta)⎥\n", "⎢y + ───────────── - ────────────────────────────────⎥\n", "⎢ \\omega \\omega ⎥\n", "⎢ ⎥\n", "⎢ \\Delta{t}⋅\\omega + \\theta ⎥\n", "⎢ ⎥\n", "⎢ v ⎥\n", "⎢ ⎥\n", "⎣ \\omega ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/markdown": [ "Plugging in the numbers:" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\omega : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}42.1647921486335\\\\23.1127403746059\\\\0.7\\\\2.0\\\\2.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡42.1647921486335⎤\n", "⎢ ⎥\n", "⎢23.1127403746059⎥\n", "⎢ ⎥\n", "⎢ 0.7 ⎥\n", "⎢ ⎥\n", "⎢ 2.0 ⎥\n", "⎢ ⎥\n", "⎣ 2.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Jacobian of the next state with respect to the previous state:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & - \\frac{\\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{\\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\Delta{t} v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega^{2}} - \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega^{2}}\\\\0 & 1 & - \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\cos{\\left(\\theta \\right)}}{\\omega} - \\frac{\\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\Delta{t} v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega^{2}} + \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega^{2}}\\\\0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡ v⋅cos(\\theta) v⋅cos(\\Delta{t}⋅\\omega + \\theta) sin(\\theta) sin\n", "⎢1 0 - ───────────── + ──────────────────────────────── - ─────────── + ───\n", "⎢ \\omega \\omega \\omega \n", "⎢ \n", "⎢ \n", "⎢ v⋅sin(\\theta) v⋅sin(\\Delta{t}⋅\\omega + \\theta) cos(\\theta) cos(\n", "⎢0 1 - ───────────── + ──────────────────────────────── ─────────── - ────\n", "⎢ \\omega \\omega \\omega \n", "⎢ \n", "⎢ \n", "⎢0 0 1 \n", "⎢ \n", "⎢0 0 0 \n", "⎢ \n", "⎣0 0 0 \n", "\n", "(\\Delta{t}⋅\\omega + \\theta) \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega + \\theta) v⋅si\n", "─────────────────────────── ────────────────────────────────────────── + ────\n", " \\omega \\omega \n", " \\\n", " \n", "\\Delta{t}⋅\\omega + \\theta) \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega + \\theta) v⋅co\n", "────────────────────────── ────────────────────────────────────────── - ────\n", " \\omega \\omega \n", " \\\n", " \n", " 0 \\Delta{\n", " \n", " 1 0 \n", " \n", " 0 1 \n", "\n", "n(\\theta) v⋅sin(\\Delta{t}⋅\\omega + \\theta)⎤\n", "───────── - ────────────────────────────────⎥\n", " 2 2 ⎥\n", "omega \\omega ⎥\n", " ⎥\n", "s(\\theta) v⋅cos(\\Delta{t}⋅\\omega + \\theta)⎥\n", "───────── + ────────────────────────────────⎥\n", " 2 2 ⎥\n", "omega \\omega ⎥\n", " ⎥\n", "t} ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", " ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/markdown": [ "Plugging in the numbers:" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\omega : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.112740374605884 & 0.082396074316744 & -0.00591185558829518\\\\0 & 1.0 & 0.164792148633488 & 0.0563701873029421 & 0.00805158142082696\\\\0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡1.0 0 -0.112740374605884 0.082396074316744 -0.00591185558829518⎤\n", "⎢ ⎥\n", "⎢ 0 1.0 0.164792148633488 0.0563701873029421 0.00805158142082696 ⎥\n", "⎢ ⎥\n", "⎢ 0 0 1.0 0 0.1 ⎥\n", "⎢ ⎥\n", "⎢ 0 0 0 1.0 0 ⎥\n", "⎢ ⎥\n", "⎣ 0 0 0 0 1.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Assuming a zero turn rate, next state is:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + x\\\\\\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + y\\\\\\theta\\\\v\\\\\\omega_{0}\\end{matrix}\\right]$" ], "text/plain": [ "⎡\\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) + x⎤\n", "⎢ ⎥\n", "⎢\\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) + y⎥\n", "⎢ ⎥\n", "⎢ \\theta ⎥\n", "⎢ ⎥\n", "⎢ v ⎥\n", "⎢ ⎥\n", "⎣ \\omega₀ ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/markdown": [ "Plugging in the numbers:" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\omega : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}42.1755165123781\\\\23.0958851077208\\\\0.5\\\\2.0\\\\0\\end{matrix}\\right]$" ], "text/plain": [ "⎡42.1755165123781⎤\n", "⎢ ⎥\n", "⎢23.0958851077208⎥\n", "⎢ ⎥\n", "⎢ 0.5 ⎥\n", "⎢ ⎥\n", "⎢ 2.0 ⎥\n", "⎢ ⎥\n", "⎣ 0 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stdout", "output_type": "stream", "text": [ "Jacobian of the next state with respect to the previous one with zero turn rate:\n" ] }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & - \\Delta{t}^{2} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}\\\\0 & 1 & \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t}^{2} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}\\\\0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$" ], "text/plain": [ "⎡ \n", "⎢1 0 -\\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) \\Delta{t}⋅cos(\\Delta{t}⋅\\\n", "⎢ \n", "⎢ \n", "⎢0 1 \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) \\Delta{t}⋅sin(\\Delta{t}⋅\\\n", "⎢ \n", "⎢0 0 1 0 \n", "⎢ \n", "⎢0 0 0 1 \n", "⎢ \n", "⎣0 0 0 0 \n", "\n", " 2 ⎤\n", "omega₀ + \\theta) -\\Delta{t} ⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n", " ⎥\n", " 2 ⎥\n", "omega₀ + \\theta) \\Delta{t} ⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 0 ⎥\n", " ⎥\n", " 1 ⎦" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/markdown": [ "Plugging in the numbers:" ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "image/png": "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\n", "text/latex": [ "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\ \\omega : 2, \\ \\omega_{0} : 0, \\ \\theta : 0.5, \\ v : 2, \\ x : 42, \\ y : 23\\right\\}$" ], "text/plain": [ "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}" ] }, "metadata": {}, "output_type": "display_data" }, { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.0958851077208406 & 0.0877582561890373 & -0.00958851077208406\\\\0 & 1.0 & 0.175516512378075 & 0.0479425538604203 & 0.0175516512378075\\\\0 & 0 & 1.0 & 0 & 0\\\\0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$" ], "text/plain": [ "⎡1.0 0 -0.0958851077208406 0.0877582561890373 -0.00958851077208406⎤\n", "⎢ ⎥\n", "⎢ 0 1.0 0.175516512378075 0.0479425538604203 0.0175516512378075 ⎥\n", "⎢ ⎥\n", "⎢ 0 0 1.0 0 0 ⎥\n", "⎢ ⎥\n", "⎢ 0 0 0 1.0 0 ⎥\n", "⎢ ⎥\n", "⎣ 0 0 0 0 1.0 ⎦" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "speed, yaw, dt, x, y, t = symbols(\"v \\\\theta \\Delta{t} x y t\")\n", "turn_rate_non_zero = Symbol(\"\\omega\", nonzero=True, finite=True)\n", "zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n", "\n", "state_cvtr = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n", "print(\"CVRT state is:\")\n", "display(state_cvtr)\n", "\n", "\n", "def get_next_state(turn_rate):\n", " # Specify the functions for yaw, x, and y.\n", " yaw_func = yaw + turn_rate * t\n", " x_speed_func = speed * cos(yaw_func)\n", " y_speed_func = speed * sin(yaw_func)\n", "\n", " # Get next state by integrating the functions.\n", " next_yaw = yaw + integrate(turn_rate, (t, 0, dt))\n", " next_x = x + integrate(x_speed_func, (t, 0, dt))\n", " next_y = y + integrate(y_speed_func, (t, 0, dt))\n", "\n", " return Matrix([next_x, next_y, next_yaw, speed, turn_rate])\n", "\n", "\n", "# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n", "print(\"Assuming a non-zero turn rate, next state is:\")\n", "next_state = get_next_state(turn_rate_non_zero)\n", "display(next_state)\n", "substitutes = {\n", " x: 42,\n", " y: 23,\n", " yaw: 0.5,\n", " speed: 2,\n", " dt: 0.1,\n", " turn_rate_non_zero: 2,\n", " zero_turn_rate: 0,\n", "}\n", "display(Markdown(\"Plugging in the numbers:\"), substitutes)\n", "display(next_state.evalf(subs=substitutes))\n", "\n", "state = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n", "print(\"Jacobian of the next state with respect to the previous state:\")\n", "J = next_state.jacobian(state)\n", "display(J)\n", "\n", "display(Markdown(\"Plugging in the numbers:\"), substitutes)\n", "display(J.evalf(subs=substitutes))\n", "\n", "print(\"Assuming a zero turn rate, next state is:\")\n", "next_state = get_next_state(zero_turn_rate)\n", "display(next_state)\n", "display(Markdown(\"Plugging in the numbers:\"), substitutes)\n", "display(next_state.evalf(subs=substitutes))\n", "\n", "state = Matrix([x, y, yaw, speed, zero_turn_rate])\n", "print(\n", " \"Jacobian of the next state with respect to the previous one with zero turn rate:\"\n", ")\n", "J = next_state.jacobian(state)\n", "display(J)\n", "\n", "display(Markdown(\"Plugging in the numbers:\"), substitutes)\n", "display(J.evalf(subs=substitutes))" ] } ], "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.8.6" } }, "nbformat": 4, "nbformat_minor": 4 }