3006 lines
410 KiB
Plaintext
3006 lines
410 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# EKF Derivations"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import symforce\n",
|
|
"symforce.set_symbolic_api(\"sympy\")\n",
|
|
"symforce.set_log_level(\"warning\")\n",
|
|
"import symforce.symbolic as sf\n",
|
|
"from symforce import ops\n",
|
|
"from symforce.notebook_util import display"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# Accelerometer and gyroscope reading\n",
|
|
"gyro = sf.Vector3.symbolic('\\omega')\n",
|
|
"accel = sf.Vector3.symbolic('a')\n",
|
|
"# Accelerometer and gyroscope bias\n",
|
|
"b_a = sf.Vector3.symbolic('b_a')\n",
|
|
"b_g = sf.Vector3.symbolic('b_g')\n",
|
|
"# Gravity in world frame\n",
|
|
"gravity = sf.Vector3([0, 0, -9.8])\n",
|
|
"# Foot positions in world frame\n",
|
|
"s1 = sf.Vector3.symbolic('s_{FR}')\n",
|
|
"s2 = sf.Vector3.symbolic('s_{FL}')\n",
|
|
"s3 = sf.Vector3.symbolic('s_{RR}')\n",
|
|
"s4 = sf.Vector3.symbolic('s_{RL}')\n",
|
|
"# Integration time interval\n",
|
|
"dT = sf.Symbol('\\Delta t')\n",
|
|
"# Body orientation, position, and velocity \n",
|
|
"q = sf.Quaternion.symbolic('q')\n",
|
|
"p = sf.Vector3.symbolic('p')\n",
|
|
"v = sf.Vector3.symbolic('v')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ qₓ ⎤\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_y ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_z ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_w ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g0 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g1 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g2 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎣s_{RL}2⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"state = sf.Matrix.block_matrix([[sf.Matrix([q.x, q.y, q.z, q.w])],\\\n",
|
|
" [p],\\\n",
|
|
" [v],\\\n",
|
|
" [b_g],\\\n",
|
|
" [b_a],\\\n",
|
|
" [s1],\\\n",
|
|
" [s2],\\\n",
|
|
" [s3],\\\n",
|
|
" [s4]])\n",
|
|
"state"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}\\omega0\\\\\\omega1\\\\\\omega2\\\\a_{0}\\\\a_{1}\\\\a_{2}\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡\\omega0⎤\n",
|
|
"⎢ ⎥\n",
|
|
"⎢\\omega1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢\\omega2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ a₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ a₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎣ a₂ ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"u = sf.Matrix.block_matrix([[gyro], [accel]])\n",
|
|
"u"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# Process Model\n",
|
|
"f1 = q*sf.Quaternion(xyz=(gyro-b_g)*dT, w = 1)\n",
|
|
"# f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n",
|
|
"f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n",
|
|
"f2 = p+dT*v\n",
|
|
"f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n",
|
|
"f4 = b_g\n",
|
|
"f5 = b_a\n",
|
|
"f6 = s1\n",
|
|
"f7 = s2\n",
|
|
"f8 = s3\n",
|
|
"f9 = s4"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"f = sf.Matrix.block_matrix([[f1],\\\n",
|
|
" [f2],\\\n",
|
|
" [f3],\\\n",
|
|
" [f4],\\\n",
|
|
" [f5],\\\n",
|
|
" [f6],\\\n",
|
|
" [f7],\\\n",
|
|
" [f8],\\\n",
|
|
" [f9]])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & \\Delta t \\left(\\omega0 - b_{g0}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{w} & \\Delta t q_{z} & - \\Delta t q_{y} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & \\Delta t \\left(\\omega0 - b_{g0}\\right) & \\Delta t \\left(\\omega1 - b_{g1}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{z} & - \\Delta t q_{w} & \\Delta t q_{x} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\\\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega0 - b_{g0}\\right) & 1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{y} & - \\Delta t q_{x} & - \\Delta t q_{w} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega0 - b_{g0}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{x} & \\Delta t q_{y} & \\Delta t q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(a_{1} - b_{a1}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{2} - b_{a2}\\right) + 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 4 q_{y} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{1} - b_{a1}\\right) + 2 q_{x} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{0} - b_{a0}\\right) & 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{z} - 2 q_{x} q_{y} & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(a_{2} - b_{a2}\\right) - 4 q_{x} \\left(a_{1} - b_{a1}\\right) + 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{1} - b_{a1}\\right) & - 2 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(a_{1} - b_{a1}\\right) - 4 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{0} - b_{a0}\\right) - 4 q_{y} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 2 q_{w} q_{y} - 2 q_{x} q_{z} & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{array}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ 1 \\Delta\n",
|
|
"⎢ \n",
|
|
"⎢ -\\Delta t⋅(\\omega2 - b_g2) \n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅(\\omega1 - b_g1) -\\Delta\n",
|
|
"⎢ \n",
|
|
"⎢ -\\Delta t⋅(\\omega0 - b_g0) -\\Delta\n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_y⋅(a₁ - bₐ₁) + 2⋅q_z⋅(a₂ - bₐ₂) 2⋅q_w⋅(a₂ - bₐ₂) + 2\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢-2⋅q_w⋅(a₂ - bₐ₂) - 4⋅qₓ⋅(a₁ - bₐ₁) + 2⋅q_y⋅(a₀ - bₐ₀) 2⋅qₓ⋅(a₀ - \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢2⋅q_w⋅(a₁ - bₐ₁) - 4⋅qₓ⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₀ - bₐ₀) -2⋅q_w⋅(a₀ - bₐ₀) - 4\n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 \n",
|
|
"⎢ \n",
|
|
"⎣ 0 \n",
|
|
"\n",
|
|
" t⋅(\\omega2 - b_g2) -\\Delta t⋅(\\omega1 - b_g1) \n",
|
|
" \n",
|
|
" 1 \\Delta t⋅(\\omega0 - b_g0) \n",
|
|
" \n",
|
|
" t⋅(\\omega0 - b_g0) 1 \n",
|
|
" \n",
|
|
" t⋅(\\omega1 - b_g1) -\\Delta t⋅(\\omega2 - b_g2) \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅qₓ⋅(a₁ - bₐ₁) - 4⋅q_y⋅(a₀ - bₐ₀) -2⋅q_w⋅(a₁ - bₐ₁) + 2⋅qₓ⋅(a₂ - bₐ₂) - 4⋅q_\n",
|
|
" \n",
|
|
" \n",
|
|
"bₐ₀) + 2⋅q_z⋅(a₂ - bₐ₂) 2⋅q_w⋅(a₀ - bₐ₀) + 2⋅q_y⋅(a₂ - bₐ₂) - 4⋅q_\n",
|
|
" \n",
|
|
" \n",
|
|
"⋅q_y⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₁ - bₐ₁) 2⋅qₓ⋅(a₀ - bₐ₀) + 2⋅q_y⋅(a₁ - bₐ\n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
" \n",
|
|
" 0 0 \n",
|
|
"\n",
|
|
" \\Delta t⋅(\\omega0 - b_g0) 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \\Delta t⋅(\\omega1 - b_g1) 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \\Delta t⋅(\\omega2 - b_g2) 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 1 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 1 0 0 \\Delta t 0 \n",
|
|
" \n",
|
|
" 0 0 1 0 0 \\Delta t\n",
|
|
" \n",
|
|
" 0 0 0 1 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"z⋅(a₀ - bₐ₀) 2⋅q_y⋅(a₂ - bₐ₂) - 2⋅q_z⋅(a₁ - bₐ₁) 0 0 0 1 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"z⋅(a₁ - bₐ₁) -2⋅qₓ⋅(a₂ - bₐ₂) + 2⋅q_z⋅(a₀ - bₐ₀) 0 0 0 0 1 \n",
|
|
" \n",
|
|
" \n",
|
|
"₁) 2⋅qₓ⋅(a₁ - bₐ₁) - 2⋅q_y⋅(a₀ - bₐ₀) 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
"\n",
|
|
" 0 -\\Delta t⋅q_w \\Delta t⋅q_z -\\Delta t⋅q_y 0 \n",
|
|
" \n",
|
|
" 0 -\\Delta t⋅q_z -\\Delta t⋅q_w \\Delta t⋅qₓ 0 \n",
|
|
" \n",
|
|
" 0 \\Delta t⋅q_y -\\Delta t⋅qₓ -\\Delta t⋅q_w 0 \n",
|
|
" \n",
|
|
" 0 \\Delta t⋅qₓ \\Delta t⋅q_y \\Delta t⋅q_z 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \\Delta t 0 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 0 0 0 2⋅q_y + 2⋅q_z - 1 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y\n",
|
|
" \n",
|
|
" \n",
|
|
" 1 0 0 0 2⋅q_w⋅q_y - 2⋅qₓ⋅q_z \n",
|
|
" \n",
|
|
" 0 1 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 1 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 1 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 1 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 \n",
|
|
"\n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 2⋅qₓ + 2⋅q_z - 1 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" -2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅qₓ + 2⋅q_y - 1 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 1 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 1 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 1 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 1 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 1 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 1 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 1 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 1 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 1 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 1 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 1 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 1 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"\n",
|
|
"0 0⎤\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"0 0⎥\n",
|
|
" ⎥\n",
|
|
"1 0⎥\n",
|
|
" ⎥\n",
|
|
"0 1⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"A_x = f.jacobian(state)\n",
|
|
"A_x "
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} & - \\Delta t q_{z} & \\Delta t q_{y} & 0 & 0 & 0\\\\\\Delta t q_{z} & \\Delta t q_{w} & - \\Delta t q_{x} & 0 & 0 & 0\\\\- \\Delta t q_{y} & \\Delta t q_{x} & \\Delta t q_{w} & 0 & 0 & 0\\\\- \\Delta t q_{x} & - \\Delta t q_{y} & - \\Delta t q_{z} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & 2 q_{w} q_{y} + 2 q_{x} q_{z}\\\\0 & 0 & 0 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & - 2 q_{w} q_{x} + 2 q_{y} q_{z}\\\\0 & 0 & 0 & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡\\Delta t⋅q_w -\\Delta t⋅q_z \\Delta t⋅q_y 0 \n",
|
|
"⎢ \n",
|
|
"⎢\\Delta t⋅q_z \\Delta t⋅q_w -\\Delta t⋅qₓ 0 \n",
|
|
"⎢ \n",
|
|
"⎢-\\Delta t⋅q_y \\Delta t⋅qₓ \\Delta t⋅q_w 0 \n",
|
|
"⎢ \n",
|
|
"⎢-\\Delta t⋅qₓ -\\Delta t⋅q_y -\\Delta t⋅q_z 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 2 2 \n",
|
|
"⎢ 0 0 0 - 2⋅q_y - 2⋅q_z + 1 -2⋅q_w⋅q_\n",
|
|
"⎢ \n",
|
|
"⎢ 2 \n",
|
|
"⎢ 0 0 0 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ -\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎣ 0 0 0 0 \n",
|
|
"\n",
|
|
" 0 0 ⎤\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"z + 2⋅qₓ⋅q_y 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z ⎥\n",
|
|
" ⎥\n",
|
|
" 2 ⎥\n",
|
|
" 2⋅q_z + 1 -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z⎥\n",
|
|
" ⎥\n",
|
|
" 2 2 ⎥\n",
|
|
"+ 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"A_u = f.jacobian(u)\n",
|
|
"A_u"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}v_{0} \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + v_{1} \\cdot \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{2} \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{0} \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{1} \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + v_{2} \\cdot \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{0} \\cdot \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + v_{1} \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + v_{2} \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ ⎛ 2 2 ⎞ \n",
|
|
"⎢ v₀⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + v₁⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢ v₀⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v₁⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠ + \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ v₀⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + v₁⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) + \n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{RL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{RL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎣(-p₀ + s_{RL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"\n",
|
|
" ⎤\n",
|
|
"₂⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"v₂⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"v₂⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{FR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{FR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{FR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{FL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{FL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{FL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{RR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{RR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{RR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{RL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{RL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{RL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"#Measurement model\n",
|
|
"R = sf.Rot3(q).to_rotation_matrix()\n",
|
|
"z1 = R.T*v\n",
|
|
"z2 = R.T*(s1-p)\n",
|
|
"z3 = R.T*(s2-p)\n",
|
|
"z4 = R.T*(s3-p)\n",
|
|
"z5 = R.T*(s4-p)\n",
|
|
"h = sf.Matrix.block_matrix([[z1],\n",
|
|
" [z2],\n",
|
|
" [z3],\n",
|
|
" [z4],\n",
|
|
" [z5]])\n",
|
|
"h"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 10,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}2 q_{y} v_{1} + 2 q_{z} v_{2} & - 2 q_{w} v_{2} + 2 q_{x} v_{1} - 4 q_{y} v_{0} & 2 q_{w} v_{1} + 2 q_{x} v_{2} - 4 q_{z} v_{0} & - 2 q_{y} v_{2} + 2 q_{z} v_{1} & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} v_{2} - 4 q_{x} v_{1} + 2 q_{y} v_{0} & 2 q_{x} v_{0} + 2 q_{z} v_{2} & - 2 q_{w} v_{0} + 2 q_{y} v_{2} - 4 q_{z} v_{1} & 2 q_{x} v_{2} - 2 q_{z} v_{0} & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} v_{1} - 4 q_{x} v_{2} + 2 q_{z} v_{0} & 2 q_{w} v_{0} - 4 q_{y} v_{2} + 2 q_{z} v_{1} & 2 q_{x} v_{0} + 2 q_{y} v_{1} & - 2 q_{x} v_{1} + 2 q_{y} v_{0} & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{z} \\left(- p_{2} + s_{FR}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{x} \\left(- p_{1} + s_{FR}1\\right) - 4 q_{y} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{x} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{z} \\left(- p_{0} + s_{FR}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{x} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{x} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{z} \\left(- p_{2} + s_{FR}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{y} \\left(- p_{2} + s_{FR}2\\right) - 4 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{x} \\left(- p_{2} + s_{FR}2\\right) - 2 q_{z} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{FR}1\\right) - 4 q_{x} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{0} + s_{FR}0\\right) & 2 q_{w} \\left(- p_{0} + s_{FR}0\\right) - 4 q_{y} \\left(- p_{2} + s_{FR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FR}1\\right) & 2 q_{x} \\left(- p_{0} + s_{FR}0\\right) + 2 q_{y} \\left(- p_{1} + s_{FR}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{FR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FR}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{z} \\left(- p_{2} + s_{FL}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{x} \\left(- p_{1} + s_{FL}1\\right) - 4 q_{y} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{x} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{z} \\left(- p_{0} + s_{FL}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{x} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{x} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{z} \\left(- p_{2} + s_{FL}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{y} \\left(- p_{2} + s_{FL}2\\right) - 4 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{x} \\left(- p_{2} + s_{FL}2\\right) - 2 q_{z} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{FL}1\\right) - 4 q_{x} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{0} + s_{FL}0\\right) & 2 q_{w} \\left(- p_{0} + s_{FL}0\\right) - 4 q_{y} \\left(- p_{2} + s_{FL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{FL}1\\right) & 2 q_{x} \\left(- p_{0} + s_{FL}0\\right) + 2 q_{y} \\left(- p_{1} + s_{FL}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{FL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{FL}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{z} \\left(- p_{2} + s_{RR}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{x} \\left(- p_{1} + s_{RR}1\\right) - 4 q_{y} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{x} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{z} \\left(- p_{0} + s_{RR}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z} & 0 & 0 & 0\\\\2 q_{w} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{x} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{x} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{z} \\left(- p_{2} + s_{RR}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{y} \\left(- p_{2} + s_{RR}2\\right) - 4 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{x} \\left(- p_{2} + s_{RR}2\\right) - 2 q_{z} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z} & 0 & 0 & 0\\\\- 2 q_{w} \\left(- p_{1} + s_{RR}1\\right) - 4 q_{x} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{0} + s_{RR}0\\right) & 2 q_{w} \\left(- p_{0} + s_{RR}0\\right) - 4 q_{y} \\left(- p_{2} + s_{RR}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RR}1\\right) & 2 q_{x} \\left(- p_{0} + s_{RR}0\\right) + 2 q_{y} \\left(- p_{1} + s_{RR}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{RR}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RR}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1 & 0 & 0 & 0\\\\2 q_{y} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{z} \\left(- p_{2} + s_{RL}2\\right) & - 2 q_{w} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{x} \\left(- p_{1} + s_{RL}1\\right) - 4 q_{y} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{x} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{z} \\left(- p_{0} + s_{RL}0\\right) & - 2 q_{y} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{y}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{w} q_{y} + 2 q_{x} q_{z}\\\\2 q_{w} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{x} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{x} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{z} \\left(- p_{2} + s_{RL}2\\right) & - 2 q_{w} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{y} \\left(- p_{2} + s_{RL}2\\right) - 4 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{x} \\left(- p_{2} + s_{RL}2\\right) - 2 q_{z} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & - 2 q_{x}^{2} - 2 q_{z}^{2} + 1 & 2 q_{w} q_{x} + 2 q_{y} q_{z}\\\\- 2 q_{w} \\left(- p_{1} + s_{RL}1\\right) - 4 q_{x} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{0} + s_{RL}0\\right) & 2 q_{w} \\left(- p_{0} + s_{RL}0\\right) - 4 q_{y} \\left(- p_{2} + s_{RL}2\\right) + 2 q_{z} \\left(- p_{1} + s_{RL}1\\right) & 2 q_{x} \\left(- p_{0} + s_{RL}0\\right) + 2 q_{y} \\left(- p_{1} + s_{RL}1\\right) & - 2 q_{x} \\left(- p_{1} + s_{RL}1\\right) + 2 q_{y} \\left(- p_{0} + s_{RL}0\\right) & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 2 q_{w} q_{y} + 2 q_{x} q_{z} & - 2 q_{w} q_{x} + 2 q_{y} q_{z} & - 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\end{array}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ \n",
|
|
"⎢ 2⋅q_y⋅v₁ + 2⋅q_z⋅v₂ \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_w⋅v₂ - 4⋅qₓ⋅v₁ + 2⋅q_y⋅v₀ \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ -2⋅q_w⋅v₁ - 4⋅qₓ⋅v₂ + 2⋅q_z⋅v₀ \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_y⋅(-p₁ + s_{FR}1) + 2⋅q_z⋅(-p₂ + s_{FR}2) -2⋅q_w\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢2⋅q_w⋅(-p₂ + s_{FR}2) - 4⋅qₓ⋅(-p₁ + s_{FR}1) + 2⋅q_y⋅(-p₀ + s_{FR}0) \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢-2⋅q_w⋅(-p₁ + s_{FR}1) - 4⋅qₓ⋅(-p₂ + s_{FR}2) + 2⋅q_z⋅(-p₀ + s_{FR}0) 2⋅q_w⋅\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_y⋅(-p₁ + s_{FL}1) + 2⋅q_z⋅(-p₂ + s_{FL}2) -2⋅q_w\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢2⋅q_w⋅(-p₂ + s_{FL}2) - 4⋅qₓ⋅(-p₁ + s_{FL}1) + 2⋅q_y⋅(-p₀ + s_{FL}0) \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢-2⋅q_w⋅(-p₁ + s_{FL}1) - 4⋅qₓ⋅(-p₂ + s_{FL}2) + 2⋅q_z⋅(-p₀ + s_{FL}0) 2⋅q_w⋅\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_y⋅(-p₁ + s_{RR}1) + 2⋅q_z⋅(-p₂ + s_{RR}2) -2⋅q_w\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢2⋅q_w⋅(-p₂ + s_{RR}2) - 4⋅qₓ⋅(-p₁ + s_{RR}1) + 2⋅q_y⋅(-p₀ + s_{RR}0) \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢-2⋅q_w⋅(-p₁ + s_{RR}1) - 4⋅qₓ⋅(-p₂ + s_{RR}2) + 2⋅q_z⋅(-p₀ + s_{RR}0) 2⋅q_w⋅\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ 2⋅q_y⋅(-p₁ + s_{RL}1) + 2⋅q_z⋅(-p₂ + s_{RL}2) -2⋅q_w\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢2⋅q_w⋅(-p₂ + s_{RL}2) - 4⋅qₓ⋅(-p₁ + s_{RL}1) + 2⋅q_y⋅(-p₀ + s_{RL}0) \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎣-2⋅q_w⋅(-p₁ + s_{RL}1) - 4⋅qₓ⋅(-p₂ + s_{RL}2) + 2⋅q_z⋅(-p₀ + s_{RL}0) 2⋅q_w⋅\n",
|
|
"\n",
|
|
" \n",
|
|
" -2⋅q_w⋅v₂ + 2⋅qₓ⋅v₁ - 4⋅q_y⋅v₀ \n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅v₀ + 2⋅q_z⋅v₂ \n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅q_w⋅v₀ - 4⋅q_y⋅v₂ + 2⋅q_z⋅v₁ \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅(-p₂ + s_{FR}2) + 2⋅qₓ⋅(-p₁ + s_{FR}1) - 4⋅q_y⋅(-p₀ + s_{FR}0) 2⋅q_w⋅(-p₁ +\n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅(-p₀ + s_{FR}0) + 2⋅q_z⋅(-p₂ + s_{FR}2) -2⋅q_w⋅(-p₀ +\n",
|
|
" \n",
|
|
" \n",
|
|
"(-p₀ + s_{FR}0) - 4⋅q_y⋅(-p₂ + s_{FR}2) + 2⋅q_z⋅(-p₁ + s_{FR}1) \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅(-p₂ + s_{FL}2) + 2⋅qₓ⋅(-p₁ + s_{FL}1) - 4⋅q_y⋅(-p₀ + s_{FL}0) 2⋅q_w⋅(-p₁ +\n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅(-p₀ + s_{FL}0) + 2⋅q_z⋅(-p₂ + s_{FL}2) -2⋅q_w⋅(-p₀ +\n",
|
|
" \n",
|
|
" \n",
|
|
"(-p₀ + s_{FL}0) - 4⋅q_y⋅(-p₂ + s_{FL}2) + 2⋅q_z⋅(-p₁ + s_{FL}1) \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅(-p₂ + s_{RR}2) + 2⋅qₓ⋅(-p₁ + s_{RR}1) - 4⋅q_y⋅(-p₀ + s_{RR}0) 2⋅q_w⋅(-p₁ +\n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅(-p₀ + s_{RR}0) + 2⋅q_z⋅(-p₂ + s_{RR}2) -2⋅q_w⋅(-p₀ +\n",
|
|
" \n",
|
|
" \n",
|
|
"(-p₀ + s_{RR}0) - 4⋅q_y⋅(-p₂ + s_{RR}2) + 2⋅q_z⋅(-p₁ + s_{RR}1) \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅(-p₂ + s_{RL}2) + 2⋅qₓ⋅(-p₁ + s_{RL}1) - 4⋅q_y⋅(-p₀ + s_{RL}0) 2⋅q_w⋅(-p₁ +\n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅(-p₀ + s_{RL}0) + 2⋅q_z⋅(-p₂ + s_{RL}2) -2⋅q_w⋅(-p₀ +\n",
|
|
" \n",
|
|
" \n",
|
|
"(-p₀ + s_{RL}0) - 4⋅q_y⋅(-p₂ + s_{RL}2) + 2⋅q_z⋅(-p₁ + s_{RL}1) \n",
|
|
"\n",
|
|
" \n",
|
|
" 2⋅q_w⋅v₁ + 2⋅qₓ⋅v₂ - 4⋅q_z⋅v₀ -2⋅q_y\n",
|
|
" \n",
|
|
" \n",
|
|
" -2⋅q_w⋅v₀ + 2⋅q_y⋅v₂ - 4⋅q_z⋅v₁ 2⋅qₓ⋅\n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅v₀ + 2⋅q_y⋅v₁ -2⋅qₓ⋅\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{FR}1) + 2⋅qₓ⋅(-p₂ + s_{FR}2) - 4⋅q_z⋅(-p₀ + s_{FR}0) -2⋅q_y⋅(-p₂ + s_{FR\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{FR}0) + 2⋅q_y⋅(-p₂ + s_{FR}2) - 4⋅q_z⋅(-p₁ + s_{FR}1) 2⋅qₓ⋅(-p₂ + s_{FR}\n",
|
|
" \n",
|
|
" \n",
|
|
"2⋅qₓ⋅(-p₀ + s_{FR}0) + 2⋅q_y⋅(-p₁ + s_{FR}1) -2⋅qₓ⋅(-p₁ + s_{FR}\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{FL}1) + 2⋅qₓ⋅(-p₂ + s_{FL}2) - 4⋅q_z⋅(-p₀ + s_{FL}0) -2⋅q_y⋅(-p₂ + s_{FL\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{FL}0) + 2⋅q_y⋅(-p₂ + s_{FL}2) - 4⋅q_z⋅(-p₁ + s_{FL}1) 2⋅qₓ⋅(-p₂ + s_{FL}\n",
|
|
" \n",
|
|
" \n",
|
|
"2⋅qₓ⋅(-p₀ + s_{FL}0) + 2⋅q_y⋅(-p₁ + s_{FL}1) -2⋅qₓ⋅(-p₁ + s_{FL}\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{RR}1) + 2⋅qₓ⋅(-p₂ + s_{RR}2) - 4⋅q_z⋅(-p₀ + s_{RR}0) -2⋅q_y⋅(-p₂ + s_{RR\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{RR}0) + 2⋅q_y⋅(-p₂ + s_{RR}2) - 4⋅q_z⋅(-p₁ + s_{RR}1) 2⋅qₓ⋅(-p₂ + s_{RR}\n",
|
|
" \n",
|
|
" \n",
|
|
"2⋅qₓ⋅(-p₀ + s_{RR}0) + 2⋅q_y⋅(-p₁ + s_{RR}1) -2⋅qₓ⋅(-p₁ + s_{RR}\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{RL}1) + 2⋅qₓ⋅(-p₂ + s_{RL}2) - 4⋅q_z⋅(-p₀ + s_{RL}0) -2⋅q_y⋅(-p₂ + s_{RL\n",
|
|
" \n",
|
|
" \n",
|
|
" s_{RL}0) + 2⋅q_y⋅(-p₂ + s_{RL}2) - 4⋅q_z⋅(-p₁ + s_{RL}1) 2⋅qₓ⋅(-p₂ + s_{RL}\n",
|
|
" \n",
|
|
" \n",
|
|
"2⋅qₓ⋅(-p₀ + s_{RL}0) + 2⋅q_y⋅(-p₁ + s_{RL}1) -2⋅qₓ⋅(-p₁ + s_{RL}\n",
|
|
"\n",
|
|
" \n",
|
|
"⋅v₂ + 2⋅q_z⋅v₁ 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"v₂ - 2⋅q_z⋅v₀ 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"v₁ + 2⋅q_y⋅v₀ 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"}2) + 2⋅q_z⋅(-p₁ + s_{FR}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"2) - 2⋅q_z⋅(-p₀ + s_{FR}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
"1) + 2⋅q_y⋅(-p₀ + s_{FR}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"}2) + 2⋅q_z⋅(-p₁ + s_{FL}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"2) - 2⋅q_z⋅(-p₀ + s_{FL}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
"1) + 2⋅q_y⋅(-p₀ + s_{FL}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"}2) + 2⋅q_z⋅(-p₁ + s_{RR}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"2) - 2⋅q_z⋅(-p₀ + s_{RR}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
"1) + 2⋅q_y⋅(-p₀ + s_{RR}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"}2) + 2⋅q_z⋅(-p₁ + s_{RL}1) 2⋅q_y + 2⋅q_z - 1 -2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅q\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"2) - 2⋅q_z⋅(-p₀ + s_{RL}0) 2⋅q_w⋅q_z - 2⋅qₓ⋅q_y 2⋅qₓ + 2⋅q_z - 1 -2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
"1) + 2⋅q_y⋅(-p₀ + s_{RL}0) -2⋅q_w⋅q_y - 2⋅qₓ⋅q_z 2⋅q_w⋅qₓ - 2⋅q_y⋅q_z 2⋅\n",
|
|
"\n",
|
|
" 2 2 \n",
|
|
" 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y +\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2\n",
|
|
" \n",
|
|
" 2 \n",
|
|
" 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
"_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"qₓ + 2⋅q_y - 1 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"qₓ + 2⋅q_y - 1 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"qₓ + 2⋅q_y - 1 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"_w⋅q_y - 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"q_w⋅qₓ - 2⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"qₓ + 2⋅q_y - 1 0 0 0 \n",
|
|
"\n",
|
|
" \n",
|
|
" 2⋅qₓ⋅q_z 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅q_y⋅q_z 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 2 \n",
|
|
"q_y + 1 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 0 0 0 0 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 0 0 0 0 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q\n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2\n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 0 0 \n",
|
|
"\n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"q_w⋅q_y + 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"_w⋅qₓ + 2⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
"⋅qₓ - 2⋅q_y + 1 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y +\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 0 -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2\n",
|
|
" \n",
|
|
" 2 \n",
|
|
" 0 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅\n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 0 \n",
|
|
"\n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 2⋅qₓ⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
"⋅q_y⋅q_z 0 0 0 \n",
|
|
" \n",
|
|
" 2 \n",
|
|
"q_y + 1 0 0 0 \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z\n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2⋅q_y⋅q_z \n",
|
|
" \n",
|
|
" 2 2 \n",
|
|
" 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
" \n",
|
|
" \n",
|
|
" 0 0 0 \n",
|
|
"\n",
|
|
" ⎤\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 2 2 ⎥\n",
|
|
" - 2⋅q_y - 2⋅q_z + 1 2⋅q_w⋅q_z + 2⋅qₓ⋅q_y -2⋅q_w⋅q_y + 2⋅qₓ⋅q_z⎥\n",
|
|
" ⎥\n",
|
|
" 2 2 ⎥\n",
|
|
" -2⋅q_w⋅q_z + 2⋅qₓ⋅q_y - 2⋅qₓ - 2⋅q_z + 1 2⋅q_w⋅qₓ + 2⋅q_y⋅q_z ⎥\n",
|
|
" ⎥\n",
|
|
" 2 2 ⎥\n",
|
|
" 2⋅q_w⋅q_y + 2⋅qₓ⋅q_z -2⋅q_w⋅qₓ + 2⋅q_y⋅q_z - 2⋅qₓ - 2⋅q_y + 1 ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"H = h.jacobian(state)\n",
|
|
"H"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 11,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{array}{ccccccccccccccc}R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & R_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & R_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RR}} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}} & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & R_{s_{RL}}\\end{array}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡Rᵥ 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 Rᵥ 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 Rᵥ 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 R_{s_{FR}} 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 R_{s_{FR}} 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 R_{s_{FR}} 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 R_{s_{FL}} 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 R_{s_{FL}} 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 R_{s_\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎢0 0 0 0 0 0 0 0 0\n",
|
|
"⎢ \n",
|
|
"⎣0 0 0 0 0 0 0 0 0\n",
|
|
"\n",
|
|
" 0 0 0 0 0 0 ⎤\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
"{FL}} 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" R_{s_{RR}} 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 R_{s_{RR}} 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 R_{s_{RR}} 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 R_{s_{RL}} 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 R_{s_{RL}} 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 R_{s_{RL}}⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"R_vel = sf.Symbol('R_v')\n",
|
|
"R_s1 = sf.Symbol('R_{s_{FR}}')\n",
|
|
"R_s2 = sf.Symbol('R_{s_{FL}}')\n",
|
|
"R_s3 = sf.Symbol('R_{s_{RR}}')\n",
|
|
"R_s4 = sf.Symbol('R_{s_{RL}}')\n",
|
|
"\n",
|
|
"R_cov = sf.Matrix.diag([R_vel,R_vel,R_vel,\n",
|
|
" R_s1, R_s1, R_s1,\n",
|
|
" R_s2, R_s2, R_s2,\n",
|
|
" R_s3, R_s3, R_s3,\n",
|
|
" R_s4, R_s4, R_s4])\n",
|
|
"R_cov"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 12,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & Q_{q} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & Q_{p} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{v} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{bg} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{ba} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FR}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{FL}} & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RR}} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}} & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & Q_{s_{RL}}\\end{array}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 Q_q 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 Qₚ 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 Qᵥ 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 Q_bg 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_ba 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_ba 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Q_b\n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎢ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"⎢ \n",
|
|
"⎣ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 \n",
|
|
"\n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
"a 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" Q_{s_{FR}} 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 Q_{s_{FR}} 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 Q_{s_{FR}} 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 Q_{s_{FL}} 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 Q_{s_{FL}} 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 Q_{s_{FL}} \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 Q_{\n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
" \n",
|
|
" 0 0 0 0 0 0 \n",
|
|
"\n",
|
|
" 0 0 0 0 0 0 ⎤\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
"s_{RR}} 0 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 Q_{s_{RR}} 0 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 Q_{s_{RR}} 0 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 Q_{s_{RL}} 0 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 Q_{s_{RL}} 0 ⎥\n",
|
|
" ⎥\n",
|
|
" 0 0 0 0 0 Q_{s_{RL}}⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"\n",
|
|
"Q_vel = sf.Symbol('Q_v')\n",
|
|
"Q_pos = sf.Symbol('Q_p')\n",
|
|
"Q_quat = sf.Symbol('Q_q')\n",
|
|
"Q_bg = sf.Symbol('Q_bg')\n",
|
|
"Q_ba = sf.Symbol('Q_ba')\n",
|
|
"Q_s1 = sf.Symbol('Q_{s_{FR}}')\n",
|
|
"Q_s2 = sf.Symbol('Q_{s_{FL}}')\n",
|
|
"Q_s3 = sf.Symbol('Q_{s_{RR}}')\n",
|
|
"Q_s4 = sf.Symbol('Q_{s_{RL}}')\n",
|
|
"\n",
|
|
"Q_cov = sf.Matrix.diag([Q_quat,Q_quat,Q_quat,Q_quat,\n",
|
|
" Q_pos, Q_pos, Q_pos,\n",
|
|
" Q_vel, Q_vel, Q_vel,\n",
|
|
" Q_bg, Q_bg, Q_bg,\n",
|
|
" Q_ba, Q_ba, Q_ba,\n",
|
|
" Q_s1, Q_s1, Q_s1,\n",
|
|
" Q_s2, Q_s2, Q_s2,\n",
|
|
" Q_s3, Q_s3, Q_s3,\n",
|
|
" Q_s4, Q_s4, Q_s4])\n",
|
|
"Q_cov\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ qₓ ⎤\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_y ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_z ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_w ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g0 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g1 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g2 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎣s_{RL}2⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"state"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 14,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"P = sf.matrix_type_from_shape((28,28)).symbolic('P')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 15,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}q_{x}\\\\q_{y}\\\\q_{z}\\\\q_{w}\\\\p_{0}\\\\p_{1}\\\\p_{2}\\\\v_{0}\\\\v_{1}\\\\v_{2}\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ qₓ ⎤\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_y ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_z ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ q_w ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ p₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ v₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g0 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g1 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ b_g2 ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₀ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₁ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢ bₐ₂ ⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{FL}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RR}2⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}0⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎢s_{RL}1⎥\n",
|
|
"⎢ ⎥\n",
|
|
"⎣s_{RL}2⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"state = sf.Matrix.block_matrix([[sf.Matrix([q.x, q.y, q.z, q.w])],\\\n",
|
|
" [p],\\\n",
|
|
" [v],\\\n",
|
|
" [b_g],\\\n",
|
|
" [b_a],\\\n",
|
|
" [s1],\\\n",
|
|
" [s2],\\\n",
|
|
" [s3],\\\n",
|
|
" [s4]])\n",
|
|
"state"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## Generate Codes"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 16,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def compute_mean( state: sf.matrix_type_from_shape((28,1)), u: sf.matrix_type_from_shape((6,1)), dT: sf.Scalar = 0, epsilon: sf.Scalar = 0) -> sf.matrix_type_from_shape((28,1)):\n",
|
|
" omega = u[0:3]\n",
|
|
" accel = u[3:6]\n",
|
|
" q = sf.Quaternion(xyz=state[0:3], w=state[3])\n",
|
|
" p = state[4:7]\n",
|
|
" v = state[7:10]\n",
|
|
" b_g = state[10:13]\n",
|
|
" b_a = state[13:16]\n",
|
|
" s1 = state[16:19]\n",
|
|
" s2 = state[19:22]\n",
|
|
" s3 = state[22:25]\n",
|
|
" s4 = state[25:28]\n",
|
|
" f1 = q*sf.Quaternion(xyz=(omega-b_g)*dT, w = 1.)\n",
|
|
" # f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n",
|
|
" f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n",
|
|
" f2 = p+dT*v\n",
|
|
" f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n",
|
|
" f4 = b_g\n",
|
|
" f5 = b_a\n",
|
|
" f6 = s1\n",
|
|
" f7 = s2\n",
|
|
" f8 = s3\n",
|
|
" f9 = s4\n",
|
|
" f = sf.Matrix.block_matrix([[f1],\\\n",
|
|
" [f2],\\\n",
|
|
" [f3],\\\n",
|
|
" [f4],\\\n",
|
|
" [f5],\\\n",
|
|
" [f6],\\\n",
|
|
" [f7],\\\n",
|
|
" [f8],\\\n",
|
|
" [f9]])\n",
|
|
" return f"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 17,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} \\left(\\omega0 - b_{g0}\\right) + \\Delta t q_{y} \\left(\\omega2 - b_{g2}\\right) - \\Delta t q_{z} \\left(\\omega1 - b_{g1}\\right) + 1.0 q_{x}\\\\\\Delta t q_{w} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{x} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{z} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{y}\\\\\\Delta t q_{w} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{x} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{y} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{z}\\\\- \\Delta t q_{x} \\left(\\omega0 - b_{g0}\\right) - \\Delta t q_{y} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{z} \\left(\\omega2 - b_{g2}\\right) + 1.0 q_{w}\\\\\\Delta t v_{0} + p_{0}\\\\\\Delta t v_{1} + p_{1}\\\\\\Delta t v_{2} + p_{2}\\\\v_{0} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{1} + \\left(a_{0} - b_{a0}\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{2} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right) + 9.8\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ \\Delta t⋅q_w⋅(\\omega0 - b_g0) + \\Delta t⋅q_y⋅(\\omega2 - b_g2) - \\Delta\n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅q_w⋅(\\omega1 - b_g1) - \\Delta t⋅qₓ⋅(\\omega2 - b_g2) + \\Delta \n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅q_w⋅(\\omega2 - b_g2) + \\Delta t⋅qₓ⋅(\\omega1 - b_g1) - \\Delta \n",
|
|
"⎢ \n",
|
|
"⎢ -\\Delta t⋅qₓ⋅(\\omega0 - b_g0) - \\Delta t⋅q_y⋅(\\omega1 - b_g1) - \\Delta\n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅v₀ + p₀ \n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅v₁ + p₁ \n",
|
|
"⎢ \n",
|
|
"⎢ \\Delta t⋅v₂ + p₂ \n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢ v₀ + (a₀ - bₐ₀)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (a₁ - bₐ₁)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞\n",
|
|
"⎢ v₁ + (a₀ - bₐ₀)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (a₁ - bₐ₁)⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢v₂ + (a₀ - bₐ₀)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (a₁ - bₐ₁)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) +\n",
|
|
"⎢ \n",
|
|
"⎢ b_g0 \n",
|
|
"⎢ \n",
|
|
"⎢ b_g1 \n",
|
|
"⎢ \n",
|
|
"⎢ b_g2 \n",
|
|
"⎢ \n",
|
|
"⎢ bₐ₀ \n",
|
|
"⎢ \n",
|
|
"⎢ bₐ₁ \n",
|
|
"⎢ \n",
|
|
"⎢ bₐ₂ \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FR}0 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FR}1 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FR}2 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FL}0 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FL}1 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{FL}2 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{RR}0 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{RR}1 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{RR}2 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{RL}0 \n",
|
|
"⎢ \n",
|
|
"⎢ s_{RL}1 \n",
|
|
"⎢ \n",
|
|
"⎣ s_{RL}2 \n",
|
|
"\n",
|
|
" t⋅q_z⋅(\\omega1 - b_g1) + 1.0⋅qₓ ⎤\n",
|
|
" ⎥\n",
|
|
"t⋅q_z⋅(\\omega0 - b_g0) + 1.0⋅q_y ⎥\n",
|
|
" ⎥\n",
|
|
"t⋅q_y⋅(\\omega0 - b_g0) + 1.0⋅q_z ⎥\n",
|
|
" ⎥\n",
|
|
" t⋅q_z⋅(\\omega2 - b_g2) + 1.0⋅q_w ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
") + (a₂ - bₐ₂)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" + (a₂ - bₐ₂)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
" (a₂ - bₐ₂)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ + 9.8⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
" ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"f_pred = compute_mean(state, u, dT)\n",
|
|
"f_pred"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 18,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"def compute_measurement( state: sf.matrix_type_from_shape((28,1)), epsilon: sf.Scalar = 0)\\\n",
|
|
" -> sf.matrix_type_from_shape((15,1)):\n",
|
|
" q = sf.Quaternion(xyz=state[0:3], w=state[3])\n",
|
|
" p = state[4:7]\n",
|
|
" v = state[7:10]\n",
|
|
" b_g = state[10:13]\n",
|
|
" b_a = state[13:16]\n",
|
|
" s1 = state[16:19]\n",
|
|
" s2 = state[19:22]\n",
|
|
" s3 = state[22:25]\n",
|
|
" s4 = state[25:28]\n",
|
|
" R = sf.Rot3(q).to_rotation_matrix()\n",
|
|
" z1 = R.T*v\n",
|
|
" z2 = R.T*(s1-p)\n",
|
|
" z3 = R.T*(s2-p)\n",
|
|
" z4 = R.T*(s3-p)\n",
|
|
" z5 = R.T*(s4-p)\n",
|
|
" h = sf.Matrix.block_matrix([[z1],\n",
|
|
" [z2],\n",
|
|
" [z3],\n",
|
|
" [z4],\n",
|
|
" [z5]])\n",
|
|
" return h"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 19,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}v_{0} \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + v_{1} \\cdot \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{2} \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{0} \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + v_{1} \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + v_{2} \\cdot \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{0} \\cdot \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + v_{1} \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + v_{2} \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{FL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{FL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{FL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RR}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RR}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RR}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\\\left(- p_{0} + s_{RL}0\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(- p_{1} + s_{RL}1\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(- p_{2} + s_{RL}2\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right)\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"⎡ ⎛ 2 2 ⎞ \n",
|
|
"⎢ v₀⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + v₁⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢ v₀⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + v₁⋅⎝- 2⋅qₓ - 2⋅q_z + 1⎠ + \n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢ v₀⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + v₁⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) + \n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{FR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{FL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{FL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{FL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{FL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RR}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RR}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎢(-p₀ + s_{RR}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RR}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 ⎞ \n",
|
|
"⎢(-p₀ + s_{RL}0)⋅⎝- 2⋅q_y - 2⋅q_z + 1⎠ + (-p₁ + s_{RL}1)⋅(2⋅q_w⋅q_z + 2⋅qₓ⋅q\n",
|
|
"⎢ \n",
|
|
"⎢ ⎛ 2 2 \n",
|
|
"⎢(-p₀ + s_{RL}0)⋅(-2⋅q_w⋅q_z + 2⋅qₓ⋅q_y) + (-p₁ + s_{RL}1)⋅⎝- 2⋅qₓ - 2⋅q_z +\n",
|
|
"⎢ \n",
|
|
"⎢ \n",
|
|
"⎣(-p₀ + s_{RL}0)⋅(2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) + (-p₁ + s_{RL}1)⋅(-2⋅q_w⋅qₓ + 2⋅q_y⋅q\n",
|
|
"\n",
|
|
" ⎤\n",
|
|
"₂⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"v₂⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"v₂⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{FR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{FR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{FR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{FL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{FL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{FL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{RR}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{RR}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{RR}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎥\n",
|
|
" ⎥\n",
|
|
" ⎥\n",
|
|
"_y) + (-p₂ + s_{RL}2)⋅(-2⋅q_w⋅q_y + 2⋅qₓ⋅q_z)⎥\n",
|
|
" ⎥\n",
|
|
" ⎞ ⎥\n",
|
|
" 1⎠ + (-p₂ + s_{RL}2)⋅(2⋅q_w⋅qₓ + 2⋅q_y⋅q_z) ⎥\n",
|
|
" ⎥\n",
|
|
" ⎛ 2 2 ⎞ ⎥\n",
|
|
"_z) + (-p₂ + s_{RL}2)⋅⎝- 2⋅qₓ - 2⋅q_y + 1⎠ ⎦"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"compute_measurement(state)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 20,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from symforce import codegen\n",
|
|
"from symforce.codegen import codegen_util\n",
|
|
"from symforce.notebook_util import display\n",
|
|
"from symforce.notebook_util import display_code_file\n",
|
|
"from symforce.values import Values"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 21,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n",
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"text/html": [
|
|
"<div class=\"highlight\" style=\"background: #f8f8f8\"><pre style=\"line-height: 125%;\"><span></span><span style=\"color: #3D7B7B; font-style: italic\">// -----------------------------------------------------------------------------</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// This file was autogenerated by symforce from template:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// function/FUNCTION.h.jinja</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// Do NOT modify by hand.</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// -----------------------------------------------------------------------------</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #9C6500\">#pragma once</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #9C6500\">#include</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\"><Eigen/Dense></span>\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">namespace</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #0000FF; font-weight: bold\">sym</span><span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">/**</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * This function was autogenerated from a symbolic function. Do not modify by hand.</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Symbolic function: compute_measurement</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Args:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * state: Matrix28_1</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * epsilon: Scalar</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Outputs:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * res: Matrix15_1</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * res_D_state: (15x28) jacobian of res (15) wrt arg state (28)</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> */</span>\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">template</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\"><</span><span style=\"color: #008000; font-weight: bold\">typename</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #0000FF; font-weight: bold\">Scalar</span><span style=\"color: #666666\">></span>\n",
|
|
"Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1></span><span style=\"color: #bbbbbb\"> </span>ComputeMeasurementWithJacobian0(\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1>&</span><span style=\"color: #bbbbbb\"> </span>state,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>epsilon,\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28>*</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">nullptr</span>)<span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Total ops: 339</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Unused inputs</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #B00040\">void</span>)epsilon;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Input arrays</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Intermediate terms (99)</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp3;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp7<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp6;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp8<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>))<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp10<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp8<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp10;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp3;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp15<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp15;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp17<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp18<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp17<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp6;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp21<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp15;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp23<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp17<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp8<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">17</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">18</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">16</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">20</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">21</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">19</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">23</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">24</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">22</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">26</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">27</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">25</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp41<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp43<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp44<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp45<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp46<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp47<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp48<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp49<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp50<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp51<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp52<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp32;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp53<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp32;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp54<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp55<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp56<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp57<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp58<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp59<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp60<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp61<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp62<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp63<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp64<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp65<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp66<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp67<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp66<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp69<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp66<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp70<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp71<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp72<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp73<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp74<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp75<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp76<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp77<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp78<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp79<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp80<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp81<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp82<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp83<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp66<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp85<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp86<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp87<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp88<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp89<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp32;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp90<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp91<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp92<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp66;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp93<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp94<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp95<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp96<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp97<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp98<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp16;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Output terms (2)</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1></span><span style=\"color: #bbbbbb\"> </span>_res;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp7<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp32;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">if</span><span style=\"color: #bbbbbb\"> </span>(res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">!=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">nullptr</span>)<span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28>&</span><span style=\"color: #bbbbbb\"> </span>_res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #666666\">*</span>res_D_state);\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state.setZero();\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp41;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp43;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp44<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp45;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp46<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp47;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp48;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp49<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp50;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp51<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp52;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp53;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp54<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp55;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp56<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp57;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp58;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp59<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp60;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp61<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp62;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp63;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp64<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp65;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp43<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp67<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp41<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp69;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp70;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp48<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp71;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp47<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp72;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp73;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp53<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp74;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp52<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp75;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp76;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp58<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp77;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp57<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp78;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp79;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp63<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp80;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp62<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp81;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp68<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp82;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp45<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp83<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp70<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp85;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp69;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp50<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp86;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp73<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp87;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp46<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp72;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp55<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp88;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp76<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp89;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp51<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp75;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp60<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp90;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp79<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp91;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp56<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp78;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp65<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp92;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp84<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp82<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp93;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp61<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp81;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp85<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp44<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp83;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp67<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp87;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp49<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp86;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp71;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp89;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp54<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp88;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp74;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp91;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp59<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp90;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp77;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp93;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp64<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp92;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp80;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp94;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp95;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp94;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp95;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp94;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp95;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp94;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp95;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp96;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp96;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp96;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp96;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp97;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp98;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp97;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp98;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp97;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp98;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp97;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp98;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">7</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">7</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">7</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">8</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">8</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">8</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">9</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">9</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">9</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">16</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">16</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">16</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">17</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">17</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">17</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">18</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">18</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">18</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">19</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">19</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">19</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">20</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">20</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">20</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">21</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">21</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">21</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">22</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">22</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">22</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">23</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">23</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">23</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">24</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">24</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">24</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">25</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">25</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">25</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">26</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">26</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">26</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">27</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">27</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">27</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>}\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">return</span><span style=\"color: #bbbbbb\"> </span>_res;\n",
|
|
"}<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// NOLINT(readability/fn_size)</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// NOLINTNEXTLINE(readability/fn_size)</span>\n",
|
|
"}<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// namespace sym</span>\n",
|
|
"</pre></div>\n"
|
|
],
|
|
"text/plain": [
|
|
"<IPython.core.display.HTML object>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"compute_measurement_codegen = codegen.Codegen.function(\n",
|
|
" func=compute_measurement,\n",
|
|
" config=codegen.CppConfig(),\n",
|
|
")\n",
|
|
"\n",
|
|
"codegen_with_jacobians = compute_measurement_codegen.with_jacobians(\n",
|
|
" # Just compute wrt the pose and point, not epsilon\n",
|
|
" which_args=[\"state\"],\n",
|
|
" # Include value, not just jacobians\n",
|
|
" include_results=True,\n",
|
|
")\n",
|
|
"\n",
|
|
"data = codegen_with_jacobians.generate_function()\n",
|
|
"\n",
|
|
"display_code_file(data.generated_files[0], \"C++\")\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 22,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n",
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"text/html": [
|
|
"<div class=\"highlight\" style=\"background: #f8f8f8\"><pre style=\"line-height: 125%;\"><span></span><span style=\"color: #3D7B7B; font-style: italic\">// -----------------------------------------------------------------------------</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// This file was autogenerated by symforce from template:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// function/FUNCTION.h.jinja</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// Do NOT modify by hand.</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// -----------------------------------------------------------------------------</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #9C6500\">#pragma once</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #9C6500\">#include</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\"><Eigen/Dense></span>\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">namespace</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #0000FF; font-weight: bold\">sym</span><span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">/**</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * This function was autogenerated from a symbolic function. Do not modify by hand.</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Symbolic function: compute_mean</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Args:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * state: Matrix28_1</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * u: Matrix61</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * dT: Scalar</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * epsilon: Scalar</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> *</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * Outputs:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * res: Matrix28_1</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> * res_D_state: (28x28) jacobian of res (28) wrt arg state (28)</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"> */</span>\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">template</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\"><</span><span style=\"color: #008000; font-weight: bold\">typename</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #0000FF; font-weight: bold\">Scalar</span><span style=\"color: #666666\">></span>\n",
|
|
"Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1></span><span style=\"color: #bbbbbb\"> </span>ComputeMeanWithJacobian0(\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1>&</span><span style=\"color: #bbbbbb\"> </span>state,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1>&</span><span style=\"color: #bbbbbb\"> </span>u,\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>dT,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>epsilon,\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28>*</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">nullptr</span>)<span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Total ops: 150</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Unused inputs</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #B00040\">void</span>)epsilon;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Input arrays</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Intermediate terms (49)</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp7<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp8<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp10<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp11<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>u(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp15<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>))<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp15;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp17<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp5<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp7;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp18<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp20<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp21<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>std<span style=\"color: #666666\">::</span>pow(state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>),<span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">2</span>));\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp23<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp15<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp10<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp11;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp19<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp20;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp14<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp22<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp27<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp0;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp28<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp1;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp4;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp18<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp3;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp35<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp2;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp37<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp39<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp36;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp6;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp41<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp18;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp42<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp36<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp43<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp44<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp4<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp45<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp46<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp47<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">const</span><span style=\"color: #bbbbbb\"> </span>Scalar<span style=\"color: #bbbbbb\"> </span>_tmp48<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// Output terms (2)</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1></span><span style=\"color: #bbbbbb\"> </span>_res;\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp0<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp1<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp2<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp12<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp16<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp8<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp17<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp21<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp23<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp24<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp25<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp26<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">9.8000000000000007</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">16</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">16</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">17</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">17</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">18</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">18</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">19</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">19</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">20</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">20</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">21</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">21</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">22</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">22</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">23</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">23</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">24</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">24</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">25</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">25</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">26</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">26</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res(<span style=\"color: #666666\">27</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">27</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>);\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">if</span><span style=\"color: #bbbbbb\"> </span>(res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">!=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">nullptr</span>)<span style=\"color: #bbbbbb\"> </span>{\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>Eigen<span style=\"color: #666666\">::</span>Matrix<span style=\"color: #666666\"><</span>Scalar,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">28>&</span><span style=\"color: #bbbbbb\"> </span>_res_D_state<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>(<span style=\"color: #666666\">*</span>res_D_state);\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state.setZero();\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp30;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp33;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp6<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp31<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp34;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp28;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp33<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp37;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp30<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp39;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp38<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp9<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp41;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp35;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp1;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp27;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp43<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp34<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp42;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp3<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp43<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp41<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp44;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp29<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp39;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp1;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp2;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp0;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>Scalar(<span style=\"color: #666666\">1.0</span>);\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp40<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp44;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">2</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>_tmp13<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">*</span><span style=\"color: #bbbbbb\"> </span>state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">0</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span><span style=\"color: #bbbbbb\"> </span>_tmp42;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">3</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp32<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">+</span><span style=\"color: #bbbbbb\"> </span>_tmp37;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">4</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">5</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">6</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">4</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">7</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">7</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">5</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">8</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">8</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">6</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">9</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>dT;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">9</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">10</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp45;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">10</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp46;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">10</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp47;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">10</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp48;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">10</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">10</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">11</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp46;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">11</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp45;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">11</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp48;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">11</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp47;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">11</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">11</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">0</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">12</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp47;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">1</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">12</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp48;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">2</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">12</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp45;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">3</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">12</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp46;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">12</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">12</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">13</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp16;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">13</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp17;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">13</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp24;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">13</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">13</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">14</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp8;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">14</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp23;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">14</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp25;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">14</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">14</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">7</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">-</span>_tmp12;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">8</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp21;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">9</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span>_tmp26;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">15</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">15</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">16</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">16</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">17</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">17</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">18</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">18</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">19</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">19</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">20</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">20</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">21</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">21</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">22</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">22</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">23</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">23</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">24</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">24</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">25</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">25</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">26</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">26</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>_res_D_state(<span style=\"color: #666666\">27</span>,<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">27</span>)<span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">=</span><span style=\"color: #bbbbbb\"> </span><span style=\"color: #666666\">1</span>;\n",
|
|
"<span style=\"color: #bbbbbb\"> </span>}\n",
|
|
"\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #008000; font-weight: bold\">return</span><span style=\"color: #bbbbbb\"> </span>_res;\n",
|
|
"}<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// NOLINT(readability/fn_size)</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\">// NOLINTNEXTLINE(readability/fn_size)</span>\n",
|
|
"}<span style=\"color: #bbbbbb\"> </span><span style=\"color: #3D7B7B; font-style: italic\">// namespace sym</span>\n",
|
|
"</pre></div>\n"
|
|
],
|
|
"text/plain": [
|
|
"<IPython.core.display.HTML object>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"compute_mean_codegen = codegen.Codegen.function(\n",
|
|
" func=compute_mean,\n",
|
|
" config=codegen.CppConfig(),\n",
|
|
")\n",
|
|
"\n",
|
|
"codegen_with_jacobians = compute_mean_codegen.with_jacobians(\n",
|
|
" # Just compute wrt the pose and point, not epsilon\n",
|
|
" which_args=[\"state\"],\n",
|
|
" # Include value, not just jacobians\n",
|
|
" include_results=True,\n",
|
|
")\n",
|
|
"\n",
|
|
"data = codegen_with_jacobians.generate_function()\n",
|
|
"\n",
|
|
"display_code_file(data.generated_files[0], \"C++\")\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 28,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"text/html": [
|
|
"<div class=\"highlight\" style=\"background: #f8f8f8\"><pre style=\"line-height: 125%;\"><span></span><span style=\"color: #3D7B7B; font-style: italic\"># -----------------------------------------------------------------------------</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"># This file was autogenerated by symforce from template:</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"># function/FUNCTION.py.jinja</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"># Do NOT modify by hand.</span>\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"># -----------------------------------------------------------------------------</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #3D7B7B; font-style: italic\"># pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument,unused-import</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">math</span>\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">typing</span> <span style=\"color: #008000; font-weight: bold\">as</span> <span style=\"color: #0000FF; font-weight: bold\">T</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">numpy</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">sym</span>\n",
|
|
"\n",
|
|
"\n",
|
|
"<span style=\"color: #008000; font-weight: bold\">def</span> <span style=\"color: #0000FF\">double_pendulum</span>(state, u, dT, epsilon):\n",
|
|
" <span style=\"color: #3D7B7B; font-style: italic\"># type: (numpy.ndarray, numpy.ndarray, float, float) -> numpy.ndarray</span>\n",
|
|
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #BA2121; font-style: italic\">"""</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> This function was autogenerated from a symbolic function. Do not modify by hand.</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> Symbolic function: compute_mean</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> Args:</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> state: Matrix28_1</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> u: Matrix61</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> dT: Scalar</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> epsilon: Scalar</span>\n",
|
|
"\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> Outputs:</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> res: Matrix28_1</span>\n",
|
|
"<span style=\"color: #BA2121; font-style: italic\"> """</span>\n",
|
|
"\n",
|
|
" <span style=\"color: #3D7B7B; font-style: italic\"># Total ops: 92</span>\n",
|
|
"\n",
|
|
" <span style=\"color: #3D7B7B; font-style: italic\"># Input arrays</span>\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">if</span> state<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">==</span> (<span style=\"color: #666666\">28</span>,):\n",
|
|
" state <span style=\"color: #666666\">=</span> state<span style=\"color: #666666\">.</span>reshape((<span style=\"color: #666666\">28</span>, <span style=\"color: #666666\">1</span>))\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">elif</span> state<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">!=</span> (<span style=\"color: #666666\">28</span>, <span style=\"color: #666666\">1</span>):\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">raise</span> <span style=\"color: #CB3F38; font-weight: bold\">IndexError</span>(\n",
|
|
" <span style=\"color: #BA2121\">"state is expected to have shape (28, 1) or (28,); instead had shape </span><span style=\"color: #A45A77; font-weight: bold\">{}</span><span style=\"color: #BA2121\">"</span><span style=\"color: #666666\">.</span>format(\n",
|
|
" state<span style=\"color: #666666\">.</span>shape\n",
|
|
" )\n",
|
|
" )\n",
|
|
"\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">if</span> u<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">==</span> (<span style=\"color: #666666\">6</span>,):\n",
|
|
" u <span style=\"color: #666666\">=</span> u<span style=\"color: #666666\">.</span>reshape((<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">1</span>))\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">elif</span> u<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">!=</span> (<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">1</span>):\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">raise</span> <span style=\"color: #CB3F38; font-weight: bold\">IndexError</span>(\n",
|
|
" <span style=\"color: #BA2121\">"u is expected to have shape (6, 1) or (6,); instead had shape </span><span style=\"color: #A45A77; font-weight: bold\">{}</span><span style=\"color: #BA2121\">"</span><span style=\"color: #666666\">.</span>format(u<span style=\"color: #666666\">.</span>shape)\n",
|
|
" )\n",
|
|
"\n",
|
|
" <span style=\"color: #3D7B7B; font-style: italic\"># Intermediate terms (16)</span>\n",
|
|
" _tmp0 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">12</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>])\n",
|
|
" _tmp1 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">10</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>])\n",
|
|
" _tmp2 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">11</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>])\n",
|
|
" _tmp3 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">14</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">4</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp4 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp5 <span style=\"color: #666666\">=</span> _tmp4 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp6 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp7 <span style=\"color: #666666\">=</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp8 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">15</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">5</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp9 <span style=\"color: #666666\">=</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp10 <span style=\"color: #666666\">=</span> _tmp4 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp11 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">13</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp12 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span>\n",
|
|
" _tmp13 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">-</span> <span style=\"color: #666666\">1</span>\n",
|
|
" _tmp14 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _tmp15 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span>\n",
|
|
"\n",
|
|
" <span style=\"color: #3D7B7B; font-style: italic\"># Output terms</span>\n",
|
|
" _res <span style=\"color: #666666\">=</span> numpy<span style=\"color: #666666\">.</span>zeros(<span style=\"color: #666666\">28</span>)\n",
|
|
" _res[<span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">=</span> _tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">1</span>] <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>_tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">2</span>] <span style=\"color: #666666\">=</span> _tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">3</span>] <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>_tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">4</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">7</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">4</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">5</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">8</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">5</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">6</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">9</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">7</span>] <span style=\"color: #666666\">=</span> (\n",
|
|
" _tmp11 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp12 <span style=\"color: #666666\">-</span> _tmp13)\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (_tmp5 <span style=\"color: #666666\">-</span> _tmp7)\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (_tmp10 <span style=\"color: #666666\">+</span> _tmp9)\n",
|
|
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">7</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" )\n",
|
|
" _res[<span style=\"color: #666666\">8</span>] <span style=\"color: #666666\">=</span> (\n",
|
|
" _tmp11 <span style=\"color: #666666\">*</span> (_tmp5 <span style=\"color: #666666\">+</span> _tmp7)\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp13 <span style=\"color: #666666\">-</span> _tmp15)\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp14 <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>])\n",
|
|
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">8</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" )\n",
|
|
" _res[<span style=\"color: #666666\">9</span>] <span style=\"color: #666666\">=</span> (\n",
|
|
" _tmp11 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp10 <span style=\"color: #666666\">+</span> _tmp9)\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (_tmp14 <span style=\"color: #666666\">+</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>])\n",
|
|
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp12 <span style=\"color: #666666\">-</span> _tmp15 <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1</span>)\n",
|
|
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">9</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">9.8</span>\n",
|
|
" )\n",
|
|
" _res[<span style=\"color: #666666\">10</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">10</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">11</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">11</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">12</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">12</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">13</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">13</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">14</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">14</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">15</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">15</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">16</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">16</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">17</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">17</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">18</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">18</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">19</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">19</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">20</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">20</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">21</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">21</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">22</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">22</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">23</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">23</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">24</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">24</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">25</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">25</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">26</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">26</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" _res[<span style=\"color: #666666\">27</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">27</span>, <span style=\"color: #666666\">0</span>]\n",
|
|
" <span style=\"color: #008000; font-weight: bold\">return</span> _res\n",
|
|
"</pre></div>\n"
|
|
],
|
|
"text/plain": [
|
|
"<IPython.core.display.HTML object>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
},
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"codegen.__init__():141 WARNING -- \n",
|
|
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
|
|
" or numerically unstable results from calling generated functions near singularities.\n",
|
|
"\n",
|
|
" In order to safely generate code, you should set epsilon to either a symbol\n",
|
|
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
|
|
" this before importing any other code from symforce, e.g. with\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_symbol()\n",
|
|
"\n",
|
|
" or\n",
|
|
"\n",
|
|
" import symforce\n",
|
|
" symforce.set_epsilon_to_number()\n",
|
|
"\n",
|
|
" For more information on use of epsilon to prevent singularities, take a look at the\n",
|
|
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
|
|
"\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"namespace = \"double_pendulum\"\n",
|
|
"double_pendulum_python = codegen.Codegen.function(\n",
|
|
" func = compute_mean,\n",
|
|
" config=codegen.PythonConfig(use_eigen_types=False),\n",
|
|
" name=\"double_pendulum\",\n",
|
|
" return_key=\"ddang\",\n",
|
|
")\n",
|
|
"double_pendulum_python_data = double_pendulum_python.generate_function(\n",
|
|
" namespace=namespace,\n",
|
|
")\n",
|
|
"\n",
|
|
"# print(\"Files generated in {}:\\n\".format(double_pendulum_python_data.output_dir))\n",
|
|
"# for f in double_pendulum_python_data.generated_files:\n",
|
|
"# print(\" |- {}\".format(f.relative_to(double_pendulum_python_data.output_dir)))\n",
|
|
"\n",
|
|
"display_code_file(\n",
|
|
" double_pendulum_python_data.function_dir / \"double_pendulum.py\",\n",
|
|
" \"python\",\n",
|
|
")\n",
|
|
"\n",
|
|
"codegen_with_jacobians = compute_mean_codegen.with_jacobians(\n",
|
|
" # Just compute wrt the pose and point, not epsilon\n",
|
|
" which_args=[\"state\"],\n",
|
|
" # Include value, not just jacobians\n",
|
|
" include_results=True,\n",
|
|
")"
|
|
]
|
|
},
|
|
{
|
|
"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.8.10"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|