{ "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": 81, "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": 93, "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": 94, "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": 98, "metadata": {}, "outputs": [], "source": [ "# Process Model\n", "f1 = q*sf.Quaternion(xyz=(omega-b_g)*dT, w = 0)\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": 99, "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": 105, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}0 & \\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) & 0 & \\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) & 0 & \\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) & 0 & 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": [ "⎡ 0 \\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", " 0 \\Delta t⋅(\\omega0 - b_g0) \n", " \n", " t⋅(\\omega0 - b_g0) 0 \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", " 0 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": 106, "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": 118, "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": 121, "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": 154, "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": 155, "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": 149, "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": 170, "metadata": {}, "outputs": [], "source": [ "P = sf.matrix_type_from_shape((28,28)).symbolic('P')" ] }, { "cell_type": "code", "execution_count": 178, "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": 234, "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 = 0)\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": 233, "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)\\\\\\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)\\\\\\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)\\\\- \\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)\\\\\\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) - \\\n", "⎢ \n", "⎢ \\Delta t⋅q_w⋅(\\omega1 - b_g1) - \\Delta t⋅qₓ⋅(\\omega2 - b_g2) + \\D\n", "⎢ \n", "⎢ \\Delta t⋅q_w⋅(\\omega2 - b_g2) + \\Delta t⋅qₓ⋅(\\omega1 - b_g1) - \\D\n", "⎢ \n", "⎢ -\\Delta t⋅qₓ⋅(\\omega0 - b_g0) - \\Delta t⋅q_y⋅(\\omega1 - b_g1) - \\\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", "Delta t⋅q_z⋅(\\omega1 - b_g1) ⎤\n", " ⎥\n", "elta t⋅q_z⋅(\\omega0 - b_g0) ⎥\n", " ⎥\n", "elta t⋅q_y⋅(\\omega0 - b_g0) ⎥\n", " ⎥\n", "Delta t⋅q_z⋅(\\omega2 - b_g2) ⎥\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 = predict_mean(state, u)\n", "f_pred" ] }, { "cell_type": "code", "execution_count": 217, "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": 222, "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": 224, "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": 228, "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": [ "
// -----------------------------------------------------------------------------\n",
       "// This file was autogenerated by symforce from template:\n",
       "//     function/FUNCTION.h.jinja\n",
       "// Do NOT modify by hand.\n",
       "// -----------------------------------------------------------------------------\n",
       "\n",
       "#pragma once\n",
       "\n",
       "#include <Eigen/Dense>\n",
       "\n",
       "namespace sym {\n",
       "\n",
       "/**\n",
       " * This function was autogenerated from a symbolic function. Do not modify by hand.\n",
       " *\n",
       " * Symbolic function: compute_measurement\n",
       " *\n",
       " * Args:\n",
       " *     state: Matrix28_1\n",
       " *     epsilon: Scalar\n",
       " *\n",
       " * Outputs:\n",
       " *     res: Matrix15_1\n",
       " *     res_D_state: (15x28) jacobian of res (15) wrt arg state (28)\n",
       " */\n",
       "template <typename Scalar>\n",
       "Eigen::Matrix<Scalar, 15, 1> ComputeMeasurementWithJacobian0(\n",
       "    const Eigen::Matrix<Scalar, 28, 1>& state, const Scalar epsilon,\n",
       "    Eigen::Matrix<Scalar, 15, 28>* const res_D_state = nullptr) {\n",
       "  // Total ops: 339\n",
       "\n",
       "  // Unused inputs\n",
       "  (void)epsilon;\n",
       "\n",
       "  // Input arrays\n",
       "\n",
       "  // Intermediate terms (99)\n",
       "  const Scalar _tmp0 = 2 * state(1, 0);\n",
       "  const Scalar _tmp1 = _tmp0 * state(0, 0);\n",
       "  const Scalar _tmp2 = 2 * state(2, 0);\n",
       "  const Scalar _tmp3 = _tmp2 * state(3, 0);\n",
       "  const Scalar _tmp4 = _tmp1 + _tmp3;\n",
       "  const Scalar _tmp5 = _tmp2 * state(0, 0);\n",
       "  const Scalar _tmp6 = _tmp0 * state(3, 0);\n",
       "  const Scalar _tmp7 = _tmp5 - _tmp6;\n",
       "  const Scalar _tmp8 = 2 * std::pow(state(1, 0), Scalar(2));\n",
       "  const Scalar _tmp9 = 2 * std::pow(state(2, 0), Scalar(2)) - 1;\n",
       "  const Scalar _tmp10 = _tmp8 + _tmp9;\n",
       "  const Scalar _tmp11 = -_tmp10;\n",
       "  const Scalar _tmp12 = _tmp1 - _tmp3;\n",
       "  const Scalar _tmp13 = 2 * state(3, 0);\n",
       "  const Scalar _tmp14 = _tmp13 * state(0, 0);\n",
       "  const Scalar _tmp15 = _tmp0 * state(2, 0);\n",
       "  const Scalar _tmp16 = _tmp14 + _tmp15;\n",
       "  const Scalar _tmp17 = 2 * std::pow(state(0, 0), Scalar(2));\n",
       "  const Scalar _tmp18 = _tmp17 + _tmp9;\n",
       "  const Scalar _tmp19 = -_tmp18;\n",
       "  const Scalar _tmp20 = _tmp5 + _tmp6;\n",
       "  const Scalar _tmp21 = _tmp14 - _tmp15;\n",
       "  const Scalar _tmp22 = -_tmp21;\n",
       "  const Scalar _tmp23 = _tmp17 + _tmp8 - 1;\n",
       "  const Scalar _tmp24 = -_tmp23;\n",
       "  const Scalar _tmp25 = -state(5, 0);\n",
       "  const Scalar _tmp26 = _tmp25 + state(17, 0);\n",
       "  const Scalar _tmp27 = -state(6, 0);\n",
       "  const Scalar _tmp28 = _tmp27 + state(18, 0);\n",
       "  const Scalar _tmp29 = -state(4, 0);\n",
       "  const Scalar _tmp30 = _tmp29 + state(16, 0);\n",
       "  const Scalar _tmp31 = _tmp25 + state(20, 0);\n",
       "  const Scalar _tmp32 = _tmp27 + state(21, 0);\n",
       "  const Scalar _tmp33 = _tmp29 + state(19, 0);\n",
       "  const Scalar _tmp34 = _tmp25 + state(23, 0);\n",
       "  const Scalar _tmp35 = _tmp27 + state(24, 0);\n",
       "  const Scalar _tmp36 = _tmp29 + state(22, 0);\n",
       "  const Scalar _tmp37 = _tmp25 + state(26, 0);\n",
       "  const Scalar _tmp38 = _tmp27 + state(27, 0);\n",
       "  const Scalar _tmp39 = _tmp29 + state(25, 0);\n",
       "  const Scalar _tmp40 = _tmp0 * state(8, 0);\n",
       "  const Scalar _tmp41 = _tmp2 * state(9, 0);\n",
       "  const Scalar _tmp42 = 4 * state(0, 0);\n",
       "  const Scalar _tmp43 = _tmp13 * state(9, 0);\n",
       "  const Scalar _tmp44 = -_tmp2 * state(7, 0);\n",
       "  const Scalar _tmp45 = _tmp13 * state(8, 0);\n",
       "  const Scalar _tmp46 = _tmp0 * _tmp26;\n",
       "  const Scalar _tmp47 = _tmp2 * _tmp28;\n",
       "  const Scalar _tmp48 = _tmp13 * _tmp28;\n",
       "  const Scalar _tmp49 = -_tmp2 * _tmp30;\n",
       "  const Scalar _tmp50 = _tmp13 * _tmp26;\n",
       "  const Scalar _tmp51 = _tmp0 * _tmp31;\n",
       "  const Scalar _tmp52 = _tmp2 * _tmp32;\n",
       "  const Scalar _tmp53 = _tmp13 * _tmp32;\n",
       "  const Scalar _tmp54 = -_tmp2 * _tmp33;\n",
       "  const Scalar _tmp55 = _tmp13 * _tmp31;\n",
       "  const Scalar _tmp56 = _tmp0 * _tmp34;\n",
       "  const Scalar _tmp57 = _tmp2 * _tmp35;\n",
       "  const Scalar _tmp58 = _tmp13 * _tmp35;\n",
       "  const Scalar _tmp59 = -_tmp2 * _tmp36;\n",
       "  const Scalar _tmp60 = _tmp13 * _tmp34;\n",
       "  const Scalar _tmp61 = _tmp0 * _tmp37;\n",
       "  const Scalar _tmp62 = _tmp2 * _tmp38;\n",
       "  const Scalar _tmp63 = _tmp13 * _tmp38;\n",
       "  const Scalar _tmp64 = -_tmp2 * _tmp39;\n",
       "  const Scalar _tmp65 = _tmp13 * _tmp37;\n",
       "  const Scalar _tmp66 = 2 * state(0, 0);\n",
       "  const Scalar _tmp67 = _tmp66 * state(8, 0);\n",
       "  const Scalar _tmp68 = 4 * state(1, 0);\n",
       "  const Scalar _tmp69 = _tmp66 * state(7, 0);\n",
       "  const Scalar _tmp70 = _tmp13 * state(7, 0);\n",
       "  const Scalar _tmp71 = _tmp26 * _tmp66;\n",
       "  const Scalar _tmp72 = _tmp30 * _tmp66;\n",
       "  const Scalar _tmp73 = _tmp13 * _tmp30;\n",
       "  const Scalar _tmp74 = _tmp31 * _tmp66;\n",
       "  const Scalar _tmp75 = _tmp33 * _tmp66;\n",
       "  const Scalar _tmp76 = _tmp13 * _tmp33;\n",
       "  const Scalar _tmp77 = _tmp34 * _tmp66;\n",
       "  const Scalar _tmp78 = _tmp36 * _tmp66;\n",
       "  const Scalar _tmp79 = _tmp13 * _tmp36;\n",
       "  const Scalar _tmp80 = _tmp37 * _tmp66;\n",
       "  const Scalar _tmp81 = _tmp39 * _tmp66;\n",
       "  const Scalar _tmp82 = _tmp13 * _tmp39;\n",
       "  const Scalar _tmp83 = _tmp66 * state(9, 0);\n",
       "  const Scalar _tmp84 = 4 * state(2, 0);\n",
       "  const Scalar _tmp85 = _tmp0 * state(9, 0);\n",
       "  const Scalar _tmp86 = _tmp28 * _tmp66;\n",
       "  const Scalar _tmp87 = _tmp0 * _tmp28;\n",
       "  const Scalar _tmp88 = _tmp32 * _tmp66;\n",
       "  const Scalar _tmp89 = _tmp0 * _tmp32;\n",
       "  const Scalar _tmp90 = _tmp35 * _tmp66;\n",
       "  const Scalar _tmp91 = _tmp0 * _tmp35;\n",
       "  const Scalar _tmp92 = _tmp38 * _tmp66;\n",
       "  const Scalar _tmp93 = _tmp0 * _tmp38;\n",
       "  const Scalar _tmp94 = -_tmp12;\n",
       "  const Scalar _tmp95 = -_tmp20;\n",
       "  const Scalar _tmp96 = -_tmp4;\n",
       "  const Scalar _tmp97 = -_tmp7;\n",
       "  const Scalar _tmp98 = -_tmp16;\n",
       "\n",
       "  // Output terms (2)\n",
       "  Eigen::Matrix<Scalar, 15, 1> _res;\n",
       "\n",
       "  _res(0, 0) = _tmp11 * state(7, 0) + _tmp4 * state(8, 0) + _tmp7 * state(9, 0);\n",
       "  _res(1, 0) = _tmp12 * state(7, 0) + _tmp16 * state(9, 0) + _tmp19 * state(8, 0);\n",
       "  _res(2, 0) = _tmp20 * state(7, 0) + _tmp22 * state(8, 0) + _tmp24 * state(9, 0);\n",
       "  _res(3, 0) = _tmp11 * _tmp30 + _tmp26 * _tmp4 + _tmp28 * _tmp7;\n",
       "  _res(4, 0) = _tmp12 * _tmp30 + _tmp16 * _tmp28 + _tmp19 * _tmp26;\n",
       "  _res(5, 0) = _tmp20 * _tmp30 + _tmp22 * _tmp26 + _tmp24 * _tmp28;\n",
       "  _res(6, 0) = _tmp11 * _tmp33 + _tmp31 * _tmp4 + _tmp32 * _tmp7;\n",
       "  _res(7, 0) = _tmp12 * _tmp33 + _tmp16 * _tmp32 + _tmp19 * _tmp31;\n",
       "  _res(8, 0) = _tmp20 * _tmp33 + _tmp22 * _tmp31 + _tmp24 * _tmp32;\n",
       "  _res(9, 0) = _tmp11 * _tmp36 + _tmp34 * _tmp4 + _tmp35 * _tmp7;\n",
       "  _res(10, 0) = _tmp12 * _tmp36 + _tmp16 * _tmp35 + _tmp19 * _tmp34;\n",
       "  _res(11, 0) = _tmp20 * _tmp36 + _tmp22 * _tmp34 + _tmp24 * _tmp35;\n",
       "  _res(12, 0) = _tmp11 * _tmp39 + _tmp37 * _tmp4 + _tmp38 * _tmp7;\n",
       "  _res(13, 0) = _tmp12 * _tmp39 + _tmp16 * _tmp38 + _tmp19 * _tmp37;\n",
       "  _res(14, 0) = _tmp20 * _tmp39 + _tmp22 * _tmp37 + _tmp24 * _tmp38;\n",
       "\n",
       "  if (res_D_state != nullptr) {\n",
       "    Eigen::Matrix<Scalar, 15, 28>& _res_D_state = (*res_D_state);\n",
       "\n",
       "    _res_D_state.setZero();\n",
       "\n",
       "    _res_D_state(0, 0) = _tmp40 + _tmp41;\n",
       "    _res_D_state(1, 0) = _tmp0 * state(7, 0) - _tmp42 * state(8, 0) + _tmp43;\n",
       "    _res_D_state(2, 0) = -_tmp42 * state(9, 0) - _tmp44 - _tmp45;\n",
       "    _res_D_state(3, 0) = _tmp46 + _tmp47;\n",
       "    _res_D_state(4, 0) = _tmp0 * _tmp30 - _tmp26 * _tmp42 + _tmp48;\n",
       "    _res_D_state(5, 0) = -_tmp28 * _tmp42 - _tmp49 - _tmp50;\n",
       "    _res_D_state(6, 0) = _tmp51 + _tmp52;\n",
       "    _res_D_state(7, 0) = _tmp0 * _tmp33 - _tmp31 * _tmp42 + _tmp53;\n",
       "    _res_D_state(8, 0) = -_tmp32 * _tmp42 - _tmp54 - _tmp55;\n",
       "    _res_D_state(9, 0) = _tmp56 + _tmp57;\n",
       "    _res_D_state(10, 0) = _tmp0 * _tmp36 - _tmp34 * _tmp42 + _tmp58;\n",
       "    _res_D_state(11, 0) = -_tmp35 * _tmp42 - _tmp59 - _tmp60;\n",
       "    _res_D_state(12, 0) = _tmp61 + _tmp62;\n",
       "    _res_D_state(13, 0) = _tmp0 * _tmp39 - _tmp37 * _tmp42 + _tmp63;\n",
       "    _res_D_state(14, 0) = -_tmp38 * _tmp42 - _tmp64 - _tmp65;\n",
       "    _res_D_state(0, 1) = -_tmp43 + _tmp67 - _tmp68 * state(7, 0);\n",
       "    _res_D_state(1, 1) = _tmp41 + _tmp69;\n",
       "    _res_D_state(2, 1) = _tmp2 * state(8, 0) - _tmp68 * state(9, 0) + _tmp70;\n",
       "    _res_D_state(3, 1) = -_tmp30 * _tmp68 - _tmp48 + _tmp71;\n",
       "    _res_D_state(4, 1) = _tmp47 + _tmp72;\n",
       "    _res_D_state(5, 1) = _tmp2 * _tmp26 - _tmp28 * _tmp68 + _tmp73;\n",
       "    _res_D_state(6, 1) = -_tmp33 * _tmp68 - _tmp53 + _tmp74;\n",
       "    _res_D_state(7, 1) = _tmp52 + _tmp75;\n",
       "    _res_D_state(8, 1) = _tmp2 * _tmp31 - _tmp32 * _tmp68 + _tmp76;\n",
       "    _res_D_state(9, 1) = -_tmp36 * _tmp68 - _tmp58 + _tmp77;\n",
       "    _res_D_state(10, 1) = _tmp57 + _tmp78;\n",
       "    _res_D_state(11, 1) = _tmp2 * _tmp34 - _tmp35 * _tmp68 + _tmp79;\n",
       "    _res_D_state(12, 1) = -_tmp39 * _tmp68 - _tmp63 + _tmp80;\n",
       "    _res_D_state(13, 1) = _tmp62 + _tmp81;\n",
       "    _res_D_state(14, 1) = _tmp2 * _tmp37 - _tmp38 * _tmp68 + _tmp82;\n",
       "    _res_D_state(0, 2) = _tmp45 + _tmp83 - _tmp84 * state(7, 0);\n",
       "    _res_D_state(1, 2) = -_tmp70 - _tmp84 * state(8, 0) + _tmp85;\n",
       "    _res_D_state(2, 2) = _tmp40 + _tmp69;\n",
       "    _res_D_state(3, 2) = -_tmp30 * _tmp84 + _tmp50 + _tmp86;\n",
       "    _res_D_state(4, 2) = -_tmp26 * _tmp84 - _tmp73 + _tmp87;\n",
       "    _res_D_state(5, 2) = _tmp46 + _tmp72;\n",
       "    _res_D_state(6, 2) = -_tmp33 * _tmp84 + _tmp55 + _tmp88;\n",
       "    _res_D_state(7, 2) = -_tmp31 * _tmp84 - _tmp76 + _tmp89;\n",
       "    _res_D_state(8, 2) = _tmp51 + _tmp75;\n",
       "    _res_D_state(9, 2) = -_tmp36 * _tmp84 + _tmp60 + _tmp90;\n",
       "    _res_D_state(10, 2) = -_tmp34 * _tmp84 - _tmp79 + _tmp91;\n",
       "    _res_D_state(11, 2) = _tmp56 + _tmp78;\n",
       "    _res_D_state(12, 2) = -_tmp39 * _tmp84 + _tmp65 + _tmp92;\n",
       "    _res_D_state(13, 2) = -_tmp37 * _tmp84 - _tmp82 + _tmp93;\n",
       "    _res_D_state(14, 2) = _tmp61 + _tmp81;\n",
       "    _res_D_state(0, 3) = -_tmp85 + 2 * state(2, 0) * state(8, 0);\n",
       "    _res_D_state(1, 3) = _tmp44 + _tmp83;\n",
       "    _res_D_state(2, 3) = -_tmp67 + 2 * state(1, 0) * state(7, 0);\n",
       "    _res_D_state(3, 3) = 2 * _tmp26 * state(2, 0) - _tmp87;\n",
       "    _res_D_state(4, 3) = _tmp49 + _tmp86;\n",
       "    _res_D_state(5, 3) = 2 * _tmp30 * state(1, 0) - _tmp71;\n",
       "    _res_D_state(6, 3) = 2 * _tmp31 * state(2, 0) - _tmp89;\n",
       "    _res_D_state(7, 3) = _tmp54 + _tmp88;\n",
       "    _res_D_state(8, 3) = 2 * _tmp33 * state(1, 0) - _tmp74;\n",
       "    _res_D_state(9, 3) = 2 * _tmp34 * state(2, 0) - _tmp91;\n",
       "    _res_D_state(10, 3) = _tmp59 + _tmp90;\n",
       "    _res_D_state(11, 3) = 2 * _tmp36 * state(1, 0) - _tmp77;\n",
       "    _res_D_state(12, 3) = 2 * _tmp37 * state(2, 0) - _tmp93;\n",
       "    _res_D_state(13, 3) = _tmp64 + _tmp92;\n",
       "    _res_D_state(14, 3) = 2 * _tmp39 * state(1, 0) - _tmp80;\n",
       "    _res_D_state(3, 4) = _tmp10;\n",
       "    _res_D_state(4, 4) = _tmp94;\n",
       "    _res_D_state(5, 4) = _tmp95;\n",
       "    _res_D_state(6, 4) = _tmp10;\n",
       "    _res_D_state(7, 4) = _tmp94;\n",
       "    _res_D_state(8, 4) = _tmp95;\n",
       "    _res_D_state(9, 4) = _tmp10;\n",
       "    _res_D_state(10, 4) = _tmp94;\n",
       "    _res_D_state(11, 4) = _tmp95;\n",
       "    _res_D_state(12, 4) = _tmp10;\n",
       "    _res_D_state(13, 4) = _tmp94;\n",
       "    _res_D_state(14, 4) = _tmp95;\n",
       "    _res_D_state(3, 5) = _tmp96;\n",
       "    _res_D_state(4, 5) = _tmp18;\n",
       "    _res_D_state(5, 5) = _tmp21;\n",
       "    _res_D_state(6, 5) = _tmp96;\n",
       "    _res_D_state(7, 5) = _tmp18;\n",
       "    _res_D_state(8, 5) = _tmp21;\n",
       "    _res_D_state(9, 5) = _tmp96;\n",
       "    _res_D_state(10, 5) = _tmp18;\n",
       "    _res_D_state(11, 5) = _tmp21;\n",
       "    _res_D_state(12, 5) = _tmp96;\n",
       "    _res_D_state(13, 5) = _tmp18;\n",
       "    _res_D_state(14, 5) = _tmp21;\n",
       "    _res_D_state(3, 6) = _tmp97;\n",
       "    _res_D_state(4, 6) = _tmp98;\n",
       "    _res_D_state(5, 6) = _tmp23;\n",
       "    _res_D_state(6, 6) = _tmp97;\n",
       "    _res_D_state(7, 6) = _tmp98;\n",
       "    _res_D_state(8, 6) = _tmp23;\n",
       "    _res_D_state(9, 6) = _tmp97;\n",
       "    _res_D_state(10, 6) = _tmp98;\n",
       "    _res_D_state(11, 6) = _tmp23;\n",
       "    _res_D_state(12, 6) = _tmp97;\n",
       "    _res_D_state(13, 6) = _tmp98;\n",
       "    _res_D_state(14, 6) = _tmp23;\n",
       "    _res_D_state(0, 7) = _tmp11;\n",
       "    _res_D_state(1, 7) = _tmp12;\n",
       "    _res_D_state(2, 7) = _tmp20;\n",
       "    _res_D_state(0, 8) = _tmp4;\n",
       "    _res_D_state(1, 8) = _tmp19;\n",
       "    _res_D_state(2, 8) = _tmp22;\n",
       "    _res_D_state(0, 9) = _tmp7;\n",
       "    _res_D_state(1, 9) = _tmp16;\n",
       "    _res_D_state(2, 9) = _tmp24;\n",
       "    _res_D_state(3, 16) = _tmp11;\n",
       "    _res_D_state(4, 16) = _tmp12;\n",
       "    _res_D_state(5, 16) = _tmp20;\n",
       "    _res_D_state(3, 17) = _tmp4;\n",
       "    _res_D_state(4, 17) = _tmp19;\n",
       "    _res_D_state(5, 17) = _tmp22;\n",
       "    _res_D_state(3, 18) = _tmp7;\n",
       "    _res_D_state(4, 18) = _tmp16;\n",
       "    _res_D_state(5, 18) = _tmp24;\n",
       "    _res_D_state(6, 19) = _tmp11;\n",
       "    _res_D_state(7, 19) = _tmp12;\n",
       "    _res_D_state(8, 19) = _tmp20;\n",
       "    _res_D_state(6, 20) = _tmp4;\n",
       "    _res_D_state(7, 20) = _tmp19;\n",
       "    _res_D_state(8, 20) = _tmp22;\n",
       "    _res_D_state(6, 21) = _tmp7;\n",
       "    _res_D_state(7, 21) = _tmp16;\n",
       "    _res_D_state(8, 21) = _tmp24;\n",
       "    _res_D_state(9, 22) = _tmp11;\n",
       "    _res_D_state(10, 22) = _tmp12;\n",
       "    _res_D_state(11, 22) = _tmp20;\n",
       "    _res_D_state(9, 23) = _tmp4;\n",
       "    _res_D_state(10, 23) = _tmp19;\n",
       "    _res_D_state(11, 23) = _tmp22;\n",
       "    _res_D_state(9, 24) = _tmp7;\n",
       "    _res_D_state(10, 24) = _tmp16;\n",
       "    _res_D_state(11, 24) = _tmp24;\n",
       "    _res_D_state(12, 25) = _tmp11;\n",
       "    _res_D_state(13, 25) = _tmp12;\n",
       "    _res_D_state(14, 25) = _tmp20;\n",
       "    _res_D_state(12, 26) = _tmp4;\n",
       "    _res_D_state(13, 26) = _tmp19;\n",
       "    _res_D_state(14, 26) = _tmp22;\n",
       "    _res_D_state(12, 27) = _tmp7;\n",
       "    _res_D_state(13, 27) = _tmp16;\n",
       "    _res_D_state(14, 27) = _tmp24;\n",
       "  }\n",
       "\n",
       "  return _res;\n",
       "}  // NOLINT(readability/fn_size)\n",
       "\n",
       "// NOLINTNEXTLINE(readability/fn_size)\n",
       "}  // namespace sym\n",
       "
