diff --git a/examples/legged_inertial_ekf.ipynb b/examples/legged_inertial_ekf.ipynb index f5ed519..80ceafd 100644 --- a/examples/legged_inertial_ekf.ipynb +++ b/examples/legged_inertial_ekf.ipynb @@ -23,7 +23,7 @@ }, { "cell_type": "code", - "execution_count": 81, + "execution_count": 2, "metadata": {}, "outputs": [], "source": [ @@ -50,7 +50,7 @@ }, { "cell_type": "code", - "execution_count": 93, + "execution_count": 3, "metadata": {}, "outputs": [ { @@ -135,7 +135,7 @@ }, { "cell_type": "code", - "execution_count": 94, + "execution_count": 4, "metadata": {}, "outputs": [ { @@ -168,12 +168,13 @@ }, { "cell_type": "code", - "execution_count": 98, + "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "# Process Model\n", - "f1 = q*sf.Quaternion(xyz=(omega-b_g)*dT, w = 0)\n", + "f1 = q*sf.Quaternion(xyz=(gyro-b_g)*dT, w = 1)\n", + "# f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n", "f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n", "f2 = p+dT*v\n", "f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n", @@ -187,7 +188,7 @@ }, { "cell_type": "code", - "execution_count": 99, + "execution_count": 6, "metadata": {}, "outputs": [], "source": [ @@ -204,16 +205,16 @@ }, { "cell_type": "code", - "execution_count": 105, + "execution_count": 7, "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]$" + "$\\displaystyle \\left[\\begin{array}{cccccccccccccccccccccccccccc}1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & \\Delta t \\left(\\omega0 - b_{g0}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{w} & \\Delta t q_{z} & - \\Delta t q_{y} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & \\Delta t \\left(\\omega0 - b_{g0}\\right) & \\Delta t \\left(\\omega1 - b_{g1}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & - \\Delta t q_{z} & - \\Delta t q_{w} & \\Delta t q_{x} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\\\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega0 - b_{g0}\\right) & 1 & \\Delta t \\left(\\omega2 - b_{g2}\\right) & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{y} & - \\Delta t q_{x} & - \\Delta t q_{w} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- \\Delta t \\left(\\omega0 - b_{g0}\\right) & - \\Delta t \\left(\\omega1 - b_{g1}\\right) & - \\Delta t \\left(\\omega2 - b_{g2}\\right) & 1 & 0 & 0 & 0 & 0 & 0 & 0 & \\Delta t q_{x} & \\Delta t q_{y} & \\Delta t q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \\Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{y} \\left(a_{1} - b_{a1}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{2} - b_{a2}\\right) + 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 4 q_{y} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{1} - b_{a1}\\right) + 2 q_{x} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{0} - b_{a0}\\right) & 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 2 q_{y}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{z} - 2 q_{x} q_{y} & - 2 q_{w} q_{y} - 2 q_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\- 2 q_{w} \\left(a_{2} - b_{a2}\\right) - 4 q_{x} \\left(a_{1} - b_{a1}\\right) + 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{z} \\left(a_{2} - b_{a2}\\right) & 2 q_{w} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{2} - b_{a2}\\right) - 4 q_{z} \\left(a_{1} - b_{a1}\\right) & - 2 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & - 2 q_{w} q_{z} - 2 q_{x} q_{y} & 2 q_{x}^{2} + 2 q_{z}^{2} - 1 & 2 q_{w} q_{x} - 2 q_{y} q_{z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\2 q_{w} \\left(a_{1} - b_{a1}\\right) - 4 q_{x} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{0} - b_{a0}\\right) & - 2 q_{w} \\left(a_{0} - b_{a0}\\right) - 4 q_{y} \\left(a_{2} - b_{a2}\\right) + 2 q_{z} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{0} - b_{a0}\\right) + 2 q_{y} \\left(a_{1} - b_{a1}\\right) & 2 q_{x} \\left(a_{1} - b_{a1}\\right) - 2 q_{y} \\left(a_{0} - b_{a0}\\right) & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 2 q_{w} q_{y} - 2 q_{x} q_{z} & - 2 q_{w} q_{x} - 2 q_{y} q_{z} & 2 q_{x}^{2} + 2 q_{y}^{2} - 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{array}\\right]$" ], "text/plain": [ - "⎡ 0 \\Delta\n", + "⎡ 1 \\Delta\n", "⎢ \n", "⎢ -\\Delta t⋅(\\omega2 - b_g2) \n", "⎢ \n", @@ -274,9 +275,9 @@ "\n", " t⋅(\\omega2 - b_g2) -\\Delta t⋅(\\omega1 - b_g1) \n", " \n", - " 0 \\Delta t⋅(\\omega0 - b_g0) \n", + " 1 \\Delta t⋅(\\omega0 - b_g0) \n", " \n", - " t⋅(\\omega0 - b_g0) 0 \n", + " t⋅(\\omega0 - b_g0) 1 \n", " \n", " t⋅(\\omega1 - b_g1) -\\Delta t⋅(\\omega2 - b_g2) \n", " \n", @@ -337,7 +338,7 @@ " \n", " \\Delta t⋅(\\omega2 - b_g2) 0 0 0 0 0 \n", " \n", - " 0 0 0 0 0 0 \n", + " 1 0 0 0 0 0 \n", " \n", " 0 1 0 0 \\Delta t 0 \n", " \n", @@ -579,7 +580,7 @@ }, { "cell_type": "code", - "execution_count": 106, + "execution_count": 8, "metadata": {}, "outputs": [ { @@ -718,7 +719,7 @@ }, { "cell_type": "code", - "execution_count": 118, + "execution_count": 9, "metadata": {}, "outputs": [ { @@ -840,7 +841,7 @@ }, { "cell_type": "code", - "execution_count": 121, + "execution_count": 10, "metadata": {}, "outputs": [ { @@ -1266,7 +1267,7 @@ }, { "cell_type": "code", - "execution_count": 154, + "execution_count": 11, "metadata": {}, "outputs": [ { @@ -1357,7 +1358,7 @@ }, { "cell_type": "code", - "execution_count": 155, + "execution_count": 12, "metadata": {}, "outputs": [ { @@ -1565,7 +1566,7 @@ }, { "cell_type": "code", - "execution_count": 149, + "execution_count": 13, "metadata": {}, "outputs": [ { @@ -1641,7 +1642,7 @@ }, { "cell_type": "code", - "execution_count": 170, + "execution_count": 14, "metadata": {}, "outputs": [], "source": [ @@ -1650,7 +1651,7 @@ }, { "cell_type": "code", - "execution_count": 178, + "execution_count": 15, "metadata": {}, "outputs": [ { @@ -1742,7 +1743,7 @@ }, { "cell_type": "code", - "execution_count": 234, + "execution_count": 16, "metadata": {}, "outputs": [], "source": [ @@ -1758,7 +1759,8 @@ " 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 = q*sf.Quaternion(xyz=(omega-b_g)*dT, w = 1.)\n", + " # f1 = q*sf.Rot3.from_tangent((gyro-b_g)*dT, epsilon=epsilon).q\n", " f1 = sf.Matrix([f1.x, f1.y, f1.z, f1.w])\n", " f2 = p+dT*v\n", " f3 = v+sf.Rot3(q).to_rotation_matrix()*(accel-b_a)-gravity\n", @@ -1782,22 +1784,22 @@ }, { "cell_type": "code", - "execution_count": 233, + "execution_count": 17, "metadata": {}, "outputs": [ { "data": { "text/latex": [ - "$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} \\left(\\omega0 - b_{g0}\\right) + \\Delta t q_{y} \\left(\\omega2 - b_{g2}\\right) - \\Delta t q_{z} \\left(\\omega1 - b_{g1}\\right)\\\\\\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]$" + "$\\displaystyle \\left[\\begin{matrix}\\Delta t q_{w} \\left(\\omega0 - b_{g0}\\right) + \\Delta t q_{y} \\left(\\omega2 - b_{g2}\\right) - \\Delta t q_{z} \\left(\\omega1 - b_{g1}\\right) + 1.0 q_{x}\\\\\\Delta t q_{w} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{x} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{z} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{y}\\\\\\Delta t q_{w} \\left(\\omega2 - b_{g2}\\right) + \\Delta t q_{x} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{y} \\left(\\omega0 - b_{g0}\\right) + 1.0 q_{z}\\\\- \\Delta t q_{x} \\left(\\omega0 - b_{g0}\\right) - \\Delta t q_{y} \\left(\\omega1 - b_{g1}\\right) - \\Delta t q_{z} \\left(\\omega2 - b_{g2}\\right) + 1.0 q_{w}\\\\\\Delta t v_{0} + p_{0}\\\\\\Delta t v_{1} + p_{1}\\\\\\Delta t v_{2} + p_{2}\\\\v_{0} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{y}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(2 q_{w} q_{y} + 2 q_{x} q_{z}\\right)\\\\v_{1} + \\left(a_{0} - b_{a0}\\right) \\left(2 q_{w} q_{z} + 2 q_{x} q_{y}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(- 2 q_{x}^{2} - 2 q_{z}^{2} + 1\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\\right)\\\\v_{2} + \\left(a_{0} - b_{a0}\\right) \\left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\\right) + \\left(a_{1} - b_{a1}\\right) \\left(2 q_{w} q_{x} + 2 q_{y} q_{z}\\right) + \\left(a_{2} - b_{a2}\\right) \\left(- 2 q_{x}^{2} - 2 q_{y}^{2} + 1\\right) + 9.8\\\\b_{g0}\\\\b_{g1}\\\\b_{g2}\\\\b_{a0}\\\\b_{a1}\\\\b_{a2}\\\\s_{FR}0\\\\s_{FR}1\\\\s_{FR}2\\\\s_{FL}0\\\\s_{FL}1\\\\s_{FL}2\\\\s_{RR}0\\\\s_{RR}1\\\\s_{RR}2\\\\s_{RL}0\\\\s_{RL}1\\\\s_{RL}2\\end{matrix}\\right]$" ], "text/plain": [ - "⎡ \\Delta t⋅q_w⋅(\\omega0 - b_g0) + \\Delta t⋅q_y⋅(\\omega2 - b_g2) - \\\n", + "⎡ \\Delta t⋅q_w⋅(\\omega0 - b_g0) + \\Delta t⋅q_y⋅(\\omega2 - b_g2) - \\Delta\n", "⎢ \n", - "⎢ \\Delta t⋅q_w⋅(\\omega1 - b_g1) - \\Delta t⋅qₓ⋅(\\omega2 - b_g2) + \\D\n", + "⎢ \\Delta t⋅q_w⋅(\\omega1 - b_g1) - \\Delta t⋅qₓ⋅(\\omega2 - b_g2) + \\Delta \n", "⎢ \n", - "⎢ \\Delta t⋅q_w⋅(\\omega2 - b_g2) + \\Delta t⋅qₓ⋅(\\omega1 - b_g1) - \\D\n", + "⎢ \\Delta t⋅q_w⋅(\\omega2 - b_g2) + \\Delta t⋅qₓ⋅(\\omega1 - b_g1) - \\Delta \n", "⎢ \n", - "⎢ -\\Delta t⋅qₓ⋅(\\omega0 - b_g0) - \\Delta t⋅q_y⋅(\\omega1 - b_g1) - \\\n", + "⎢ -\\Delta t⋅qₓ⋅(\\omega0 - b_g0) - \\Delta t⋅q_y⋅(\\omega1 - b_g1) - \\Delta\n", "⎢ \n", "⎢ \\Delta t⋅v₀ + p₀ \n", "⎢ \n", @@ -1850,13 +1852,13 @@ "⎢ \n", "⎣ s_{RL}2 \n", "\n", - "Delta t⋅q_z⋅(\\omega1 - b_g1) ⎤\n", + " t⋅q_z⋅(\\omega1 - b_g1) + 1.0⋅qₓ ⎤\n", " ⎥\n", - "elta t⋅q_z⋅(\\omega0 - b_g0) ⎥\n", + "t⋅q_z⋅(\\omega0 - b_g0) + 1.0⋅q_y ⎥\n", " ⎥\n", - "elta t⋅q_y⋅(\\omega0 - b_g0) ⎥\n", + "t⋅q_y⋅(\\omega0 - b_g0) + 1.0⋅q_z ⎥\n", " ⎥\n", - "Delta t⋅q_z⋅(\\omega2 - b_g2) ⎥\n", + " t⋅q_z⋅(\\omega2 - b_g2) + 1.0⋅q_w ⎥\n", " ⎥\n", " ⎥\n", " ⎥\n", @@ -1915,13 +1917,13 @@ } ], "source": [ - "f_pred = predict_mean(state, u)\n", + "f_pred = compute_mean(state, u, dT)\n", "f_pred" ] }, { "cell_type": "code", - "execution_count": 217, + "execution_count": 18, "metadata": {}, "outputs": [], "source": [ @@ -1952,7 +1954,7 @@ }, { "cell_type": "code", - "execution_count": 222, + "execution_count": 19, "metadata": {}, "outputs": [ { @@ -2062,7 +2064,7 @@ }, { "cell_type": "code", - "execution_count": 224, + "execution_count": 20, "metadata": {}, "outputs": [], "source": [ @@ -2075,7 +2077,7 @@ }, { "cell_type": "code", - "execution_count": 228, + "execution_count": 21, "metadata": {}, "outputs": [ { @@ -2464,7 +2466,7 @@ }, { "cell_type": "code", - "execution_count": 235, + "execution_count": 22, "metadata": {}, "outputs": [ { @@ -2489,13 +2491,7 @@ "\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": [ + "\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", @@ -2552,7 +2548,7 @@ " 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", + " // Total ops: 150\n", "\n", " // Unused inputs\n", " (void)epsilon;\n", @@ -2613,10 +2609,14 @@ " // 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(0, 0) =\n", + " _tmp0 * state(1, 0) + _tmp1 * state(3, 0) - _tmp2 * state(2, 0) + Scalar(1.0) * state(0, 0);\n", + " _res(1, 0) =\n", + " -_tmp0 * state(0, 0) + _tmp1 * state(2, 0) + _tmp2 * state(3, 0) + Scalar(1.0) * state(1, 0);\n", + " _res(2, 0) =\n", + " _tmp0 * state(3, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0) + Scalar(1.0) * state(2, 0);\n", + " _res(3, 0) =\n", + " -_tmp0 * state(2, 0) - _tmp1 * state(0, 0) - _tmp2 * state(1, 0) + Scalar(1.0) * state(3, 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", @@ -2648,6 +2648,7 @@ "\n", " _res_D_state.setZero();\n", "\n", + " _res_D_state(0, 0) = Scalar(1.0);\n", " _res_D_state(1, 0) = _tmp27;\n", " _res_D_state(2, 0) = _tmp2;\n", " _res_D_state(3, 0) = _tmp28;\n", @@ -2655,6 +2656,7 @@ " _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(1, 1) = Scalar(1.0);\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", @@ -2662,6 +2664,7 @@ " _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(2, 2) = Scalar(1.0);\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", @@ -2669,6 +2672,7 @@ " _res_D_state(0, 3) = _tmp1;\n", " _res_D_state(1, 3) = _tmp2;\n", " _res_D_state(2, 3) = _tmp0;\n", + " _res_D_state(3, 3) = Scalar(1.0);\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",