1237 lines
107 KiB
Plaintext
1237 lines
107 KiB
Plaintext
{
|
|
"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": [
|
|
"<IPython.core.display.Markdown object>"
|
|
]
|
|
},
|
|
"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": [
|
|
"<IPython.core.display.Markdown object>"
|
|
]
|
|
},
|
|
"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": [
|
|
"<IPython.core.display.Markdown object>"
|
|
]
|
|
},
|
|
"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": [
|
|
"<IPython.core.display.Markdown object>"
|
|
]
|
|
},
|
|
"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
|
|
}
|