\n" ], "text/plain": [ "" ] }, "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": 235, "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" ] }, { "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": [ "
// -----------------------------------------------------------------------------\n",
       "// This file was autogenerated by symforce from template:\n",
       "//     function/FUNCTION.h.jinja\n",
       "// Do NOT modify by hand.\n",
       "// -----------------------------------------------------------------------------\n",
       "\n",
       "#pragma once\n",
       "\n",
       "#include <Eigen/Dense>\n",
       "\n",
       "namespace sym {\n",
       "\n",
       "/**\n",
       " * This function was autogenerated from a symbolic function. Do not modify by hand.\n",
       " *\n",
       " * Symbolic function: compute_mean\n",
       " *\n",
       " * Args:\n",
       " *     state: Matrix28_1\n",
       " *     u: Matrix61\n",
       " *     dT: Scalar\n",
       " *     epsilon: Scalar\n",
       " *\n",
       " * Outputs:\n",
       " *     res: Matrix28_1\n",
       " *     res_D_state: (28x28) jacobian of res (28) wrt arg state (28)\n",
       " */\n",
       "template <typename Scalar>\n",
       "Eigen::Matrix<Scalar, 28, 1> ComputeMeanWithJacobian0(\n",
       "    const Eigen::Matrix<Scalar, 28, 1>& state, const Eigen::Matrix<Scalar, 6, 1>& u,\n",
       "    const Scalar dT, const Scalar epsilon,\n",
       "    Eigen::Matrix<Scalar, 28, 28>* const res_D_state = nullptr) {\n",
       "  // Total ops: 143\n",
       "\n",
       "  // Unused inputs\n",
       "  (void)epsilon;\n",
       "\n",
       "  // Input arrays\n",
       "\n",
       "  // Intermediate terms (49)\n",
       "  const Scalar _tmp0 = dT * (-state(12, 0) + u(2, 0));\n",
       "  const Scalar _tmp1 = dT * (-state(10, 0) + u(0, 0));\n",
       "  const Scalar _tmp2 = dT * (-state(11, 0) + u(1, 0));\n",
       "  const Scalar _tmp3 = -state(14, 0) + u(4, 0);\n",
       "  const Scalar _tmp4 = 2 * state(1, 0);\n",
       "  const Scalar _tmp5 = _tmp4 * state(0, 0);\n",
       "  const Scalar _tmp6 = 2 * state(2, 0);\n",
       "  const Scalar _tmp7 = _tmp6 * state(3, 0);\n",
       "  const Scalar _tmp8 = _tmp5 - _tmp7;\n",
       "  const Scalar _tmp9 = -state(15, 0) + u(5, 0);\n",
       "  const Scalar _tmp10 = _tmp6 * state(0, 0);\n",
       "  const Scalar _tmp11 = _tmp4 * state(3, 0);\n",
       "  const Scalar _tmp12 = _tmp10 + _tmp11;\n",
       "  const Scalar _tmp13 = -state(13, 0) + u(3, 0);\n",
       "  const Scalar _tmp14 = 2 * std::pow(state(1, 0), Scalar(2));\n",
       "  const Scalar _tmp15 = 2 * std::pow(state(2, 0), Scalar(2)) - 1;\n",
       "  const Scalar _tmp16 = _tmp14 + _tmp15;\n",
       "  const Scalar _tmp17 = _tmp5 + _tmp7;\n",
       "  const Scalar _tmp18 = 2 * state(3, 0);\n",
       "  const Scalar _tmp19 = _tmp18 * state(0, 0);\n",
       "  const Scalar _tmp20 = _tmp6 * state(1, 0);\n",
       "  const Scalar _tmp21 = _tmp19 - _tmp20;\n",
       "  const Scalar _tmp22 = 2 * std::pow(state(0, 0), Scalar(2));\n",
       "  const Scalar _tmp23 = _tmp15 + _tmp22;\n",
       "  const Scalar _tmp24 = _tmp10 - _tmp11;\n",
       "  const Scalar _tmp25 = _tmp19 + _tmp20;\n",
       "  const Scalar _tmp26 = _tmp14 + _tmp22 - 1;\n",
       "  const Scalar _tmp27 = -_tmp0;\n",
       "  const Scalar _tmp28 = -_tmp1;\n",
       "  const Scalar _tmp29 = _tmp3 * _tmp4;\n",
       "  const Scalar _tmp30 = _tmp6 * _tmp9;\n",
       "  const Scalar _tmp31 = 4 * state(0, 0);\n",
       "  const Scalar _tmp32 = -_tmp13 * _tmp4;\n",
       "  const Scalar _tmp33 = _tmp18 * _tmp9;\n",
       "  const Scalar _tmp34 = _tmp18 * _tmp3;\n",
       "  const Scalar _tmp35 = -_tmp2;\n",
       "  const Scalar _tmp36 = 2 * state(0, 0);\n",
       "  const Scalar _tmp37 = _tmp3 * _tmp36;\n",
       "  const Scalar _tmp38 = 4 * state(1, 0);\n",
       "  const Scalar _tmp39 = _tmp13 * _tmp36;\n",
       "  const Scalar _tmp40 = -_tmp3 * _tmp6;\n",
       "  const Scalar _tmp41 = _tmp13 * _tmp18;\n",
       "  const Scalar _tmp42 = _tmp36 * _tmp9;\n",
       "  const Scalar _tmp43 = 4 * state(2, 0);\n",
       "  const Scalar _tmp44 = _tmp4 * _tmp9;\n",
       "  const Scalar _tmp45 = -dT * state(3, 0);\n",
       "  const Scalar _tmp46 = dT * state(2, 0);\n",
       "  const Scalar _tmp47 = dT * state(1, 0);\n",
       "  const Scalar _tmp48 = dT * state(0, 0);\n",
       "\n",
       "  // Output terms (2)\n",
       "  Eigen::Matrix<Scalar, 28, 1> _res;\n",
       "\n",
       "  _res(0, 0) = _tmp0 * state(1, 0) + _tmp1 * state(3, 0) - _tmp2 * state(2, 0);\n",
       "  _res(1, 0) = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0) + _tmp2 * state(3, 0);\n",
       "  _res(2, 0) = _tmp0 * state(3, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0);\n",
       "  _res(3, 0) = -_tmp0 * state(2, 0) - _tmp1 * state(0, 0) - _tmp2 * state(1, 0);\n",
       "  _res(4, 0) = dT * state(7, 0) + state(4, 0);\n",
       "  _res(5, 0) = dT * state(8, 0) + state(5, 0);\n",
       "  _res(6, 0) = dT * state(9, 0) + state(6, 0);\n",
       "  _res(7, 0) = _tmp12 * _tmp9 - _tmp13 * _tmp16 + _tmp3 * _tmp8 + state(7, 0);\n",
       "  _res(8, 0) = _tmp13 * _tmp17 - _tmp21 * _tmp9 - _tmp23 * _tmp3 + state(8, 0);\n",
       "  _res(9, 0) =\n",
       "      _tmp13 * _tmp24 + _tmp25 * _tmp3 - _tmp26 * _tmp9 + state(9, 0) + Scalar(9.8000000000000007);\n",
       "  _res(10, 0) = state(10, 0);\n",
       "  _res(11, 0) = state(11, 0);\n",
       "  _res(12, 0) = state(12, 0);\n",
       "  _res(13, 0) = state(13, 0);\n",
       "  _res(14, 0) = state(14, 0);\n",
       "  _res(15, 0) = state(15, 0);\n",
       "  _res(16, 0) = state(16, 0);\n",
       "  _res(17, 0) = state(17, 0);\n",
       "  _res(18, 0) = state(18, 0);\n",
       "  _res(19, 0) = state(19, 0);\n",
       "  _res(20, 0) = state(20, 0);\n",
       "  _res(21, 0) = state(21, 0);\n",
       "  _res(22, 0) = state(22, 0);\n",
       "  _res(23, 0) = state(23, 0);\n",
       "  _res(24, 0) = state(24, 0);\n",
       "  _res(25, 0) = state(25, 0);\n",
       "  _res(26, 0) = state(26, 0);\n",
       "  _res(27, 0) = state(27, 0);\n",
       "\n",
       "  if (res_D_state != nullptr) {\n",
       "    Eigen::Matrix<Scalar, 28, 28>& _res_D_state = (*res_D_state);\n",
       "\n",
       "    _res_D_state.setZero();\n",
       "\n",
       "    _res_D_state(1, 0) = _tmp27;\n",
       "    _res_D_state(2, 0) = _tmp2;\n",
       "    _res_D_state(3, 0) = _tmp28;\n",
       "    _res_D_state(7, 0) = _tmp29 + _tmp30;\n",
       "    _res_D_state(8, 0) = -_tmp3 * _tmp31 - _tmp32 - _tmp33;\n",
       "    _res_D_state(9, 0) = _tmp13 * _tmp6 - _tmp31 * _tmp9 + _tmp34;\n",
       "    _res_D_state(0, 1) = _tmp0;\n",
       "    _res_D_state(2, 1) = _tmp28;\n",
       "    _res_D_state(3, 1) = _tmp35;\n",
       "    _res_D_state(7, 1) = -_tmp13 * _tmp38 + _tmp33 + _tmp37;\n",
       "    _res_D_state(8, 1) = _tmp30 + _tmp39;\n",
       "    _res_D_state(9, 1) = -_tmp38 * _tmp9 - _tmp40 - _tmp41;\n",
       "    _res_D_state(0, 2) = _tmp35;\n",
       "    _res_D_state(1, 2) = _tmp1;\n",
       "    _res_D_state(3, 2) = _tmp27;\n",
       "    _res_D_state(7, 2) = -_tmp13 * _tmp43 - _tmp34 + _tmp42;\n",
       "    _res_D_state(8, 2) = -_tmp3 * _tmp43 + _tmp41 + _tmp44;\n",
       "    _res_D_state(9, 2) = _tmp29 + _tmp39;\n",
       "    _res_D_state(0, 3) = _tmp1;\n",
       "    _res_D_state(1, 3) = _tmp2;\n",
       "    _res_D_state(2, 3) = _tmp0;\n",
       "    _res_D_state(7, 3) = _tmp40 + _tmp44;\n",
       "    _res_D_state(8, 3) = 2 * _tmp13 * state(2, 0) - _tmp42;\n",
       "    _res_D_state(9, 3) = _tmp32 + _tmp37;\n",
       "    _res_D_state(4, 4) = 1;\n",
       "    _res_D_state(5, 5) = 1;\n",
       "    _res_D_state(6, 6) = 1;\n",
       "    _res_D_state(4, 7) = dT;\n",
       "    _res_D_state(7, 7) = 1;\n",
       "    _res_D_state(5, 8) = dT;\n",
       "    _res_D_state(8, 8) = 1;\n",
       "    _res_D_state(6, 9) = dT;\n",
       "    _res_D_state(9, 9) = 1;\n",
       "    _res_D_state(0, 10) = _tmp45;\n",
       "    _res_D_state(1, 10) = -_tmp46;\n",
       "    _res_D_state(2, 10) = _tmp47;\n",
       "    _res_D_state(3, 10) = _tmp48;\n",
       "    _res_D_state(10, 10) = 1;\n",
       "    _res_D_state(0, 11) = _tmp46;\n",
       "    _res_D_state(1, 11) = _tmp45;\n",
       "    _res_D_state(2, 11) = -_tmp48;\n",
       "    _res_D_state(3, 11) = _tmp47;\n",
       "    _res_D_state(11, 11) = 1;\n",
       "    _res_D_state(0, 12) = -_tmp47;\n",
       "    _res_D_state(1, 12) = _tmp48;\n",
       "    _res_D_state(2, 12) = _tmp45;\n",
       "    _res_D_state(3, 12) = _tmp46;\n",
       "    _res_D_state(12, 12) = 1;\n",
       "    _res_D_state(7, 13) = _tmp16;\n",
       "    _res_D_state(8, 13) = -_tmp17;\n",
       "    _res_D_state(9, 13) = -_tmp24;\n",
       "    _res_D_state(13, 13) = 1;\n",
       "    _res_D_state(7, 14) = -_tmp8;\n",
       "    _res_D_state(8, 14) = _tmp23;\n",
       "    _res_D_state(9, 14) = -_tmp25;\n",
       "    _res_D_state(14, 14) = 1;\n",
       "    _res_D_state(7, 15) = -_tmp12;\n",
       "    _res_D_state(8, 15) = _tmp21;\n",
       "    _res_D_state(9, 15) = _tmp26;\n",
       "    _res_D_state(15, 15) = 1;\n",
       "    _res_D_state(16, 16) = 1;\n",
       "    _res_D_state(17, 17) = 1;\n",
       "    _res_D_state(18, 18) = 1;\n",
       "    _res_D_state(19, 19) = 1;\n",
       "    _res_D_state(20, 20) = 1;\n",
       "    _res_D_state(21, 21) = 1;\n",
       "    _res_D_state(22, 22) = 1;\n",
       "    _res_D_state(23, 23) = 1;\n",
       "    _res_D_state(24, 24) = 1;\n",
       "    _res_D_state(25, 25) = 1;\n",
       "    _res_D_state(26, 26) = 1;\n",
       "    _res_D_state(27, 27) = 1;\n",
       "  }\n",
       "\n",
       "  return _res;\n",
       "}  // NOLINT(readability/fn_size)\n",
       "\n",
       "// NOLINTNEXTLINE(readability/fn_size)\n",
       "}  // namespace sym\n",
       "
\n" ], "text/plain": [ "" ] }, "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" ] } ], "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 }