Generate Kalman Filter C Code.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Define Dynamic System Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "\n",
    "# states, inputs, and process noise\n",
    "X=Matrix(symbols('X[0] X[1] X[2] X[3] X[4] X[5] X[6] X[7] X[8] X[9] X[10] X[11] X[12] X[13] X[14]'))\n",
    "U=Matrix(symbols('U[0] U[1] U[2] U[3] U[4] U[5]'))\n",
    "W=Matrix(symbols('W[0] W[1] W[2] W[3] W[4] W[5]'))\n",
    "\n",
    "# time step\n",
    "dt=symbols('dt')\n",
    "\n",
    "# continuous dynamics:\n",
    "g = 9.81\n",
    "x,y,z,vx,vy,vz,phi,theta,psi,lx,ly,lz,lp,lq,lr = X\n",
    "ax,ay,az,p,q,r = U\n",
    "wx,wy,wz,wp,wq,wr = W\n",
    "\n",
    "Rx = Matrix([[1, 0, 0], [0, cos(phi), -sin(phi)], [0, sin(phi), cos(phi)]])\n",
    "Ry = Matrix([[cos(theta), 0, sin(theta)],[0, 1, 0],[-sin(theta), 0, cos(theta)]])\n",
    "Rz = Matrix([[cos(psi), -sin(psi), 0],[sin(psi), cos(psi), 0], [0, 0, 1]])\n",
    "R = Rz*Ry*Rx\n",
    "a_NED = R*Matrix([ax-lx-wx,ay-ly-wy,az-lz-wz])\n",
    "\n",
    "f_continuous = Matrix([\n",
    "    vx,\n",
    "    vy,\n",
    "    vz,\n",
    "    a_NED[0],\n",
    "    a_NED[1],\n",
    "    a_NED[2] + g,\n",
    "    (p-lp-wp) + (q-lq-wq)*sin(phi)*tan(theta) + (r-lr-wr)*cos(phi)*tan(theta),\n",
    "    (q-lq-wq)*cos(phi) - (r-lr-wr)*sin(phi),\n",
    "    (q-lq-wq)*sin(phi)/cos(theta) + (r-lr-wr)*cos(phi)/cos(theta),\n",
    "    0,0,0,0,0,0\n",
    "])\n",
    "\n",
    "\n",
    "# discretized dynamics:\n",
    "f = X + f_continuous*dt\n",
    "\n",
    "# output function (measurement model):\n",
    "use_phi, use_theta, use_psi = symbols('ekf_use_phi ekf_use_theta ekf_use_psi') # 0 or 1\n",
    "h = Matrix([x,y,z,use_phi*phi,use_theta*theta,use_psi*psi])\n",
    "\n",
    "# matrices:\n",
    "F = f.jacobian(X)\n",
    "L = f.jacobian(W)\n",
    "H = h.jacobian(X)\n",
    "\n",
    "# substitute W with 0\n",
    "f = f.subs([(w,0) for w in W])\n",
    "F = F.subs([(w,0) for w in W])\n",
    "L = L.subs([(w,0) for w in W])\n",
    "H = H.subs([(w,0) for w in W])\n",
    "\n",
    "# extra matrices\n",
    "symmetric_indexing = lambda i,j: int(i*(i+1)/2+j) if i>=j else int(j*(j+1)/2+i)\n",
    "P = Matrix([[symbols(f'P[{symmetric_indexing(i,j)}]') for j in range(len(X))] for i in range(len(X))])\n",
    "Q = Matrix([[symbols(f'Q[{i}]') if i==j else '0' for j in range(len(W))] for i in range(len(W))])\n",
    "R = Matrix([[symbols(f'R[{i}]') if i==j else '0' for j in range(len(h))] for i in range(len(h))])\n",
    "Z = Matrix([symbols(f'Z[{i}]') for i in range(len(h))])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Generate EKF code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# function for inverting matrix with symbolic elements that is quicker\n",
    "def invert_function(num):\n",
    "    M = Matrix([[symbols(f'M[{i}{j}]') for j in range(num)] for i in range(num)])\n",
    "    M_inv = M.inverse_LU()\n",
    "    return lambda M_: M_inv.subs({M[i,j]:M_[i,j] for i in range(num) for j in range(num)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "PREDICTION:\n",
      "CSE\n",
      "SIMPLIFY\n",
      "CCODE\n",
      "UPDATE:\n",
      "CSE\n",
      "SIMPLIFY\n",
      "CCODE\n"
     ]
    }
   ],
   "source": [
    "# from sympy import symbols, sin\n",
    "from sympy.codegen.ast import CodeBlock, Assignment\n",
    "\n",
    "# EKF equations from: https://en.wikipedia.org/wiki/Extended_Kalman_filter: Non-additive noise formulation and equations\n",
    "\n",
    "# PREDICTION STEP\n",
    "Xpred = f\n",
    "Ppred = F*P*F.T + L*Q*L.T\n",
    "\n",
    "# UPDATE STEP\n",
    "S = H*P*H.T + R\n",
    "inv__ = invert_function(S.shape[0])\n",
    "K = P*H.T*inv__(S)\n",
    "Xup = X + K*(Z - h)\n",
    "Pup = (eye(len(X)) - K*H)*P\n",
    "\n",
    "\n",
    "# assignments\n",
    "Xpred_assigments = [Assignment(symbols(f'X_new[{i}]'), Xpred[i]) for i in range(len(X))]\n",
    "Ppred_assigments = [Assignment(symbols(f'P_new[{symmetric_indexing(i,j)}]'), Ppred[i,j]) for i in range(len(X)) for j in range(len(X)) if i >= j] # only the lower diagonal will be calculated\n",
    "\n",
    "Xup_assigments = [Assignment(symbols(f'X_new[{i}]'), Xup[i]) for i in range(len(X))]\n",
    "Pup_assigments = [Assignment(symbols(f'P_new[{symmetric_indexing(i,j)}]'), Pup[i,j]) for i in range(len(X)) for j in range(len(X)) if i >= j] # only the lower diagonal will be calculated\n",
    "\n",
    "# PREDICTION STEP\n",
    "print('PREDICTION:')\n",
    "# code generation\n",
    "prediction_code = CodeBlock(*Xpred_assigments, *Ppred_assigments)\n",
    "# common subexpression elimination with tmp variables\n",
    "print('CSE')\n",
    "prediction_code = prediction_code.cse(symbols=(symbols(f'tmp[{i}]') for i in range(10000)))\n",
    "# simplify\n",
    "print('SIMPLIFY')\n",
    "prediction_code = prediction_code.simplify()\n",
    "# count number of tmp variables\n",
    "prediction_code_N_tmps = len([s for s in prediction_code.left_hand_sides if s.name.startswith('tmp')])\n",
    "# generate C code\n",
    "print('CCODE')\n",
    "prediction_code = ccode(prediction_code)\n",
    "\n",
    "# UPDATE STEP\n",
    "print('UPDATE:')\n",
    "# code generation\n",
    "update_code = CodeBlock(*Xup_assigments, *Pup_assigments)\n",
    "# common subexpression elimination with tmp variables\n",
    "print('CSE')\n",
    "update_code = update_code.cse(symbols=(symbols(f'tmp[{i}]') for i in range(10000)))\n",
    "# simplify\n",
    "print('SIMPLIFY')\n",
    "update_code = update_code.simplify()\n",
    "# count number of tmp variables\n",
    "update_code_N_tmps = len([s for s in update_code.left_hand_sides if s.name.startswith('tmp')])\n",
    "# generate C code\n",
    "print('CCODE')\n",
    "update_code = ccode(update_code)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Prediction code:\n",
      "tmp[0] = cosf(X[8]);\n",
      "tmp[1] = cosf(X[7]);\n",
      "tmp[2] = U[0] - X[9];\n",
      "tmp[3] = tmp[1]*tmp[2];\n",
      "tmp[4] = U[2] - X[11];\n",
      "tmp[5] = sinf(X[6]);\n",
      "tmp[6] = sinf(X[8]);\n",
      "tmp[7] = tmp[5]*tmp[6];\n",
      "tmp[8] = sinf(X[7]);\n",
      "tmp[9] = cosf(X[6]);\n",
      "tmp[10] = tmp[0]*tmp[9];\n",
      "tmp[11] = tmp[10]*tmp[8] + tmp[7];\n",
      "tmp[12] = U[1] - X[10];\n",
      "tmp[13] = tmp[6]*tmp[9];\n",
      "tmp[14] = tmp[0]*tmp[5];\n",
      "tmp[15] = -tmp[13] + tmp[14]*tmp[8];\n",
      "tmp[16] = dt*(tmp[0]*tmp[3] + tmp[11]*tmp[4] + tmp[12]*tmp[15]);\n",
      "tmp[17] = tmp[3]*tmp[6];\n",
      "tmp[18] = tmp[10] + tmp[7]*tmp[8];\n",
      "tmp[19] = tmp[14] - tmp[6]*tmp[8]*tmp[9];\n",
      "tmp[20] = -tmp[19];\n",
      "tmp[21] = tmp[2]*tmp[8];\n",
      "tmp[22] = tmp[12]*tmp[1];\n",
      "tmp[23] = tmp[1]*tmp[4];\n",
      "tmp[24] = tanf(X[7]);\n",
      "tmp[25] = U[4] - X[13];\n",
      "tmp[26] = tmp[25]*tmp[5];\n",
      "tmp[27] = U[5] - X[14];\n",
      "tmp[28] = tmp[27]*tmp[9];\n",
      "tmp[29] = tmp[25]*tmp[9];\n",
      "tmp[30] = tmp[27]*tmp[5];\n",
      "tmp[31] = 1.0/tmp[1];\n",
      "tmp[32] = tmp[0]*tmp[1];\n",
      "tmp[33] = dt*tmp[32];\n",
      "tmp[34] = -tmp[15];\n",
      "tmp[35] = dt*tmp[34];\n",
      "tmp[36] = -tmp[11];\n",
      "tmp[37] = dt*tmp[36];\n",
      "tmp[38] = -tmp[0]*tmp[21] + tmp[10]*tmp[23] + tmp[14]*tmp[22];\n",
      "tmp[39] = dt*tmp[38];\n",
      "tmp[40] = tmp[11]*tmp[12] + tmp[34]*tmp[4];\n",
      "tmp[41] = dt*tmp[40];\n",
      "tmp[42] = -tmp[18];\n",
      "tmp[43] = tmp[12]*tmp[42] - tmp[17] + tmp[19]*tmp[4];\n",
      "tmp[44] = dt*tmp[43];\n",
      "tmp[45] = P[48]*dt;\n",
      "tmp[46] = P[58]*dt;\n",
      "tmp[47] = P[69]*dt;\n",
      "tmp[48] = P[24]*tmp[41] + P[31]*tmp[39] + P[39]*tmp[44] + P[9] - tmp[32]*tmp[45] + tmp[34]*tmp[46] + tmp[36]*tmp[47];\n",
      "tmp[49] = P[49]*dt;\n",
      "tmp[50] = P[59]*dt;\n",
      "tmp[51] = P[70]*dt;\n",
      "tmp[52] = P[50]*dt;\n",
      "tmp[53] = P[60]*dt;\n",
      "tmp[54] = P[71]*dt;\n",
      "tmp[55] = powf(dt, 2);\n",
      "tmp[56] = Q[1]*tmp[55];\n",
      "tmp[57] = Q[2]*tmp[55];\n",
      "tmp[58] = P[58] + P[61]*tmp[41] + P[62]*tmp[39] + P[63]*tmp[44] - P[64]*tmp[33] + P[65]*tmp[35] + P[76]*tmp[37];\n",
      "tmp[59] = P[69] + P[72]*tmp[41] + P[73]*tmp[39] + P[74]*tmp[44] - P[75]*tmp[33] + P[76]*tmp[35] + P[77]*tmp[37];\n",
      "tmp[60] = powf(tmp[1], 2);\n",
      "tmp[61] = Q[0]*tmp[55];\n",
      "tmp[62] = tmp[60]*tmp[61];\n",
      "tmp[63] = P[48] + P[51]*tmp[41] + P[52]*tmp[39] + P[53]*tmp[44] - P[54]*tmp[33] + P[64]*tmp[35] + P[75]*tmp[37];\n",
      "tmp[64] = tmp[1]*tmp[6];\n",
      "tmp[65] = dt*tmp[64];\n",
      "tmp[66] = dt*tmp[19];\n",
      "tmp[67] = dt*tmp[42];\n",
      "tmp[68] = tmp[13]*tmp[23] - tmp[21]*tmp[6] + tmp[22]*tmp[7];\n",
      "tmp[69] = dt*tmp[68];\n",
      "tmp[70] = tmp[12]*tmp[20] + tmp[42]*tmp[4];\n",
      "tmp[71] = dt*tmp[70];\n",
      "tmp[72] = P[13] + P[24]*tmp[71] + P[31]*tmp[69] + P[39]*tmp[16] + tmp[19]*tmp[47] + tmp[42]*tmp[46] - tmp[45]*tmp[64];\n",
      "tmp[73] = P[14] + P[25]*tmp[71] + P[32]*tmp[69] + P[40]*tmp[16] + tmp[19]*tmp[51] + tmp[42]*tmp[50] - tmp[49]*tmp[64];\n",
      "tmp[74] = P[25] + P[27]*tmp[71] + P[34]*tmp[69] + P[42]*tmp[16] - P[51]*tmp[65] + P[61]*tmp[67] + P[72]*tmp[66];\n",
      "tmp[75] = P[59] + P[61]*tmp[71] + P[62]*tmp[69] + P[63]*tmp[16] - P[64]*tmp[65] + P[65]*tmp[67] + P[76]*tmp[66];\n",
      "tmp[76] = P[70] + P[72]*tmp[71] + P[73]*tmp[69] + P[74]*tmp[16] - P[75]*tmp[65] + P[76]*tmp[67] + P[77]*tmp[66];\n",
      "tmp[77] = P[40] + P[42]*tmp[71] + P[43]*tmp[69] + P[44]*tmp[16] - P[53]*tmp[65] + P[63]*tmp[67] + P[74]*tmp[66];\n",
      "tmp[78] = P[32] + P[34]*tmp[71] + P[35]*tmp[69] + P[43]*tmp[16] - P[52]*tmp[65] + P[62]*tmp[67] + P[73]*tmp[66];\n",
      "tmp[79] = tmp[34]*tmp[56];\n",
      "tmp[80] = tmp[36]*tmp[57];\n",
      "tmp[81] = P[49] + P[51]*tmp[71] + P[52]*tmp[69] + P[53]*tmp[16] - P[54]*tmp[65] + P[64]*tmp[67] + P[75]*tmp[66];\n",
      "tmp[82] = dt*tmp[8];\n",
      "tmp[83] = dt*tmp[5];\n",
      "tmp[84] = tmp[1]*tmp[83];\n",
      "tmp[85] = dt*tmp[9];\n",
      "tmp[86] = tmp[1]*tmp[85];\n",
      "tmp[87] = tmp[22]*tmp[9] - tmp[23]*tmp[5];\n",
      "tmp[88] = dt*tmp[87];\n",
      "tmp[89] = -tmp[12]*tmp[5]*tmp[8] - tmp[3] - tmp[4]*tmp[8]*tmp[9];\n",
      "tmp[90] = dt*tmp[89];\n",
      "tmp[91] = tmp[1]*tmp[5];\n",
      "tmp[92] = tmp[1]*tmp[9];\n",
      "tmp[93] = P[18] + P[24]*tmp[88] + P[31]*tmp[90] + tmp[45]*tmp[8] - tmp[46]*tmp[91] - tmp[47]*tmp[92];\n",
      "tmp[94] = P[19] + P[25]*tmp[88] + P[32]*tmp[90] + tmp[49]*tmp[8] - tmp[50]*tmp[91] - tmp[51]*tmp[92];\n",
      "tmp[95] = P[20] + P[26]*tmp[88] + P[33]*tmp[90] + tmp[52]*tmp[8] - tmp[53]*tmp[91] - tmp[54]*tmp[92];\n",
      "tmp[96] = P[26] + P[27]*tmp[88] + P[34]*tmp[90] + P[51]*tmp[82] - P[61]*tmp[84] - P[72]*tmp[86];\n",
      "tmp[97] = P[60] + P[61]*tmp[88] + P[62]*tmp[90] + P[64]*tmp[82] - P[65]*tmp[84] - P[76]*tmp[86];\n",
      "tmp[98] = P[71] + P[72]*tmp[88] + P[73]*tmp[90] + P[75]*tmp[82] - P[76]*tmp[84] - P[77]*tmp[86];\n",
      "tmp[99] = P[41] + P[42]*tmp[88] + P[43]*tmp[90] + P[53]*tmp[82] - P[63]*tmp[84] - P[74]*tmp[86];\n",
      "tmp[100] = P[33] + P[34]*tmp[88] + P[35]*tmp[90] + P[52]*tmp[82] - P[62]*tmp[84] - P[73]*tmp[86];\n",
      "tmp[101] = P[50] + P[51]*tmp[88] + P[52]*tmp[90] + P[54]*tmp[82] - P[64]*tmp[84] - P[75]*tmp[86];\n",
      "tmp[102] = tmp[61]*tmp[8];\n",
      "tmp[103] = powf(tmp[5], 2);\n",
      "tmp[104] = powf(tmp[9], 2);\n",
      "tmp[105] = tmp[24]*tmp[85];\n",
      "tmp[106] = tmp[24]*tmp[83];\n",
      "tmp[107] = dt*(tmp[24]*tmp[29] - tmp[24]*tmp[30]) + 1;\n",
      "tmp[108] = powf(tmp[24], 2);\n",
      "tmp[109] = tmp[108] + 1;\n",
      "tmp[110] = tmp[109]*tmp[26] + tmp[109]*tmp[28];\n",
      "tmp[111] = P[81]*dt;\n",
      "tmp[112] = P[108]*dt;\n",
      "tmp[113] = tmp[24]*tmp[9];\n",
      "tmp[114] = tmp[112]*tmp[113];\n",
      "tmp[115] = P[94]*dt;\n",
      "tmp[116] = tmp[24]*tmp[5];\n",
      "tmp[117] = tmp[115]*tmp[116];\n",
      "tmp[118] = P[82]*dt;\n",
      "tmp[119] = P[109]*dt;\n",
      "tmp[120] = tmp[113]*tmp[119];\n",
      "tmp[121] = P[95]*dt;\n",
      "tmp[122] = tmp[116]*tmp[121];\n",
      "tmp[123] = P[83]*dt;\n",
      "tmp[124] = P[110]*dt;\n",
      "tmp[125] = P[96]*dt;\n",
      "tmp[126] = -P[26]*tmp[107] - P[33]*dt*tmp[110] + tmp[113]*tmp[124] + tmp[116]*tmp[125] + tmp[123];\n",
      "tmp[127] = dt*tmp[110];\n",
      "tmp[128] = P[87]*dt;\n",
      "tmp[129] = -P[100]*tmp[106] - P[114]*tmp[105] + P[51]*tmp[107] + P[52]*dt*tmp[110] - tmp[128];\n",
      "tmp[130] = P[88]*dt;\n",
      "tmp[131] = -P[101]*tmp[106] - P[115]*tmp[105] + P[61]*tmp[107] + P[62]*dt*tmp[110] - tmp[130];\n",
      "tmp[132] = P[89]*dt;\n",
      "tmp[133] = -P[102]*tmp[106] - P[116]*tmp[105] + P[72]*tmp[107] + P[73]*dt*tmp[110] - tmp[132];\n",
      "tmp[134] = P[85]*dt;\n",
      "tmp[135] = -P[112]*tmp[105] + P[34]*tmp[107] + P[35]*dt*tmp[110] - P[98]*tmp[106] - tmp[134];\n",
      "tmp[136] = P[84]*dt;\n",
      "tmp[137] = -P[111]*tmp[105] + P[27]*tmp[107] + P[34]*dt*tmp[110] - P[97]*tmp[106] - tmp[136];\n",
      "tmp[138] = P[86]*dt;\n",
      "tmp[139] = -P[113]*tmp[105] + P[42]*tmp[107] + P[43]*dt*tmp[110] - P[99]*tmp[106] - tmp[138];\n",
      "tmp[140] = Q[4]*tmp[55];\n",
      "tmp[141] = tmp[103]*tmp[140];\n",
      "tmp[142] = Q[5]*tmp[55];\n",
      "tmp[143] = tmp[104]*tmp[142];\n",
      "tmp[144] = P[103]*dt;\n",
      "tmp[145] = P[117]*dt;\n",
      "tmp[146] = P[84]*tmp[107] + P[85]*dt*tmp[110] - P[90]*dt - tmp[113]*tmp[145] - tmp[116]*tmp[144];\n",
      "tmp[147] = P[118]*tmp[85];\n",
      "tmp[148] = -P[104]*tmp[106] + P[97]*tmp[107] + P[98]*dt*tmp[110] - tmp[144] - tmp[147]*tmp[24];\n",
      "tmp[149] = P[118]*tmp[83];\n",
      "tmp[150] = P[111]*tmp[107] + P[112]*dt*tmp[110] - P[119]*tmp[105] - tmp[145] - tmp[149]*tmp[24];\n",
      "tmp[151] = -tmp[26] - tmp[28];\n",
      "tmp[152] = dt*tmp[151];\n",
      "tmp[153] = P[24]*tmp[152] + P[31] + tmp[112]*tmp[5] - tmp[115]*tmp[9];\n",
      "tmp[154] = P[25]*tmp[152] + P[32] + tmp[119]*tmp[5] - tmp[121]*tmp[9];\n",
      "tmp[155] = P[26]*tmp[152] + P[33] + tmp[124]*tmp[5] - tmp[125]*tmp[9];\n",
      "tmp[156] = P[111]*tmp[83] + P[27]*tmp[152] + P[34] - P[97]*tmp[85];\n",
      "tmp[157] = P[115]*tmp[83];\n",
      "tmp[158] = -P[101]*tmp[85] + P[61]*tmp[152] + P[62] + tmp[157];\n",
      "tmp[159] = P[102]*tmp[85];\n",
      "tmp[160] = P[116]*tmp[83] + P[72]*tmp[152] + P[73] - tmp[159];\n",
      "tmp[161] = P[113]*tmp[83] + P[42]*tmp[152] + P[43] - P[99]*tmp[85];\n",
      "tmp[162] = P[112]*tmp[83] + P[34]*tmp[152] + P[35] - P[98]*tmp[85];\n",
      "tmp[163] = -P[100]*tmp[85] + P[114]*tmp[83] + P[51]*tmp[152] + P[52];\n",
      "tmp[164] = P[85] + tmp[136]*tmp[151] - tmp[144]*tmp[9] + tmp[145]*tmp[5];\n",
      "tmp[165] = -P[104]*tmp[85] + P[97]*tmp[152] + P[98] + tmp[149];\n",
      "tmp[166] = P[111]*tmp[152] + P[112] + P[119]*tmp[83] - tmp[147];\n",
      "tmp[167] = tmp[31]*tmp[85];\n",
      "tmp[168] = tmp[31]*tmp[83];\n",
      "tmp[169] = tmp[29]*tmp[31] - tmp[30]*tmp[31];\n",
      "tmp[170] = dt*tmp[169];\n",
      "tmp[171] = 1.0/tmp[60];\n",
      "tmp[172] = tmp[171]*tmp[8];\n",
      "tmp[173] = tmp[172]*tmp[26] + tmp[172]*tmp[28];\n",
      "tmp[174] = dt*tmp[173];\n",
      "tmp[175] = tmp[31]*tmp[9];\n",
      "tmp[176] = tmp[31]*tmp[5];\n",
      "tmp[177] = P[24]*tmp[170] + P[31]*tmp[174] + P[39] - tmp[112]*tmp[175] - tmp[115]*tmp[176];\n",
      "tmp[178] = P[25]*tmp[170] + P[32]*tmp[174] + P[40] - tmp[119]*tmp[175] - tmp[121]*tmp[176];\n",
      "tmp[179] = P[26]*tmp[170] + P[33]*tmp[174] + P[41] - tmp[124]*tmp[175] - tmp[125]*tmp[176];\n",
      "tmp[180] = -P[111]*tmp[167] + P[27]*tmp[170] + P[34]*tmp[174] + P[42] - P[97]*tmp[168];\n",
      "tmp[181] = -P[101]*tmp[168] - P[115]*tmp[167] + P[61]*tmp[170] + P[62]*tmp[174] + P[63];\n",
      "tmp[182] = -P[102]*tmp[168] - P[116]*tmp[167] + P[72]*tmp[170] + P[73]*tmp[174] + P[74];\n",
      "tmp[183] = -P[113]*tmp[167] + P[42]*tmp[170] + P[43]*tmp[174] + P[44] - P[99]*tmp[168];\n",
      "tmp[184] = -P[112]*tmp[167] + P[34]*tmp[170] + P[35]*tmp[174] + P[43] - P[98]*tmp[168];\n",
      "tmp[185] = -P[100]*tmp[168] - P[114]*tmp[167] + P[51]*tmp[170] + P[52]*tmp[174] + P[53];\n",
      "tmp[186] = tmp[24]*tmp[31];\n",
      "tmp[187] = P[86] + tmp[134]*tmp[173] + tmp[136]*tmp[169] - tmp[144]*tmp[176] - tmp[145]*tmp[175];\n",
      "tmp[188] = P[111]*tmp[170] + P[112]*tmp[174] + P[113] - P[119]*tmp[167] - tmp[149]*tmp[31];\n",
      "tmp[189] = -P[104]*tmp[168] + P[97]*tmp[170] + P[98]*tmp[174] + P[99] - tmp[147]*tmp[31];\n",
      "tmp[190] = tmp[175]*tmp[5];\n",
      "X_new[0] = X[0] + X[3]*dt;\n",
      "X_new[1] = X[1] + X[4]*dt;\n",
      "X_new[2] = X[2] + X[5]*dt;\n",
      "X_new[3] = X[3] + tmp[16];\n",
      "X_new[4] = X[4] + dt*(tmp[12]*tmp[18] + tmp[17] + tmp[20]*tmp[4]);\n",
      "X_new[5] = X[5] + dt*(-tmp[21] + tmp[22]*tmp[5] + tmp[23]*tmp[9] + 9.8100000000000005);\n",
      "X_new[6] = X[6] + dt*(U[3] - X[12] + tmp[24]*tmp[26] + tmp[24]*tmp[28]);\n",
      "X_new[7] = X[7] + dt*(tmp[29] - tmp[30]);\n",
      "X_new[8] = X[8] + dt*(tmp[26]*tmp[31] + tmp[28]*tmp[31]);\n",
      "X_new[9] = X[9];\n",
      "X_new[10] = X[10];\n",
      "X_new[11] = X[11];\n",
      "X_new[12] = X[12];\n",
      "X_new[13] = X[13];\n",
      "X_new[14] = X[14];\n",
      "P_new[0] = P[0] + P[6]*dt + dt*(P[6] + P[9]*dt);\n",
      "P_new[1] = P[10]*dt + P[1] + dt*(P[13]*dt + P[7]);\n",
      "P_new[2] = P[11]*dt + P[2] + dt*(P[11] + P[14]*dt);\n",
      "P_new[3] = P[15]*dt + P[3] + dt*(P[18]*dt + P[8]);\n",
      "P_new[4] = P[16]*dt + P[4] + dt*(P[12] + P[19]*dt);\n",
      "P_new[5] = P[17]*dt + P[5] + dt*(P[17] + P[20]*dt);\n",
      "P_new[6] = P[21]*tmp[41] + P[28]*tmp[39] + P[36]*tmp[44] - P[45]*tmp[33] + P[55]*tmp[35] + P[66]*tmp[37] + P[6] + dt*tmp[48];\n",
      "P_new[7] = P[22]*tmp[41] + P[29]*tmp[39] + P[37]*tmp[44] - P[46]*tmp[33] + P[56]*tmp[35] + P[67]*tmp[37] + P[7] + dt*(P[13] + P[25]*tmp[41] + P[32]*tmp[39] + P[40]*tmp[44] - tmp[32]*tmp[49] + tmp[34]*tmp[50] + tmp[36]*tmp[51]);\n",
      "P_new[8] = P[23]*tmp[41] + P[30]*tmp[39] + P[38]*tmp[44] - P[47]*tmp[33] + P[57]*tmp[35] + P[68]*tmp[37] + P[8] + dt*(P[18] + P[26]*tmp[41] + P[33]*tmp[39] + P[41]*tmp[44] - tmp[32]*tmp[52] + tmp[34]*tmp[53] + tmp[36]*tmp[54]);\n",
      "P_new[9] = powf(tmp[0], 2)*tmp[62] - tmp[33]*tmp[63] + powf(tmp[34], 2)*tmp[56] + tmp[35]*tmp[58] + powf(tmp[36], 2)*tmp[57] + tmp[37]*tmp[59] + tmp[39]*(P[31] + P[34]*tmp[41] + P[35]*tmp[39] + P[43]*tmp[44] - P[52]*tmp[33] + P[62]*tmp[35] + P[73]*tmp[37]) + tmp[41]*(P[24] + P[27]*tmp[41] + P[34]*tmp[39] + P[42]*tmp[44] - P[51]*tmp[33] + P[61]*tmp[35] + P[72]*tmp[37]) + tmp[44]*(P[39] + P[42]*tmp[41] + P[43]*tmp[39] + P[44]*tmp[44] - P[53]*tmp[33] + P[63]*tmp[35] + P[74]*tmp[37]) + tmp[48];\n",
      "P_new[10] = P[10] + P[21]*tmp[71] + P[28]*tmp[69] + P[36]*tmp[16] - P[45]*tmp[65] + P[55]*tmp[67] + P[66]*tmp[66] + dt*tmp[72];\n",
      "P_new[11] = P[11] + P[22]*tmp[71] + P[29]*tmp[69] + P[37]*tmp[16] - P[46]*tmp[65] + P[56]*tmp[67] + P[67]*tmp[66] + dt*tmp[73];\n",
      "P_new[12] = P[12] + P[23]*tmp[71] + P[30]*tmp[69] + P[38]*tmp[16] - P[47]*tmp[65] + P[57]*tmp[67] + P[68]*tmp[66] + dt*(P[19] + P[26]*tmp[71] + P[33]*tmp[69] + P[41]*tmp[16] + tmp[19]*tmp[54] + tmp[42]*tmp[53] - tmp[52]*tmp[64]);\n",
      "P_new[13] = tmp[0]*tmp[62]*tmp[6] + tmp[19]*tmp[80] - tmp[33]*tmp[81] + tmp[35]*tmp[75] + tmp[37]*tmp[76] + tmp[39]*tmp[78] + tmp[41]*tmp[74] + tmp[42]*tmp[79] + tmp[44]*tmp[77] + tmp[72];\n",
      "P_new[14] = tmp[16]*tmp[77] + powf(tmp[19], 2)*tmp[57] + powf(tmp[42], 2)*tmp[56] + tmp[62]*powf(tmp[6], 2) - tmp[65]*tmp[81] + tmp[66]*tmp[76] + tmp[67]*tmp[75] + tmp[69]*tmp[78] + tmp[71]*tmp[74] + tmp[73];\n",
      "P_new[15] = P[15] + P[21]*tmp[88] + P[28]*tmp[90] + P[45]*tmp[82] - P[55]*tmp[84] - P[66]*tmp[86] + dt*tmp[93];\n",
      "P_new[16] = P[16] + P[22]*tmp[88] + P[29]*tmp[90] + P[46]*tmp[82] - P[56]*tmp[84] - P[67]*tmp[86] + dt*tmp[94];\n",
      "P_new[17] = P[17] + P[23]*tmp[88] + P[30]*tmp[90] + P[47]*tmp[82] - P[57]*tmp[84] - P[68]*tmp[86] + dt*tmp[95];\n",
      "P_new[18] = tmp[100]*tmp[39] - tmp[101]*tmp[33] - tmp[102]*tmp[32] + tmp[35]*tmp[97] + tmp[37]*tmp[98] + tmp[41]*tmp[96] + tmp[44]*tmp[99] - tmp[79]*tmp[91] - tmp[80]*tmp[92] + tmp[93];\n",
      "P_new[19] = tmp[100]*tmp[69] - tmp[101]*tmp[65] - tmp[102]*tmp[64] + tmp[16]*tmp[99] - tmp[19]*tmp[57]*tmp[92] - tmp[42]*tmp[56]*tmp[91] + tmp[66]*tmp[98] + tmp[67]*tmp[97] + tmp[71]*tmp[96] + tmp[94];\n",
      "P_new[20] = tmp[100]*tmp[90] + tmp[101]*tmp[82] + tmp[103]*tmp[56]*tmp[60] + tmp[104]*tmp[57]*tmp[60] + tmp[61]*powf(tmp[8], 2) - tmp[84]*tmp[97] - tmp[86]*tmp[98] + tmp[88]*tmp[96] + tmp[95];\n",
      "P_new[21] = -P[105]*tmp[105] + P[21]*tmp[107] + P[28]*dt*tmp[110] - P[78]*dt - P[91]*tmp[106] + dt*(P[24]*tmp[107] + P[31]*dt*tmp[110] - tmp[111] - tmp[114] - tmp[117]);\n",
      "P_new[22] = -P[106]*tmp[105] + P[22]*tmp[107] + P[29]*dt*tmp[110] - P[79]*dt - P[92]*tmp[106] + dt*(P[25]*tmp[107] + P[32]*dt*tmp[110] - tmp[118] - tmp[120] - tmp[122]);\n",
      "P_new[23] = -P[107]*tmp[105] + P[23]*tmp[107] + P[30]*dt*tmp[110] - P[80]*dt - P[93]*tmp[106] - dt*tmp[126];\n",
      "P_new[24] = P[24]*tmp[107] + P[31]*tmp[127] - tmp[111] - tmp[114] - tmp[117] - tmp[129]*tmp[33] + tmp[131]*tmp[35] + tmp[133]*tmp[37] + tmp[135]*tmp[39] + tmp[137]*tmp[41] + tmp[139]*tmp[44];\n",
      "P_new[25] = P[25]*tmp[107] + P[32]*tmp[127] - tmp[118] - tmp[120] - tmp[122] - tmp[129]*tmp[65] + tmp[131]*tmp[67] + tmp[133]*tmp[66] + tmp[135]*tmp[69] + tmp[137]*tmp[71] + tmp[139]*tmp[16];\n",
      "P_new[26] = dt*tmp[129]*tmp[8] + dt*tmp[135]*tmp[89] + dt*tmp[137]*tmp[87] - tmp[126] - tmp[131]*tmp[84] - tmp[133]*tmp[86];\n",
      "P_new[27] = Q[3]*tmp[55] - dt*tmp[146] - tmp[105]*tmp[150] - tmp[106]*tmp[148] + tmp[107]*tmp[137] + tmp[108]*tmp[141] + tmp[108]*tmp[143] + tmp[127]*tmp[135];\n",
      "P_new[28] = P[105]*tmp[83] + P[21]*tmp[152] + P[28] - P[91]*tmp[85] + dt*tmp[153];\n",
      "P_new[29] = P[106]*tmp[83] + P[22]*tmp[152] + P[29] - P[92]*tmp[85] + dt*tmp[154];\n",
      "P_new[30] = P[107]*tmp[83] + P[23]*tmp[152] + P[30] - P[93]*tmp[85] + dt*tmp[155];\n",
      "P_new[31] = tmp[153] + tmp[156]*tmp[41] + tmp[158]*tmp[35] + tmp[160]*tmp[37] + tmp[161]*tmp[44] + tmp[162]*tmp[39] - tmp[163]*tmp[33];\n",
      "P_new[32] = tmp[154] + tmp[156]*tmp[71] + tmp[158]*tmp[67] + tmp[160]*tmp[66] + tmp[161]*tmp[16] + tmp[162]*tmp[69] - tmp[163]*tmp[65];\n",
      "P_new[33] = tmp[155] + tmp[156]*tmp[88] - tmp[158]*tmp[84] - tmp[160]*tmp[86] + tmp[162]*tmp[90] + tmp[163]*tmp[82];\n",
      "P_new[34] = Q[4]*tmp[24]*tmp[55]*tmp[5]*tmp[9] + dt*tmp[110]*tmp[162] - dt*tmp[164] - tmp[105]*tmp[166] - tmp[106]*tmp[165] + tmp[107]*tmp[156] - tmp[113]*tmp[142]*tmp[5];\n",
      "P_new[35] = tmp[103]*tmp[142] + tmp[104]*tmp[140] + tmp[152]*tmp[156] + tmp[162] - tmp[165]*tmp[85] + tmp[166]*tmp[83];\n",
      "P_new[36] = -P[105]*tmp[167] + P[21]*tmp[170] + P[28]*tmp[174] + P[36] - P[91]*tmp[168] + dt*tmp[177];\n",
      "P_new[37] = -P[106]*tmp[167] + P[22]*tmp[170] + P[29]*tmp[174] + P[37] - P[92]*tmp[168] + dt*tmp[178];\n",
      "P_new[38] = -P[107]*tmp[167] + P[23]*tmp[170] + P[30]*tmp[174] + P[38] - P[93]*tmp[168] + dt*tmp[179];\n",
      "P_new[39] = tmp[177] + tmp[180]*tmp[41] + tmp[181]*tmp[35] + tmp[182]*tmp[37] + tmp[183]*tmp[44] + tmp[184]*tmp[39] - tmp[185]*tmp[33];\n",
      "P_new[40] = tmp[16]*tmp[183] + tmp[178] + tmp[180]*tmp[71] + tmp[181]*tmp[67] + tmp[182]*tmp[66] + tmp[184]*tmp[69] - tmp[185]*tmp[65];\n",
      "P_new[41] = tmp[179] + tmp[180]*tmp[88] - tmp[181]*tmp[84] - tmp[182]*tmp[86] + tmp[184]*tmp[90] + tmp[185]*tmp[82];\n",
      "P_new[42] = -dt*tmp[187] - tmp[105]*tmp[188] - tmp[106]*tmp[189] + tmp[107]*tmp[180] + tmp[127]*tmp[184] + tmp[141]*tmp[186] + tmp[143]*tmp[186];\n",
      "P_new[43] = tmp[140]*tmp[190] - tmp[142]*tmp[190] + tmp[152]*tmp[180] + tmp[184] + tmp[188]*tmp[83] - tmp[189]*tmp[85];\n",
      "P_new[44] = tmp[141]*tmp[171] + tmp[143]*tmp[171] - tmp[167]*tmp[188] - tmp[168]*tmp[189] + tmp[170]*tmp[180] + tmp[174]*tmp[184] + tmp[183];\n",
      "P_new[45] = P[45] + tmp[45];\n",
      "P_new[46] = P[46] + tmp[49];\n",
      "P_new[47] = P[47] + tmp[52];\n",
      "P_new[48] = tmp[63];\n",
      "P_new[49] = tmp[81];\n",
      "P_new[50] = tmp[101];\n",
      "P_new[51] = tmp[129];\n",
      "P_new[52] = tmp[163];\n",
      "P_new[53] = tmp[185];\n",
      "P_new[54] = P[54];\n",
      "P_new[55] = P[55] + tmp[46];\n",
      "P_new[56] = P[56] + tmp[50];\n",
      "P_new[57] = P[57] + tmp[53];\n",
      "P_new[58] = tmp[58];\n",
      "P_new[59] = tmp[75];\n",
      "P_new[60] = tmp[97];\n",
      "P_new[61] = tmp[131];\n",
      "P_new[62] = tmp[158];\n",
      "P_new[63] = tmp[181];\n",
      "P_new[64] = P[64];\n",
      "P_new[65] = P[65];\n",
      "P_new[66] = P[66] + tmp[47];\n",
      "P_new[67] = P[67] + tmp[51];\n",
      "P_new[68] = P[68] + tmp[54];\n",
      "P_new[69] = tmp[59];\n",
      "P_new[70] = tmp[76];\n",
      "P_new[71] = tmp[98];\n",
      "P_new[72] = tmp[133];\n",
      "P_new[73] = tmp[160];\n",
      "P_new[74] = tmp[182];\n",
      "P_new[75] = P[75];\n",
      "P_new[76] = P[76];\n",
      "P_new[77] = P[77];\n",
      "P_new[78] = P[78] + tmp[111];\n",
      "P_new[79] = P[79] + tmp[118];\n",
      "P_new[80] = P[80] + tmp[123];\n",
      "P_new[81] = P[81] - tmp[128]*tmp[32] + tmp[130]*tmp[34] + tmp[132]*tmp[36] + tmp[134]*tmp[38] + tmp[136]*tmp[40] + tmp[138]*tmp[43];\n",
      "P_new[82] = P[82] + P[86]*tmp[16] - tmp[128]*tmp[64] + tmp[130]*tmp[42] + tmp[132]*tmp[19] + tmp[134]*tmp[68] + tmp[136]*tmp[70];\n",
      "P_new[83] = P[83] + tmp[128]*tmp[8] - tmp[130]*tmp[91] - tmp[132]*tmp[92] + tmp[134]*tmp[89] + tmp[136]*tmp[87];\n",
      "P_new[84] = tmp[146];\n",
      "P_new[85] = tmp[164];\n",
      "P_new[86] = tmp[187];\n",
      "P_new[87] = P[87];\n",
      "P_new[88] = P[88];\n",
      "P_new[89] = P[89];\n",
      "P_new[90] = P[90];\n",
      "P_new[91] = P[91] + tmp[115];\n",
      "P_new[92] = P[92] + tmp[121];\n",
      "P_new[93] = P[93] + tmp[125];\n",
      "P_new[94] = -P[100]*tmp[33] + P[101]*tmp[35] + P[102]*tmp[37] + P[94] + P[97]*tmp[41] + P[98]*tmp[39] + P[99]*tmp[44];\n",
      "P_new[95] = -P[100]*tmp[65] + P[101]*tmp[67] + P[102]*tmp[66] + P[95] + P[97]*tmp[71] + P[98]*tmp[69] + P[99]*tmp[16];\n",
      "P_new[96] = P[100]*tmp[82] - P[101]*tmp[84] + P[96] + P[97]*tmp[88] + P[98]*tmp[90] - tmp[159]*tmp[1];\n",
      "P_new[97] = tmp[148];\n",
      "P_new[98] = tmp[165];\n",
      "P_new[99] = tmp[189];\n",
      "P_new[100] = P[100];\n",
      "P_new[101] = P[101];\n",
      "P_new[102] = P[102];\n",
      "P_new[103] = P[103];\n",
      "P_new[104] = P[104];\n",
      "P_new[105] = P[105] + tmp[112];\n",
      "P_new[106] = P[106] + tmp[119];\n",
      "P_new[107] = P[107] + tmp[124];\n",
      "P_new[108] = P[108] + P[111]*tmp[41] + P[112]*tmp[39] + P[113]*tmp[44] - P[114]*tmp[33] + P[115]*tmp[35] + P[116]*tmp[37];\n",
      "P_new[109] = P[109] + P[111]*tmp[71] + P[112]*tmp[69] + P[113]*tmp[16] - P[114]*tmp[65] + P[115]*tmp[67] + P[116]*tmp[66];\n",
      "P_new[110] = P[110] + P[111]*tmp[88] + P[112]*tmp[90] + P[114]*tmp[82] - P[116]*tmp[86] - tmp[157]*tmp[1];\n",
      "P_new[111] = tmp[150];\n",
      "P_new[112] = tmp[166];\n",
      "P_new[113] = tmp[188];\n",
      "P_new[114] = P[114];\n",
      "P_new[115] = P[115];\n",
      "P_new[116] = P[116];\n",
      "P_new[117] = P[117];\n",
      "P_new[118] = P[118];\n",
      "P_new[119] = P[119];\n",
      "Update code:\n",
      "tmp[0] = -X[8]*ekf_use_psi + Z[5];\n",
      "tmp[1] = powf(ekf_use_psi, 2);\n",
      "tmp[2] = 1.0/(P[0] + R[0]);\n",
      "tmp[3] = 1.0/(-powf(P[1], 2)*tmp[2] + P[2] + R[1]);\n",
      "tmp[4] = P[1]*tmp[2];\n",
      "tmp[5] = P[36]*ekf_use_psi;\n",
      "tmp[6] = P[37]*ekf_use_psi - tmp[4]*tmp[5];\n",
      "tmp[7] = -P[3]*tmp[4] + P[4];\n",
      "tmp[8] = 1.0/(-powf(P[3], 2)*tmp[2] + P[5] + R[2] - tmp[3]*powf(tmp[7], 2));\n",
      "tmp[9] = P[3]*tmp[2];\n",
      "tmp[10] = tmp[3]*tmp[7];\n",
      "tmp[11] = P[38]*ekf_use_psi - tmp[10]*tmp[6] - tmp[5]*tmp[9];\n",
      "tmp[12] = powf(ekf_use_phi, 2);\n",
      "tmp[13] = P[21]*ekf_use_phi;\n",
      "tmp[14] = P[22]*ekf_use_phi - tmp[13]*tmp[4];\n",
      "tmp[15] = P[23]*ekf_use_phi - tmp[10]*tmp[14] - tmp[13]*tmp[9];\n",
      "tmp[16] = 1.0/(-powf(P[21], 2)*tmp[12]*tmp[2] + P[27]*tmp[12] + R[3] - powf(tmp[14], 2)*tmp[3] - powf(tmp[15], 2)*tmp[8]);\n",
      "tmp[17] = P[21]*tmp[2];\n",
      "tmp[18] = ekf_use_phi*tmp[17];\n",
      "tmp[19] = tmp[14]*tmp[3];\n",
      "tmp[20] = tmp[15]*tmp[8];\n",
      "tmp[21] = P[42]*ekf_use_phi*ekf_use_psi - tmp[11]*tmp[20] - tmp[18]*tmp[5] - tmp[19]*tmp[6];\n",
      "tmp[22] = powf(ekf_use_theta, 2);\n",
      "tmp[23] = P[28]*ekf_use_theta;\n",
      "tmp[24] = P[29]*ekf_use_theta - tmp[23]*tmp[4];\n",
      "tmp[25] = P[30]*ekf_use_theta - tmp[10]*tmp[24] - tmp[23]*tmp[9];\n",
      "tmp[26] = P[34]*ekf_use_phi*ekf_use_theta - tmp[18]*tmp[23] - tmp[19]*tmp[24] - tmp[20]*tmp[25];\n",
      "tmp[27] = 1.0/(-powf(P[28], 2)*tmp[22]*tmp[2] + P[35]*tmp[22] + R[4] - tmp[16]*powf(tmp[26], 2) - powf(tmp[24], 2)*tmp[3] - powf(tmp[25], 2)*tmp[8]);\n",
      "tmp[28] = P[28]*tmp[2];\n",
      "tmp[29] = ekf_use_theta*tmp[28];\n",
      "tmp[30] = tmp[24]*tmp[3];\n",
      "tmp[31] = tmp[25]*tmp[8];\n",
      "tmp[32] = tmp[16]*tmp[26];\n",
      "tmp[33] = P[43]*ekf_use_psi*ekf_use_theta - tmp[11]*tmp[31] - tmp[21]*tmp[32] - tmp[29]*tmp[5] - tmp[30]*tmp[6];\n",
      "tmp[34] = tmp[27]*powf(tmp[33], 2);\n",
      "tmp[35] = 1.0/(-powf(P[36], 2)*tmp[1]*tmp[2] + P[44]*tmp[1] + R[5] - powf(tmp[11], 2)*tmp[8] - tmp[16]*powf(tmp[21], 2) - tmp[34] - tmp[3]*powf(tmp[6], 2));\n",
      "tmp[36] = tmp[35]*tmp[5];\n",
      "tmp[37] = tmp[27]*tmp[33];\n",
      "tmp[38] = tmp[35]*tmp[37];\n",
      "tmp[39] = tmp[21]*tmp[35];\n",
      "tmp[40] = tmp[16]*(tmp[26]*tmp[38] - tmp[39]);\n",
      "tmp[41] = tmp[11]*tmp[35];\n",
      "tmp[42] = tmp[8]*(-tmp[15]*tmp[40] + tmp[25]*tmp[27]*tmp[33]*tmp[35] - tmp[41]);\n",
      "tmp[43] = tmp[35]*tmp[6];\n",
      "tmp[44] = -tmp[14]*tmp[40] + tmp[24]*tmp[27]*tmp[33]*tmp[35] - tmp[42]*tmp[7] - tmp[43];\n",
      "tmp[45] = P[1]*tmp[3];\n",
      "tmp[46] = P[3]*tmp[42] + tmp[13]*tmp[40] - tmp[23]*tmp[38] + tmp[36] + tmp[44]*tmp[45];\n",
      "tmp[47] = -tmp[46];\n",
      "tmp[48] = P[0]*tmp[2];\n",
      "tmp[49] = tmp[46] + tmp[47]*tmp[48];\n",
      "tmp[50] = -X[7]*ekf_use_theta + Z[4];\n",
      "tmp[51] = tmp[27]*(tmp[34]*tmp[35] + 1);\n",
      "tmp[52] = tmp[16]*(tmp[21]*tmp[27]*tmp[33]*tmp[35] - tmp[26]*tmp[51]);\n",
      "tmp[53] = tmp[8]*(tmp[11]*tmp[27]*tmp[33]*tmp[35] - tmp[15]*tmp[52] - tmp[25]*tmp[51]);\n",
      "tmp[54] = -tmp[14]*tmp[52] - tmp[24]*tmp[51] + tmp[27]*tmp[33]*tmp[35]*tmp[6] - tmp[53]*tmp[7];\n",
      "tmp[55] = P[3]*tmp[53] + tmp[13]*tmp[52] + tmp[23]*tmp[51] - tmp[36]*tmp[37] + tmp[45]*tmp[54];\n",
      "tmp[56] = -tmp[55];\n",
      "tmp[57] = tmp[48]*tmp[56] + tmp[55];\n",
      "tmp[58] = -X[6]*ekf_use_phi + Z[3];\n",
      "tmp[59] = tmp[16]*tmp[21];\n",
      "tmp[60] = tmp[32]*tmp[37] - tmp[59];\n",
      "tmp[61] = tmp[33]*tmp[35];\n",
      "tmp[62] = tmp[27]*(-tmp[32] - tmp[60]*tmp[61]);\n",
      "tmp[63] = tmp[16]*(-tmp[26]*tmp[62] - tmp[39]*tmp[60] + 1);\n",
      "tmp[64] = tmp[8]*(-tmp[15]*tmp[63] - tmp[25]*tmp[62] - tmp[41]*tmp[60]);\n",
      "tmp[65] = -tmp[14]*tmp[63] - tmp[24]*tmp[62] - tmp[43]*tmp[60] - tmp[64]*tmp[7];\n",
      "tmp[66] = P[3]*tmp[64] + tmp[13]*tmp[63] + tmp[23]*tmp[62] + tmp[36]*tmp[60] + tmp[45]*tmp[65];\n",
      "tmp[67] = -tmp[66];\n",
      "tmp[68] = tmp[48]*tmp[67] + tmp[66];\n",
      "tmp[69] = -X[2] + Z[2];\n",
      "tmp[70] = tmp[11]*tmp[8];\n",
      "tmp[71] = tmp[15]*tmp[16]*tmp[21]*tmp[8] - tmp[37]*(tmp[20]*tmp[32] - tmp[31]) - tmp[70];\n",
      "tmp[72] = tmp[27]*(tmp[15]*tmp[16]*tmp[26]*tmp[8] - tmp[31] - tmp[61]*tmp[71]);\n",
      "tmp[73] = tmp[16]*(-tmp[20] - tmp[26]*tmp[72] - tmp[39]*tmp[71]);\n",
      "tmp[74] = tmp[8]*(-tmp[15]*tmp[73] - tmp[25]*tmp[72] - tmp[41]*tmp[71] + 1);\n",
      "tmp[75] = -tmp[14]*tmp[73] - tmp[24]*tmp[72] - tmp[43]*tmp[71] - tmp[74]*tmp[7];\n",
      "tmp[76] = P[3]*tmp[74] + tmp[13]*tmp[73] + tmp[23]*tmp[72] + tmp[36]*tmp[71] + tmp[45]*tmp[75];\n",
      "tmp[77] = -tmp[76];\n",
      "tmp[78] = tmp[48]*tmp[77] + tmp[76];\n",
      "tmp[79] = -X[1] + Z[1];\n",
      "tmp[80] = tmp[10]*tmp[20] - tmp[19];\n",
      "tmp[81] = -tmp[25]*tmp[3]*tmp[7]*tmp[8] + tmp[30] + tmp[32]*tmp[80];\n",
      "tmp[82] = tmp[11]*tmp[3]*tmp[7]*tmp[8] + tmp[37]*tmp[81] - tmp[3]*tmp[6] - tmp[59]*tmp[80];\n",
      "tmp[83] = tmp[27]*(-tmp[61]*tmp[82] - tmp[81]);\n",
      "tmp[84] = tmp[16]*(tmp[15]*tmp[3]*tmp[7]*tmp[8] - tmp[19] - tmp[26]*tmp[83] - tmp[39]*tmp[82]);\n",
      "tmp[85] = tmp[8]*(-tmp[10] - tmp[15]*tmp[84] - tmp[25]*tmp[83] - tmp[41]*tmp[82]);\n",
      "tmp[86] = -tmp[14]*tmp[84] - tmp[24]*tmp[83] - tmp[43]*tmp[82] - tmp[7]*tmp[85] + 1;\n",
      "tmp[87] = P[3]*tmp[85] + tmp[13]*tmp[84] + tmp[23]*tmp[83] + tmp[36]*tmp[82] + tmp[45]*tmp[86];\n",
      "tmp[88] = -tmp[87];\n",
      "tmp[89] = tmp[48]*tmp[88] + tmp[87];\n",
      "tmp[90] = -X[0] + Z[0];\n",
      "tmp[91] = tmp[10]*tmp[4] - tmp[9];\n",
      "tmp[92] = -P[1]*tmp[14]*tmp[2]*tmp[3] + tmp[18] + tmp[20]*tmp[91];\n",
      "tmp[93] = -tmp[92];\n",
      "tmp[94] = -P[1]*tmp[24]*tmp[2]*tmp[3] + tmp[29] + tmp[31]*tmp[91] + tmp[32]*tmp[93];\n",
      "tmp[95] = P[1]*tmp[2]*tmp[3]*tmp[6] - tmp[2]*tmp[5] + tmp[37]*tmp[94] - tmp[59]*tmp[93] - tmp[70]*tmp[91];\n",
      "tmp[96] = tmp[27]*(-tmp[61]*tmp[95] - tmp[94]);\n",
      "tmp[97] = tmp[16]*(-tmp[26]*tmp[96] - tmp[39]*tmp[95] - tmp[92]);\n",
      "tmp[98] = tmp[8]*(P[1]*tmp[2]*tmp[3]*tmp[7] - tmp[15]*tmp[97] - tmp[25]*tmp[96] - tmp[41]*tmp[95] - tmp[9]);\n",
      "tmp[99] = -tmp[14]*tmp[97] - tmp[24]*tmp[96] - tmp[43]*tmp[95] - tmp[4] - tmp[7]*tmp[98];\n",
      "tmp[100] = P[3]*tmp[98] + tmp[13]*tmp[97] + tmp[23]*tmp[96] + tmp[36]*tmp[95] + tmp[45]*tmp[99];\n",
      "tmp[101] = tmp[100] - 1;\n",
      "tmp[102] = -tmp[101];\n",
      "tmp[103] = tmp[102]*tmp[48];\n",
      "tmp[104] = P[37]*ekf_use_psi;\n",
      "tmp[105] = tmp[104]*tmp[35];\n",
      "tmp[106] = P[29]*ekf_use_theta;\n",
      "tmp[107] = P[22]*ekf_use_phi;\n",
      "tmp[108] = P[2]*tmp[3];\n",
      "tmp[109] = P[4]*tmp[42] + tmp[105] - tmp[106]*tmp[38] + tmp[107]*tmp[40] + tmp[108]*tmp[44] + tmp[47]*tmp[4];\n",
      "tmp[110] = P[4]*tmp[53] - tmp[105]*tmp[37] + tmp[106]*tmp[51] + tmp[107]*tmp[52] + tmp[108]*tmp[54] + tmp[4]*tmp[56];\n",
      "tmp[111] = P[4]*tmp[64] + tmp[105]*tmp[60] + tmp[106]*tmp[62] + tmp[107]*tmp[63] + tmp[108]*tmp[65] + tmp[4]*tmp[67];\n",
      "tmp[112] = P[4]*tmp[74] + tmp[105]*tmp[71] + tmp[106]*tmp[72] + tmp[107]*tmp[73] + tmp[108]*tmp[75] + tmp[4]*tmp[77];\n",
      "tmp[113] = P[4]*tmp[85] + tmp[105]*tmp[82] + tmp[106]*tmp[83] + tmp[107]*tmp[84] + tmp[108]*tmp[86] + tmp[4]*tmp[88];\n",
      "tmp[114] = P[4]*tmp[98] + tmp[102]*tmp[4] + tmp[105]*tmp[95] + tmp[106]*tmp[96] + tmp[107]*tmp[97] + tmp[108]*tmp[99];\n",
      "tmp[115] = P[38]*ekf_use_psi;\n",
      "tmp[116] = tmp[115]*tmp[35];\n",
      "tmp[117] = P[30]*ekf_use_theta;\n",
      "tmp[118] = P[23]*ekf_use_phi;\n",
      "tmp[119] = P[4]*tmp[3];\n",
      "tmp[120] = P[5]*tmp[42] + tmp[116] - tmp[117]*tmp[38] + tmp[118]*tmp[40] + tmp[119]*tmp[44] + tmp[47]*tmp[9];\n",
      "tmp[121] = P[5]*tmp[53] - tmp[116]*tmp[37] + tmp[117]*tmp[51] + tmp[118]*tmp[52] + tmp[119]*tmp[54] + tmp[56]*tmp[9];\n",
      "tmp[122] = P[5]*tmp[64] + tmp[116]*tmp[60] + tmp[117]*tmp[62] + tmp[118]*tmp[63] + tmp[119]*tmp[65] + tmp[67]*tmp[9];\n",
      "tmp[123] = P[5]*tmp[74] + tmp[116]*tmp[71] + tmp[117]*tmp[72] + tmp[118]*tmp[73] + tmp[119]*tmp[75] + tmp[77]*tmp[9];\n",
      "tmp[124] = P[5]*tmp[85] + tmp[116]*tmp[82] + tmp[117]*tmp[83] + tmp[118]*tmp[84] + tmp[119]*tmp[86] + tmp[88]*tmp[9];\n",
      "tmp[125] = P[5]*tmp[98] + tmp[102]*tmp[9] + tmp[116]*tmp[95] + tmp[117]*tmp[96] + tmp[118]*tmp[97] + tmp[119]*tmp[99];\n",
      "tmp[126] = ekf_use_psi*tmp[35];\n",
      "tmp[127] = P[39]*tmp[126];\n",
      "tmp[128] = P[31]*ekf_use_theta;\n",
      "tmp[129] = P[24]*ekf_use_phi;\n",
      "tmp[130] = P[7]*tmp[3];\n",
      "tmp[131] = P[6]*tmp[2];\n",
      "tmp[132] = P[8]*tmp[42] + tmp[127] - tmp[128]*tmp[38] + tmp[129]*tmp[40] + tmp[130]*tmp[44] + tmp[131]*tmp[47];\n",
      "tmp[133] = P[8]*tmp[53] - tmp[127]*tmp[37] + tmp[128]*tmp[51] + tmp[129]*tmp[52] + tmp[130]*tmp[54] + tmp[131]*tmp[56];\n",
      "tmp[134] = P[8]*tmp[64] + tmp[127]*tmp[60] + tmp[128]*tmp[62] + tmp[129]*tmp[63] + tmp[130]*tmp[65] + tmp[131]*tmp[67];\n",
      "tmp[135] = P[8]*tmp[74] + tmp[127]*tmp[71] + tmp[128]*tmp[72] + tmp[129]*tmp[73] + tmp[130]*tmp[75] + tmp[131]*tmp[77];\n",
      "tmp[136] = P[8]*tmp[85] + tmp[127]*tmp[82] + tmp[128]*tmp[83] + tmp[129]*tmp[84] + tmp[130]*tmp[86] + tmp[131]*tmp[88];\n",
      "tmp[137] = P[8]*tmp[98] + tmp[102]*tmp[131] + tmp[127]*tmp[95] + tmp[128]*tmp[96] + tmp[129]*tmp[97] + tmp[130]*tmp[99];\n",
      "tmp[138] = P[40]*tmp[126];\n",
      "tmp[139] = P[32]*ekf_use_theta;\n",
      "tmp[140] = P[25]*ekf_use_phi;\n",
      "tmp[141] = P[11]*tmp[3];\n",
      "tmp[142] = P[10]*tmp[2];\n",
      "tmp[143] = P[12]*tmp[42] + tmp[138] - tmp[139]*tmp[38] + tmp[140]*tmp[40] + tmp[141]*tmp[44] + tmp[142]*tmp[47];\n",
      "tmp[144] = P[12]*tmp[53] - tmp[138]*tmp[37] + tmp[139]*tmp[51] + tmp[140]*tmp[52] + tmp[141]*tmp[54] + tmp[142]*tmp[56];\n",
      "tmp[145] = P[12]*tmp[64] + tmp[138]*tmp[60] + tmp[139]*tmp[62] + tmp[140]*tmp[63] + tmp[141]*tmp[65] + tmp[142]*tmp[67];\n",
      "tmp[146] = P[12]*tmp[74] + tmp[138]*tmp[71] + tmp[139]*tmp[72] + tmp[140]*tmp[73] + tmp[141]*tmp[75] + tmp[142]*tmp[77];\n",
      "tmp[147] = P[12]*tmp[85] + tmp[138]*tmp[82] + tmp[139]*tmp[83] + tmp[140]*tmp[84] + tmp[141]*tmp[86] + tmp[142]*tmp[88];\n",
      "tmp[148] = P[12]*tmp[98] + tmp[102]*tmp[142] + tmp[138]*tmp[95] + tmp[139]*tmp[96] + tmp[140]*tmp[97] + tmp[141]*tmp[99];\n",
      "tmp[149] = P[41]*tmp[126];\n",
      "tmp[150] = P[33]*ekf_use_theta;\n",
      "tmp[151] = P[26]*ekf_use_phi;\n",
      "tmp[152] = P[16]*tmp[3];\n",
      "tmp[153] = P[15]*tmp[2];\n",
      "tmp[154] = P[17]*tmp[42] + tmp[149] - tmp[150]*tmp[38] + tmp[151]*tmp[40] + tmp[152]*tmp[44] + tmp[153]*tmp[47];\n",
      "tmp[155] = P[17]*tmp[53] - tmp[149]*tmp[37] + tmp[150]*tmp[51] + tmp[151]*tmp[52] + tmp[152]*tmp[54] + tmp[153]*tmp[56];\n",
      "tmp[156] = P[17]*tmp[64] + tmp[149]*tmp[60] + tmp[150]*tmp[62] + tmp[151]*tmp[63] + tmp[152]*tmp[65] + tmp[153]*tmp[67];\n",
      "tmp[157] = P[17]*tmp[74] + tmp[149]*tmp[71] + tmp[150]*tmp[72] + tmp[151]*tmp[73] + tmp[152]*tmp[75] + tmp[153]*tmp[77];\n",
      "tmp[158] = P[17]*tmp[85] + tmp[149]*tmp[82] + tmp[150]*tmp[83] + tmp[151]*tmp[84] + tmp[152]*tmp[86] + tmp[153]*tmp[88];\n",
      "tmp[159] = P[17]*tmp[98] + tmp[102]*tmp[153] + tmp[149]*tmp[95] + tmp[150]*tmp[96] + tmp[151]*tmp[97] + tmp[152]*tmp[99];\n",
      "tmp[160] = P[42]*ekf_use_psi;\n",
      "tmp[161] = tmp[160]*tmp[35];\n",
      "tmp[162] = P[34]*ekf_use_theta;\n",
      "tmp[163] = P[27]*ekf_use_phi;\n",
      "tmp[164] = P[22]*tmp[3];\n",
      "tmp[165] = P[23]*tmp[42] + tmp[161] - tmp[162]*tmp[38] + tmp[163]*tmp[40] + tmp[164]*tmp[44] + tmp[17]*tmp[47];\n",
      "tmp[166] = P[23]*tmp[53] - tmp[161]*tmp[37] + tmp[162]*tmp[51] + tmp[163]*tmp[52] + tmp[164]*tmp[54] + tmp[17]*tmp[56];\n",
      "tmp[167] = P[23]*tmp[64] + tmp[161]*tmp[60] + tmp[162]*tmp[62] + tmp[163]*tmp[63] + tmp[164]*tmp[65] + tmp[17]*tmp[67];\n",
      "tmp[168] = P[23]*tmp[74] + tmp[161]*tmp[71] + tmp[162]*tmp[72] + tmp[163]*tmp[73] + tmp[164]*tmp[75] + tmp[17]*tmp[77];\n",
      "tmp[169] = P[23]*tmp[85] + tmp[161]*tmp[82] + tmp[162]*tmp[83] + tmp[163]*tmp[84] + tmp[164]*tmp[86] + tmp[17]*tmp[88];\n",
      "tmp[170] = P[23]*tmp[98] + tmp[102]*tmp[17] + tmp[161]*tmp[95] + tmp[162]*tmp[96] + tmp[163]*tmp[97] + tmp[164]*tmp[99];\n",
      "tmp[171] = P[43]*ekf_use_psi;\n",
      "tmp[172] = tmp[171]*tmp[35];\n",
      "tmp[173] = P[35]*ekf_use_theta;\n",
      "tmp[174] = P[34]*ekf_use_phi;\n",
      "tmp[175] = P[29]*tmp[3];\n",
      "tmp[176] = P[30]*tmp[42] + tmp[172] - tmp[173]*tmp[38] + tmp[174]*tmp[40] + tmp[175]*tmp[44] + tmp[28]*tmp[47];\n",
      "tmp[177] = P[30]*tmp[53] - tmp[172]*tmp[37] + tmp[173]*tmp[51] + tmp[174]*tmp[52] + tmp[175]*tmp[54] + tmp[28]*tmp[56];\n",
      "tmp[178] = P[30]*tmp[64] + tmp[172]*tmp[60] + tmp[173]*tmp[62] + tmp[174]*tmp[63] + tmp[175]*tmp[65] + tmp[28]*tmp[67];\n",
      "tmp[179] = P[30]*tmp[74] + tmp[172]*tmp[71] + tmp[173]*tmp[72] + tmp[174]*tmp[73] + tmp[175]*tmp[75] + tmp[28]*tmp[77];\n",
      "tmp[180] = P[30]*tmp[85] + tmp[172]*tmp[82] + tmp[173]*tmp[83] + tmp[174]*tmp[84] + tmp[175]*tmp[86] + tmp[28]*tmp[88];\n",
      "tmp[181] = P[30]*tmp[98] + tmp[102]*tmp[28] + tmp[172]*tmp[95] + tmp[173]*tmp[96] + tmp[174]*tmp[97] + tmp[175]*tmp[99];\n",
      "tmp[182] = P[44]*tmp[126];\n",
      "tmp[183] = P[43]*ekf_use_theta;\n",
      "tmp[184] = P[42]*ekf_use_phi;\n",
      "tmp[185] = P[37]*tmp[3];\n",
      "tmp[186] = P[36]*tmp[2];\n",
      "tmp[187] = P[38]*tmp[42] + tmp[182] - tmp[183]*tmp[38] + tmp[184]*tmp[40] + tmp[185]*tmp[44] + tmp[186]*tmp[47];\n",
      "tmp[188] = P[38]*tmp[53] - tmp[182]*tmp[37] + tmp[183]*tmp[51] + tmp[184]*tmp[52] + tmp[185]*tmp[54] + tmp[186]*tmp[56];\n",
      "tmp[189] = P[38]*tmp[64] + tmp[182]*tmp[60] + tmp[183]*tmp[62] + tmp[184]*tmp[63] + tmp[185]*tmp[65] + tmp[186]*tmp[67];\n",
      "tmp[190] = P[38]*tmp[74] + tmp[182]*tmp[71] + tmp[183]*tmp[72] + tmp[184]*tmp[73] + tmp[185]*tmp[75] + tmp[186]*tmp[77];\n",
      "tmp[191] = P[38]*tmp[85] + tmp[182]*tmp[82] + tmp[183]*tmp[83] + tmp[184]*tmp[84] + tmp[185]*tmp[86] + tmp[186]*tmp[88];\n",
      "tmp[192] = P[38]*tmp[98] + tmp[102]*tmp[186] + tmp[182]*tmp[95] + tmp[183]*tmp[96] + tmp[184]*tmp[97] + tmp[185]*tmp[99];\n",
      "tmp[193] = P[53]*tmp[126];\n",
      "tmp[194] = P[52]*ekf_use_theta;\n",
      "tmp[195] = P[51]*ekf_use_phi;\n",
      "tmp[196] = P[46]*tmp[3];\n",
      "tmp[197] = P[45]*tmp[2];\n",
      "tmp[198] = P[47]*tmp[42] + tmp[193] - tmp[194]*tmp[38] + tmp[195]*tmp[40] + tmp[196]*tmp[44] + tmp[197]*tmp[47];\n",
      "tmp[199] = P[47]*tmp[53] - tmp[193]*tmp[37] + tmp[194]*tmp[51] + tmp[195]*tmp[52] + tmp[196]*tmp[54] + tmp[197]*tmp[56];\n",
      "tmp[200] = P[47]*tmp[64] + tmp[193]*tmp[60] + tmp[194]*tmp[62] + tmp[195]*tmp[63] + tmp[196]*tmp[65] + tmp[197]*tmp[67];\n",
      "tmp[201] = P[47]*tmp[74] + tmp[193]*tmp[71] + tmp[194]*tmp[72] + tmp[195]*tmp[73] + tmp[196]*tmp[75] + tmp[197]*tmp[77];\n",
      "tmp[202] = P[47]*tmp[85] + tmp[193]*tmp[82] + tmp[194]*tmp[83] + tmp[195]*tmp[84] + tmp[196]*tmp[86] + tmp[197]*tmp[88];\n",
      "tmp[203] = P[47]*tmp[98] + tmp[102]*tmp[197] + tmp[193]*tmp[95] + tmp[194]*tmp[96] + tmp[195]*tmp[97] + tmp[196]*tmp[99];\n",
      "tmp[204] = P[63]*tmp[126];\n",
      "tmp[205] = P[62]*ekf_use_theta;\n",
      "tmp[206] = P[61]*ekf_use_phi;\n",
      "tmp[207] = P[56]*tmp[3];\n",
      "tmp[208] = P[55]*tmp[2];\n",
      "tmp[209] = P[57]*tmp[42] + tmp[204] - tmp[205]*tmp[38] + tmp[206]*tmp[40] + tmp[207]*tmp[44] + tmp[208]*tmp[47];\n",
      "tmp[210] = P[57]*tmp[53] - tmp[204]*tmp[37] + tmp[205]*tmp[51] + tmp[206]*tmp[52] + tmp[207]*tmp[54] + tmp[208]*tmp[56];\n",
      "tmp[211] = P[57]*tmp[64] + tmp[204]*tmp[60] + tmp[205]*tmp[62] + tmp[206]*tmp[63] + tmp[207]*tmp[65] + tmp[208]*tmp[67];\n",
      "tmp[212] = P[57]*tmp[74] + tmp[204]*tmp[71] + tmp[205]*tmp[72] + tmp[206]*tmp[73] + tmp[207]*tmp[75] + tmp[208]*tmp[77];\n",
      "tmp[213] = P[57]*tmp[85] + tmp[204]*tmp[82] + tmp[205]*tmp[83] + tmp[206]*tmp[84] + tmp[207]*tmp[86] + tmp[208]*tmp[88];\n",
      "tmp[214] = P[57]*tmp[98] + tmp[102]*tmp[208] + tmp[204]*tmp[95] + tmp[205]*tmp[96] + tmp[206]*tmp[97] + tmp[207]*tmp[99];\n",
      "tmp[215] = P[74]*tmp[126];\n",
      "tmp[216] = P[73]*ekf_use_theta;\n",
      "tmp[217] = P[72]*ekf_use_phi;\n",
      "tmp[218] = P[67]*tmp[3];\n",
      "tmp[219] = P[66]*tmp[2];\n",
      "tmp[220] = P[68]*tmp[42] + tmp[215] - tmp[216]*tmp[38] + tmp[217]*tmp[40] + tmp[218]*tmp[44] + tmp[219]*tmp[47];\n",
      "tmp[221] = P[68]*tmp[53] - tmp[215]*tmp[37] + tmp[216]*tmp[51] + tmp[217]*tmp[52] + tmp[218]*tmp[54] + tmp[219]*tmp[56];\n",
      "tmp[222] = P[68]*tmp[64] + tmp[215]*tmp[60] + tmp[216]*tmp[62] + tmp[217]*tmp[63] + tmp[218]*tmp[65] + tmp[219]*tmp[67];\n",
      "tmp[223] = P[68]*tmp[74] + tmp[215]*tmp[71] + tmp[216]*tmp[72] + tmp[217]*tmp[73] + tmp[218]*tmp[75] + tmp[219]*tmp[77];\n",
      "tmp[224] = P[68]*tmp[85] + tmp[215]*tmp[82] + tmp[216]*tmp[83] + tmp[217]*tmp[84] + tmp[218]*tmp[86] + tmp[219]*tmp[88];\n",
      "tmp[225] = P[68]*tmp[98] + tmp[102]*tmp[219] + tmp[215]*tmp[95] + tmp[216]*tmp[96] + tmp[217]*tmp[97] + tmp[218]*tmp[99];\n",
      "tmp[226] = P[86]*tmp[126];\n",
      "tmp[227] = P[85]*ekf_use_theta;\n",
      "tmp[228] = P[84]*ekf_use_phi;\n",
      "tmp[229] = P[79]*tmp[3];\n",
      "tmp[230] = P[78]*tmp[2];\n",
      "tmp[231] = P[80]*tmp[42] + tmp[226] - tmp[227]*tmp[38] + tmp[228]*tmp[40] + tmp[229]*tmp[44] + tmp[230]*tmp[47];\n",
      "tmp[232] = P[80]*tmp[53] - tmp[226]*tmp[37] + tmp[227]*tmp[51] + tmp[228]*tmp[52] + tmp[229]*tmp[54] + tmp[230]*tmp[56];\n",
      "tmp[233] = P[80]*tmp[64] + tmp[226]*tmp[60] + tmp[227]*tmp[62] + tmp[228]*tmp[63] + tmp[229]*tmp[65] + tmp[230]*tmp[67];\n",
      "tmp[234] = P[80]*tmp[74] + tmp[226]*tmp[71] + tmp[227]*tmp[72] + tmp[228]*tmp[73] + tmp[229]*tmp[75] + tmp[230]*tmp[77];\n",
      "tmp[235] = P[80]*tmp[85] + tmp[226]*tmp[82] + tmp[227]*tmp[83] + tmp[228]*tmp[84] + tmp[229]*tmp[86] + tmp[230]*tmp[88];\n",
      "tmp[236] = P[80]*tmp[98] + tmp[102]*tmp[230] + tmp[226]*tmp[95] + tmp[227]*tmp[96] + tmp[228]*tmp[97] + tmp[229]*tmp[99];\n",
      "tmp[237] = P[99]*tmp[126];\n",
      "tmp[238] = P[98]*ekf_use_theta;\n",
      "tmp[239] = P[97]*ekf_use_phi;\n",
      "tmp[240] = P[92]*tmp[3];\n",
      "tmp[241] = P[91]*tmp[2];\n",
      "tmp[242] = P[93]*tmp[42] + tmp[237] - tmp[238]*tmp[38] + tmp[239]*tmp[40] + tmp[240]*tmp[44] + tmp[241]*tmp[47];\n",
      "tmp[243] = P[93]*tmp[53] - tmp[237]*tmp[37] + tmp[238]*tmp[51] + tmp[239]*tmp[52] + tmp[240]*tmp[54] + tmp[241]*tmp[56];\n",
      "tmp[244] = P[93]*tmp[64] + tmp[237]*tmp[60] + tmp[238]*tmp[62] + tmp[239]*tmp[63] + tmp[240]*tmp[65] + tmp[241]*tmp[67];\n",
      "tmp[245] = P[93]*tmp[74] + tmp[237]*tmp[71] + tmp[238]*tmp[72] + tmp[239]*tmp[73] + tmp[240]*tmp[75] + tmp[241]*tmp[77];\n",
      "tmp[246] = P[93]*tmp[85] + tmp[237]*tmp[82] + tmp[238]*tmp[83] + tmp[239]*tmp[84] + tmp[240]*tmp[86] + tmp[241]*tmp[88];\n",
      "tmp[247] = P[93]*tmp[98] + tmp[102]*tmp[241] + tmp[237]*tmp[95] + tmp[238]*tmp[96] + tmp[239]*tmp[97] + tmp[240]*tmp[99];\n",
      "tmp[248] = P[113]*tmp[126];\n",
      "tmp[249] = P[112]*ekf_use_theta;\n",
      "tmp[250] = P[111]*ekf_use_phi;\n",
      "tmp[251] = P[106]*tmp[3];\n",
      "tmp[252] = P[105]*tmp[2];\n",
      "tmp[253] = P[107]*tmp[42] + tmp[248] - tmp[249]*tmp[38] + tmp[250]*tmp[40] + tmp[251]*tmp[44] + tmp[252]*tmp[47];\n",
      "tmp[254] = P[107]*tmp[53] - tmp[248]*tmp[37] + tmp[249]*tmp[51] + tmp[250]*tmp[52] + tmp[251]*tmp[54] + tmp[252]*tmp[56];\n",
      "tmp[255] = P[107]*tmp[64] + tmp[248]*tmp[60] + tmp[249]*tmp[62] + tmp[250]*tmp[63] + tmp[251]*tmp[65] + tmp[252]*tmp[67];\n",
      "tmp[256] = P[107]*tmp[74] + tmp[248]*tmp[71] + tmp[249]*tmp[72] + tmp[250]*tmp[73] + tmp[251]*tmp[75] + tmp[252]*tmp[77];\n",
      "tmp[257] = P[107]*tmp[85] + tmp[248]*tmp[82] + tmp[249]*tmp[83] + tmp[250]*tmp[84] + tmp[251]*tmp[86] + tmp[252]*tmp[88];\n",
      "tmp[258] = P[107]*tmp[98] + tmp[102]*tmp[252] + tmp[248]*tmp[95] + tmp[249]*tmp[96] + tmp[250]*tmp[97] + tmp[251]*tmp[99];\n",
      "tmp[259] = -tmp[112];\n",
      "tmp[260] = 1 - tmp[113];\n",
      "tmp[261] = -tmp[114];\n",
      "tmp[262] = 1 - tmp[123];\n",
      "tmp[263] = -tmp[124];\n",
      "tmp[264] = -tmp[125];\n",
      "tmp[265] = -tmp[135];\n",
      "tmp[266] = -tmp[136];\n",
      "tmp[267] = -tmp[137];\n",
      "tmp[268] = P[39]*ekf_use_psi;\n",
      "tmp[269] = -tmp[146];\n",
      "tmp[270] = -tmp[147];\n",
      "tmp[271] = -tmp[148];\n",
      "tmp[272] = P[40]*ekf_use_psi;\n",
      "tmp[273] = -tmp[157];\n",
      "tmp[274] = -tmp[158];\n",
      "tmp[275] = -tmp[159];\n",
      "tmp[276] = P[41]*ekf_use_psi;\n",
      "tmp[277] = -ekf_use_phi*tmp[167] + 1;\n",
      "tmp[278] = -tmp[168];\n",
      "tmp[279] = -tmp[169];\n",
      "tmp[280] = -tmp[170];\n",
      "tmp[281] = -ekf_use_theta*tmp[177] + 1;\n",
      "tmp[282] = -tmp[179];\n",
      "tmp[283] = -tmp[180];\n",
      "tmp[284] = -tmp[181];\n",
      "tmp[285] = ekf_use_phi*tmp[178];\n",
      "tmp[286] = -ekf_use_psi*tmp[187] + 1;\n",
      "tmp[287] = -tmp[190];\n",
      "tmp[288] = -tmp[191];\n",
      "tmp[289] = -tmp[192];\n",
      "tmp[290] = ekf_use_phi*tmp[189];\n",
      "tmp[291] = ekf_use_theta*tmp[188];\n",
      "tmp[292] = -tmp[201];\n",
      "tmp[293] = -tmp[202];\n",
      "tmp[294] = -tmp[203];\n",
      "tmp[295] = ekf_use_phi*tmp[200];\n",
      "tmp[296] = ekf_use_theta*tmp[199];\n",
      "tmp[297] = ekf_use_psi*tmp[198];\n",
      "tmp[298] = -tmp[212];\n",
      "tmp[299] = -tmp[213];\n",
      "tmp[300] = -tmp[214];\n",
      "tmp[301] = ekf_use_phi*tmp[211];\n",
      "tmp[302] = ekf_use_theta*tmp[210];\n",
      "tmp[303] = ekf_use_psi*tmp[209];\n",
      "tmp[304] = -tmp[223];\n",
      "tmp[305] = -tmp[224];\n",
      "tmp[306] = -tmp[225];\n",
      "tmp[307] = ekf_use_phi*tmp[222];\n",
      "tmp[308] = ekf_use_theta*tmp[221];\n",
      "tmp[309] = ekf_use_psi*tmp[220];\n",
      "tmp[310] = -tmp[234];\n",
      "tmp[311] = -tmp[235];\n",
      "tmp[312] = -tmp[236];\n",
      "tmp[313] = ekf_use_phi*tmp[233];\n",
      "tmp[314] = ekf_use_theta*tmp[232];\n",
      "tmp[315] = ekf_use_psi*tmp[231];\n",
      "tmp[316] = -tmp[245];\n",
      "tmp[317] = -tmp[246];\n",
      "tmp[318] = -tmp[247];\n",
      "tmp[319] = ekf_use_phi*tmp[244];\n",
      "tmp[320] = ekf_use_theta*tmp[243];\n",
      "tmp[321] = ekf_use_psi*tmp[242];\n",
      "tmp[322] = -tmp[256];\n",
      "tmp[323] = -tmp[257];\n",
      "tmp[324] = -tmp[258];\n",
      "tmp[325] = ekf_use_phi*tmp[255];\n",
      "tmp[326] = ekf_use_theta*tmp[254];\n",
      "tmp[327] = ekf_use_psi*tmp[253];\n",
      "X_new[0] = X[0] + tmp[0]*tmp[49] + tmp[50]*tmp[57] + tmp[58]*tmp[68] + tmp[69]*tmp[78] + tmp[79]*tmp[89] + tmp[90]*(tmp[100] + tmp[103]);\n",
      "X_new[1] = X[1] + tmp[0]*tmp[109] + tmp[110]*tmp[50] + tmp[111]*tmp[58] + tmp[112]*tmp[69] + tmp[113]*tmp[79] + tmp[114]*tmp[90];\n",
      "X_new[2] = X[2] + tmp[0]*tmp[120] + tmp[121]*tmp[50] + tmp[122]*tmp[58] + tmp[123]*tmp[69] + tmp[124]*tmp[79] + tmp[125]*tmp[90];\n",
      "X_new[3] = X[3] + tmp[0]*tmp[132] + tmp[133]*tmp[50] + tmp[134]*tmp[58] + tmp[135]*tmp[69] + tmp[136]*tmp[79] + tmp[137]*tmp[90];\n",
      "X_new[4] = X[4] + tmp[0]*tmp[143] + tmp[144]*tmp[50] + tmp[145]*tmp[58] + tmp[146]*tmp[69] + tmp[147]*tmp[79] + tmp[148]*tmp[90];\n",
      "X_new[5] = X[5] + tmp[0]*tmp[154] + tmp[155]*tmp[50] + tmp[156]*tmp[58] + tmp[157]*tmp[69] + tmp[158]*tmp[79] + tmp[159]*tmp[90];\n",
      "X_new[6] = X[6] + tmp[0]*tmp[165] + tmp[166]*tmp[50] + tmp[167]*tmp[58] + tmp[168]*tmp[69] + tmp[169]*tmp[79] + tmp[170]*tmp[90];\n",
      "X_new[7] = X[7] + tmp[0]*tmp[176] + tmp[177]*tmp[50] + tmp[178]*tmp[58] + tmp[179]*tmp[69] + tmp[180]*tmp[79] + tmp[181]*tmp[90];\n",
      "X_new[8] = X[8] + tmp[0]*tmp[187] + tmp[188]*tmp[50] + tmp[189]*tmp[58] + tmp[190]*tmp[69] + tmp[191]*tmp[79] + tmp[192]*tmp[90];\n",
      "X_new[9] = X[9] + tmp[0]*tmp[198] + tmp[199]*tmp[50] + tmp[200]*tmp[58] + tmp[201]*tmp[69] + tmp[202]*tmp[79] + tmp[203]*tmp[90];\n",
      "X_new[10] = X[10] + tmp[0]*tmp[209] + tmp[210]*tmp[50] + tmp[211]*tmp[58] + tmp[212]*tmp[69] + tmp[213]*tmp[79] + tmp[214]*tmp[90];\n",
      "X_new[11] = X[11] + tmp[0]*tmp[220] + tmp[221]*tmp[50] + tmp[222]*tmp[58] + tmp[223]*tmp[69] + tmp[224]*tmp[79] + tmp[225]*tmp[90];\n",
      "X_new[12] = X[12] + tmp[0]*tmp[231] + tmp[232]*tmp[50] + tmp[233]*tmp[58] + tmp[234]*tmp[69] + tmp[235]*tmp[79] + tmp[236]*tmp[90];\n",
      "X_new[13] = X[13] + tmp[0]*tmp[242] + tmp[243]*tmp[50] + tmp[244]*tmp[58] + tmp[245]*tmp[69] + tmp[246]*tmp[79] + tmp[247]*tmp[90];\n",
      "X_new[14] = X[14] + tmp[0]*tmp[253] + tmp[254]*tmp[50] + tmp[255]*tmp[58] + tmp[256]*tmp[69] + tmp[257]*tmp[79] + tmp[258]*tmp[90];\n",
      "P_new[0] = P[0]*(-tmp[101] - tmp[103]) - P[1]*tmp[89] - P[3]*tmp[78] - tmp[13]*tmp[68] - tmp[23]*tmp[57] - tmp[49]*tmp[5];\n",
      "P_new[1] = P[0]*tmp[261] + P[1]*tmp[260] + P[3]*tmp[259] - tmp[109]*tmp[5] - tmp[110]*tmp[23] - tmp[111]*tmp[13];\n",
      "P_new[2] = P[1]*tmp[261] + P[2]*tmp[260] + P[4]*tmp[259] - tmp[104]*tmp[109] - tmp[106]*tmp[110] - tmp[107]*tmp[111];\n",
      "P_new[3] = P[0]*tmp[264] + P[1]*tmp[263] + P[3]*tmp[262] - tmp[120]*tmp[5] - tmp[121]*tmp[23] - tmp[122]*tmp[13];\n",
      "P_new[4] = P[1]*tmp[264] + P[2]*tmp[263] + P[4]*tmp[262] - tmp[104]*tmp[120] - tmp[106]*tmp[121] - tmp[107]*tmp[122];\n",
      "P_new[5] = P[3]*tmp[264] + P[4]*tmp[263] + P[5]*tmp[262] - tmp[115]*tmp[120] - tmp[117]*tmp[121] - tmp[118]*tmp[122];\n",
      "P_new[6] = P[0]*tmp[267] + P[1]*tmp[266] + P[3]*tmp[265] + P[6] - tmp[132]*tmp[5] - tmp[133]*tmp[23] - tmp[134]*tmp[13];\n",
      "P_new[7] = P[1]*tmp[267] + P[2]*tmp[266] + P[4]*tmp[265] + P[7] - tmp[104]*tmp[132] - tmp[106]*tmp[133] - tmp[107]*tmp[134];\n",
      "P_new[8] = P[3]*tmp[267] + P[4]*tmp[266] + P[5]*tmp[265] + P[8] - tmp[115]*tmp[132] - tmp[117]*tmp[133] - tmp[118]*tmp[134];\n",
      "P_new[9] = P[6]*tmp[267] + P[7]*tmp[266] + P[8]*tmp[265] + P[9] - tmp[128]*tmp[133] - tmp[129]*tmp[134] - tmp[132]*tmp[268];\n",
      "P_new[10] = P[0]*tmp[271] + P[10] + P[1]*tmp[270] + P[3]*tmp[269] - tmp[13]*tmp[145] - tmp[143]*tmp[5] - tmp[144]*tmp[23];\n",
      "P_new[11] = P[11] + P[1]*tmp[271] + P[2]*tmp[270] + P[4]*tmp[269] - tmp[104]*tmp[143] - tmp[106]*tmp[144] - tmp[107]*tmp[145];\n",
      "P_new[12] = P[12] + P[3]*tmp[271] + P[4]*tmp[270] + P[5]*tmp[269] - tmp[115]*tmp[143] - tmp[117]*tmp[144] - tmp[118]*tmp[145];\n",
      "P_new[13] = P[13] + P[6]*tmp[271] + P[7]*tmp[270] + P[8]*tmp[269] - tmp[128]*tmp[144] - tmp[129]*tmp[145] - tmp[143]*tmp[268];\n",
      "P_new[14] = P[10]*tmp[271] + P[11]*tmp[270] + P[12]*tmp[269] + P[14] - tmp[139]*tmp[144] - tmp[140]*tmp[145] - tmp[143]*tmp[272];\n",
      "P_new[15] = P[0]*tmp[275] + P[15] + P[1]*tmp[274] + P[3]*tmp[273] - tmp[13]*tmp[156] - tmp[154]*tmp[5] - tmp[155]*tmp[23];\n",
      "P_new[16] = P[16] + P[1]*tmp[275] + P[2]*tmp[274] + P[4]*tmp[273] - tmp[104]*tmp[154] - tmp[106]*tmp[155] - tmp[107]*tmp[156];\n",
      "P_new[17] = P[17] + P[3]*tmp[275] + P[4]*tmp[274] + P[5]*tmp[273] - tmp[115]*tmp[154] - tmp[117]*tmp[155] - tmp[118]*tmp[156];\n",
      "P_new[18] = P[18] + P[6]*tmp[275] + P[7]*tmp[274] + P[8]*tmp[273] - tmp[128]*tmp[155] - tmp[129]*tmp[156] - tmp[154]*tmp[268];\n",
      "P_new[19] = P[10]*tmp[275] + P[11]*tmp[274] + P[12]*tmp[273] + P[19] - tmp[139]*tmp[155] - tmp[140]*tmp[156] - tmp[154]*tmp[272];\n",
      "P_new[20] = P[15]*tmp[275] + P[16]*tmp[274] + P[17]*tmp[273] + P[20] - tmp[150]*tmp[155] - tmp[151]*tmp[156] - tmp[154]*tmp[276];\n",
      "P_new[21] = P[0]*tmp[280] + P[1]*tmp[279] + P[21]*tmp[277] + P[3]*tmp[278] - tmp[165]*tmp[5] - tmp[166]*tmp[23];\n",
      "P_new[22] = P[1]*tmp[280] + P[22]*tmp[277] + P[2]*tmp[279] + P[4]*tmp[278] - tmp[104]*tmp[165] - tmp[106]*tmp[166];\n",
      "P_new[23] = P[23]*tmp[277] + P[3]*tmp[280] + P[4]*tmp[279] + P[5]*tmp[278] - tmp[115]*tmp[165] - tmp[117]*tmp[166];\n",
      "P_new[24] = P[24]*tmp[277] + P[6]*tmp[280] + P[7]*tmp[279] + P[8]*tmp[278] - tmp[128]*tmp[166] - tmp[165]*tmp[268];\n",
      "P_new[25] = P[10]*tmp[280] + P[11]*tmp[279] + P[12]*tmp[278] + P[25]*tmp[277] - tmp[139]*tmp[166] - tmp[165]*tmp[272];\n",
      "P_new[26] = P[15]*tmp[280] + P[16]*tmp[279] + P[17]*tmp[278] + P[26]*tmp[277] - tmp[150]*tmp[166] - tmp[165]*tmp[276];\n",
      "P_new[27] = P[21]*tmp[280] + P[22]*tmp[279] + P[23]*tmp[278] + P[27]*tmp[277] - tmp[160]*tmp[165] - tmp[162]*tmp[166];\n",
      "P_new[28] = P[0]*tmp[284] + P[1]*tmp[283] + P[28]*tmp[281] + P[3]*tmp[282] - tmp[13]*tmp[178] - tmp[176]*tmp[5];\n",
      "P_new[29] = P[1]*tmp[284] + P[29]*tmp[281] + P[2]*tmp[283] + P[4]*tmp[282] - tmp[104]*tmp[176] - tmp[107]*tmp[178];\n",
      "P_new[30] = P[30]*tmp[281] + P[3]*tmp[284] + P[4]*tmp[283] + P[5]*tmp[282] - tmp[115]*tmp[176] - tmp[118]*tmp[178];\n",
      "P_new[31] = P[31]*tmp[281] + P[6]*tmp[284] + P[7]*tmp[283] + P[8]*tmp[282] - tmp[129]*tmp[178] - tmp[176]*tmp[268];\n",
      "P_new[32] = P[10]*tmp[284] + P[11]*tmp[283] + P[12]*tmp[282] + P[32]*tmp[281] - tmp[140]*tmp[178] - tmp[176]*tmp[272];\n",
      "P_new[33] = P[15]*tmp[284] + P[16]*tmp[283] + P[17]*tmp[282] + P[33]*tmp[281] - tmp[151]*tmp[178] - tmp[176]*tmp[276];\n",
      "P_new[34] = P[21]*tmp[284] + P[22]*tmp[283] + P[23]*tmp[282] - P[27]*tmp[285] + P[34]*tmp[281] - tmp[160]*tmp[176];\n",
      "P_new[35] = P[28]*tmp[284] + P[29]*tmp[283] + P[30]*tmp[282] - P[34]*tmp[285] + P[35]*tmp[281] - tmp[171]*tmp[176];\n",
      "P_new[36] = P[0]*tmp[289] + P[1]*tmp[288] + P[36]*tmp[286] + P[3]*tmp[287] - tmp[13]*tmp[189] - tmp[188]*tmp[23];\n",
      "P_new[37] = P[1]*tmp[289] + P[2]*tmp[288] + P[37]*tmp[286] + P[4]*tmp[287] - tmp[106]*tmp[188] - tmp[107]*tmp[189];\n",
      "P_new[38] = P[38]*tmp[286] + P[3]*tmp[289] + P[4]*tmp[288] + P[5]*tmp[287] - tmp[117]*tmp[188] - tmp[118]*tmp[189];\n",
      "P_new[39] = P[39]*tmp[286] + P[6]*tmp[289] + P[7]*tmp[288] + P[8]*tmp[287] - tmp[128]*tmp[188] - tmp[129]*tmp[189];\n",
      "P_new[40] = P[10]*tmp[289] + P[11]*tmp[288] + P[12]*tmp[287] + P[40]*tmp[286] - tmp[139]*tmp[188] - tmp[140]*tmp[189];\n",
      "P_new[41] = P[15]*tmp[289] + P[16]*tmp[288] + P[17]*tmp[287] + P[41]*tmp[286] - tmp[150]*tmp[188] - tmp[151]*tmp[189];\n",
      "P_new[42] = P[21]*tmp[289] + P[22]*tmp[288] + P[23]*tmp[287] - P[27]*tmp[290] + P[42]*tmp[286] - tmp[162]*tmp[188];\n",
      "P_new[43] = P[28]*tmp[289] + P[29]*tmp[288] + P[30]*tmp[287] - P[34]*tmp[290] - P[35]*tmp[291] + P[43]*tmp[286];\n",
      "P_new[44] = P[36]*tmp[289] + P[37]*tmp[288] + P[38]*tmp[287] - P[42]*tmp[290] - P[43]*tmp[291] + P[44]*tmp[286];\n",
      "P_new[45] = P[0]*tmp[294] + P[1]*tmp[293] + P[3]*tmp[292] + P[45] - tmp[13]*tmp[200] - tmp[198]*tmp[5] - tmp[199]*tmp[23];\n",
      "P_new[46] = P[1]*tmp[294] + P[2]*tmp[293] + P[46] + P[4]*tmp[292] - tmp[104]*tmp[198] - tmp[106]*tmp[199] - tmp[107]*tmp[200];\n",
      "P_new[47] = P[3]*tmp[294] + P[47] + P[4]*tmp[293] + P[5]*tmp[292] - tmp[115]*tmp[198] - tmp[117]*tmp[199] - tmp[118]*tmp[200];\n",
      "P_new[48] = P[48] + P[6]*tmp[294] + P[7]*tmp[293] + P[8]*tmp[292] - tmp[128]*tmp[199] - tmp[129]*tmp[200] - tmp[198]*tmp[268];\n",
      "P_new[49] = P[10]*tmp[294] + P[11]*tmp[293] + P[12]*tmp[292] + P[49] - tmp[139]*tmp[199] - tmp[140]*tmp[200] - tmp[198]*tmp[272];\n",
      "P_new[50] = P[15]*tmp[294] + P[16]*tmp[293] + P[17]*tmp[292] + P[50] - tmp[150]*tmp[199] - tmp[151]*tmp[200] - tmp[198]*tmp[276];\n",
      "P_new[51] = P[21]*tmp[294] + P[22]*tmp[293] + P[23]*tmp[292] - P[27]*tmp[295] + P[51] - tmp[160]*tmp[198] - tmp[162]*tmp[199];\n",
      "P_new[52] = P[28]*tmp[294] + P[29]*tmp[293] + P[30]*tmp[292] - P[34]*tmp[295] - P[35]*tmp[296] + P[52] - tmp[171]*tmp[198];\n",
      "P_new[53] = P[36]*tmp[294] + P[37]*tmp[293] + P[38]*tmp[292] - P[42]*tmp[295] - P[43]*tmp[296] - P[44]*tmp[297] + P[53];\n",
      "P_new[54] = P[45]*tmp[294] + P[46]*tmp[293] + P[47]*tmp[292] - P[51]*tmp[295] - P[52]*tmp[296] - P[53]*tmp[297] + P[54];\n",
      "P_new[55] = P[0]*tmp[300] + P[1]*tmp[299] + P[3]*tmp[298] + P[55] - tmp[13]*tmp[211] - tmp[209]*tmp[5] - tmp[210]*tmp[23];\n",
      "P_new[56] = P[1]*tmp[300] + P[2]*tmp[299] + P[4]*tmp[298] + P[56] - tmp[104]*tmp[209] - tmp[106]*tmp[210] - tmp[107]*tmp[211];\n",
      "P_new[57] = P[3]*tmp[300] + P[4]*tmp[299] + P[57] + P[5]*tmp[298] - tmp[115]*tmp[209] - tmp[117]*tmp[210] - tmp[118]*tmp[211];\n",
      "P_new[58] = P[58] + P[6]*tmp[300] + P[7]*tmp[299] + P[8]*tmp[298] - tmp[128]*tmp[210] - tmp[129]*tmp[211] - tmp[209]*tmp[268];\n",
      "P_new[59] = P[10]*tmp[300] + P[11]*tmp[299] + P[12]*tmp[298] + P[59] - tmp[139]*tmp[210] - tmp[140]*tmp[211] - tmp[209]*tmp[272];\n",
      "P_new[60] = P[15]*tmp[300] + P[16]*tmp[299] + P[17]*tmp[298] + P[60] - tmp[150]*tmp[210] - tmp[151]*tmp[211] - tmp[209]*tmp[276];\n",
      "P_new[61] = P[21]*tmp[300] + P[22]*tmp[299] + P[23]*tmp[298] - P[27]*tmp[301] + P[61] - tmp[160]*tmp[209] - tmp[162]*tmp[210];\n",
      "P_new[62] = P[28]*tmp[300] + P[29]*tmp[299] + P[30]*tmp[298] - P[34]*tmp[301] - P[35]*tmp[302] + P[62] - tmp[171]*tmp[209];\n",
      "P_new[63] = P[36]*tmp[300] + P[37]*tmp[299] + P[38]*tmp[298] - P[42]*tmp[301] - P[43]*tmp[302] - P[44]*tmp[303] + P[63];\n",
      "P_new[64] = P[45]*tmp[300] + P[46]*tmp[299] + P[47]*tmp[298] - P[51]*tmp[301] - P[52]*tmp[302] - P[53]*tmp[303] + P[64];\n",
      "P_new[65] = P[55]*tmp[300] + P[56]*tmp[299] + P[57]*tmp[298] - P[61]*tmp[301] - P[62]*tmp[302] - P[63]*tmp[303] + P[65];\n",
      "P_new[66] = P[0]*tmp[306] + P[1]*tmp[305] + P[3]*tmp[304] + P[66] - tmp[13]*tmp[222] - tmp[220]*tmp[5] - tmp[221]*tmp[23];\n",
      "P_new[67] = P[1]*tmp[306] + P[2]*tmp[305] + P[4]*tmp[304] + P[67] - tmp[104]*tmp[220] - tmp[106]*tmp[221] - tmp[107]*tmp[222];\n",
      "P_new[68] = P[3]*tmp[306] + P[4]*tmp[305] + P[5]*tmp[304] + P[68] - tmp[115]*tmp[220] - tmp[117]*tmp[221] - tmp[118]*tmp[222];\n",
      "P_new[69] = P[69] + P[6]*tmp[306] + P[7]*tmp[305] + P[8]*tmp[304] - tmp[128]*tmp[221] - tmp[129]*tmp[222] - tmp[220]*tmp[268];\n",
      "P_new[70] = P[10]*tmp[306] + P[11]*tmp[305] + P[12]*tmp[304] + P[70] - tmp[139]*tmp[221] - tmp[140]*tmp[222] - tmp[220]*tmp[272];\n",
      "P_new[71] = P[15]*tmp[306] + P[16]*tmp[305] + P[17]*tmp[304] + P[71] - tmp[150]*tmp[221] - tmp[151]*tmp[222] - tmp[220]*tmp[276];\n",
      "P_new[72] = P[21]*tmp[306] + P[22]*tmp[305] + P[23]*tmp[304] - P[27]*tmp[307] + P[72] - tmp[160]*tmp[220] - tmp[162]*tmp[221];\n",
      "P_new[73] = P[28]*tmp[306] + P[29]*tmp[305] + P[30]*tmp[304] - P[34]*tmp[307] - P[35]*tmp[308] + P[73] - tmp[171]*tmp[220];\n",
      "P_new[74] = P[36]*tmp[306] + P[37]*tmp[305] + P[38]*tmp[304] - P[42]*tmp[307] - P[43]*tmp[308] - P[44]*tmp[309] + P[74];\n",
      "P_new[75] = P[45]*tmp[306] + P[46]*tmp[305] + P[47]*tmp[304] - P[51]*tmp[307] - P[52]*tmp[308] - P[53]*tmp[309] + P[75];\n",
      "P_new[76] = P[55]*tmp[306] + P[56]*tmp[305] + P[57]*tmp[304] - P[61]*tmp[307] - P[62]*tmp[308] - P[63]*tmp[309] + P[76];\n",
      "P_new[77] = P[66]*tmp[306] + P[67]*tmp[305] + P[68]*tmp[304] - P[72]*tmp[307] - P[73]*tmp[308] - P[74]*tmp[309] + P[77];\n",
      "P_new[78] = P[0]*tmp[312] + P[1]*tmp[311] + P[3]*tmp[310] + P[78] - tmp[13]*tmp[233] - tmp[231]*tmp[5] - tmp[232]*tmp[23];\n",
      "P_new[79] = P[1]*tmp[312] + P[2]*tmp[311] + P[4]*tmp[310] + P[79] - tmp[104]*tmp[231] - tmp[106]*tmp[232] - tmp[107]*tmp[233];\n",
      "P_new[80] = P[3]*tmp[312] + P[4]*tmp[311] + P[5]*tmp[310] + P[80] - tmp[115]*tmp[231] - tmp[117]*tmp[232] - tmp[118]*tmp[233];\n",
      "P_new[81] = P[6]*tmp[312] + P[7]*tmp[311] + P[81] + P[8]*tmp[310] - tmp[128]*tmp[232] - tmp[129]*tmp[233] - tmp[231]*tmp[268];\n",
      "P_new[82] = P[10]*tmp[312] + P[11]*tmp[311] + P[12]*tmp[310] + P[82] - tmp[139]*tmp[232] - tmp[140]*tmp[233] - tmp[231]*tmp[272];\n",
      "P_new[83] = P[15]*tmp[312] + P[16]*tmp[311] + P[17]*tmp[310] + P[83] - tmp[150]*tmp[232] - tmp[151]*tmp[233] - tmp[231]*tmp[276];\n",
      "P_new[84] = P[21]*tmp[312] + P[22]*tmp[311] + P[23]*tmp[310] - P[27]*tmp[313] + P[84] - tmp[160]*tmp[231] - tmp[162]*tmp[232];\n",
      "P_new[85] = P[28]*tmp[312] + P[29]*tmp[311] + P[30]*tmp[310] - P[34]*tmp[313] - P[35]*tmp[314] + P[85] - tmp[171]*tmp[231];\n",
      "P_new[86] = P[36]*tmp[312] + P[37]*tmp[311] + P[38]*tmp[310] - P[42]*tmp[313] - P[43]*tmp[314] - P[44]*tmp[315] + P[86];\n",
      "P_new[87] = P[45]*tmp[312] + P[46]*tmp[311] + P[47]*tmp[310] - P[51]*tmp[313] - P[52]*tmp[314] - P[53]*tmp[315] + P[87];\n",
      "P_new[88] = P[55]*tmp[312] + P[56]*tmp[311] + P[57]*tmp[310] - P[61]*tmp[313] - P[62]*tmp[314] - P[63]*tmp[315] + P[88];\n",
      "P_new[89] = P[66]*tmp[312] + P[67]*tmp[311] + P[68]*tmp[310] - P[72]*tmp[313] - P[73]*tmp[314] - P[74]*tmp[315] + P[89];\n",
      "P_new[90] = P[78]*tmp[312] + P[79]*tmp[311] + P[80]*tmp[310] - P[84]*tmp[313] - P[85]*tmp[314] - P[86]*tmp[315] + P[90];\n",
      "P_new[91] = P[0]*tmp[318] + P[1]*tmp[317] + P[3]*tmp[316] + P[91] - tmp[13]*tmp[244] - tmp[23]*tmp[243] - tmp[242]*tmp[5];\n",
      "P_new[92] = P[1]*tmp[318] + P[2]*tmp[317] + P[4]*tmp[316] + P[92] - tmp[104]*tmp[242] - tmp[106]*tmp[243] - tmp[107]*tmp[244];\n",
      "P_new[93] = P[3]*tmp[318] + P[4]*tmp[317] + P[5]*tmp[316] + P[93] - tmp[115]*tmp[242] - tmp[117]*tmp[243] - tmp[118]*tmp[244];\n",
      "P_new[94] = P[6]*tmp[318] + P[7]*tmp[317] + P[8]*tmp[316] + P[94] - tmp[128]*tmp[243] - tmp[129]*tmp[244] - tmp[242]*tmp[268];\n",
      "P_new[95] = P[10]*tmp[318] + P[11]*tmp[317] + P[12]*tmp[316] + P[95] - tmp[139]*tmp[243] - tmp[140]*tmp[244] - tmp[242]*tmp[272];\n",
      "P_new[96] = P[15]*tmp[318] + P[16]*tmp[317] + P[17]*tmp[316] + P[96] - tmp[150]*tmp[243] - tmp[151]*tmp[244] - tmp[242]*tmp[276];\n",
      "P_new[97] = P[21]*tmp[318] + P[22]*tmp[317] + P[23]*tmp[316] - P[27]*tmp[319] + P[97] - tmp[160]*tmp[242] - tmp[162]*tmp[243];\n",
      "P_new[98] = P[28]*tmp[318] + P[29]*tmp[317] + P[30]*tmp[316] - P[34]*tmp[319] - P[35]*tmp[320] + P[98] - tmp[171]*tmp[242];\n",
      "P_new[99] = P[36]*tmp[318] + P[37]*tmp[317] + P[38]*tmp[316] - P[42]*tmp[319] - P[43]*tmp[320] - P[44]*tmp[321] + P[99];\n",
      "P_new[100] = P[100] + P[45]*tmp[318] + P[46]*tmp[317] + P[47]*tmp[316] - P[51]*tmp[319] - P[52]*tmp[320] - P[53]*tmp[321];\n",
      "P_new[101] = P[101] + P[55]*tmp[318] + P[56]*tmp[317] + P[57]*tmp[316] - P[61]*tmp[319] - P[62]*tmp[320] - P[63]*tmp[321];\n",
      "P_new[102] = P[102] + P[66]*tmp[318] + P[67]*tmp[317] + P[68]*tmp[316] - P[72]*tmp[319] - P[73]*tmp[320] - P[74]*tmp[321];\n",
      "P_new[103] = P[103] + P[78]*tmp[318] + P[79]*tmp[317] + P[80]*tmp[316] - P[84]*tmp[319] - P[85]*tmp[320] - P[86]*tmp[321];\n",
      "P_new[104] = P[104] + P[91]*tmp[318] + P[92]*tmp[317] + P[93]*tmp[316] - P[97]*tmp[319] - P[98]*tmp[320] - P[99]*tmp[321];\n",
      "P_new[105] = P[0]*tmp[324] + P[105] + P[1]*tmp[323] + P[3]*tmp[322] - tmp[13]*tmp[255] - tmp[23]*tmp[254] - tmp[253]*tmp[5];\n",
      "P_new[106] = P[106] + P[1]*tmp[324] + P[2]*tmp[323] + P[4]*tmp[322] - tmp[104]*tmp[253] - tmp[106]*tmp[254] - tmp[107]*tmp[255];\n",
      "P_new[107] = P[107] + P[3]*tmp[324] + P[4]*tmp[323] + P[5]*tmp[322] - tmp[115]*tmp[253] - tmp[117]*tmp[254] - tmp[118]*tmp[255];\n",
      "P_new[108] = P[108] + P[6]*tmp[324] + P[7]*tmp[323] + P[8]*tmp[322] - tmp[128]*tmp[254] - tmp[129]*tmp[255] - tmp[253]*tmp[268];\n",
      "P_new[109] = P[109] + P[10]*tmp[324] + P[11]*tmp[323] + P[12]*tmp[322] - tmp[139]*tmp[254] - tmp[140]*tmp[255] - tmp[253]*tmp[272];\n",
      "P_new[110] = P[110] + P[15]*tmp[324] + P[16]*tmp[323] + P[17]*tmp[322] - tmp[150]*tmp[254] - tmp[151]*tmp[255] - tmp[253]*tmp[276];\n",
      "P_new[111] = P[111] + P[21]*tmp[324] + P[22]*tmp[323] + P[23]*tmp[322] - P[27]*tmp[325] - tmp[160]*tmp[253] - tmp[162]*tmp[254];\n",
      "P_new[112] = P[112] + P[28]*tmp[324] + P[29]*tmp[323] + P[30]*tmp[322] - P[34]*tmp[325] - P[35]*tmp[326] - tmp[171]*tmp[253];\n",
      "P_new[113] = P[113] + P[36]*tmp[324] + P[37]*tmp[323] + P[38]*tmp[322] - P[42]*tmp[325] - P[43]*tmp[326] - P[44]*tmp[327];\n",
      "P_new[114] = P[114] + P[45]*tmp[324] + P[46]*tmp[323] + P[47]*tmp[322] - P[51]*tmp[325] - P[52]*tmp[326] - P[53]*tmp[327];\n",
      "P_new[115] = P[115] + P[55]*tmp[324] + P[56]*tmp[323] + P[57]*tmp[322] - P[61]*tmp[325] - P[62]*tmp[326] - P[63]*tmp[327];\n",
      "P_new[116] = P[116] + P[66]*tmp[324] + P[67]*tmp[323] + P[68]*tmp[322] - P[72]*tmp[325] - P[73]*tmp[326] - P[74]*tmp[327];\n",
      "P_new[117] = P[117] + P[78]*tmp[324] + P[79]*tmp[323] + P[80]*tmp[322] - P[84]*tmp[325] - P[85]*tmp[326] - P[86]*tmp[327];\n",
      "P_new[118] = P[118] + P[91]*tmp[324] + P[92]*tmp[323] + P[93]*tmp[322] - P[97]*tmp[325] - P[98]*tmp[326] - P[99]*tmp[327];\n",
      "P_new[119] = P[105]*tmp[324] + P[106]*tmp[323] + P[107]*tmp[322] - P[111]*tmp[325] - P[112]*tmp[326] - P[113]*tmp[327] + P[119];\n"
     ]
    }
   ],
   "source": [
    "# replace sin, cos, tan, pow with sinf, cosf, tanf, powf\n",
    "prediction_code = prediction_code.replace('sin(', 'sinf(')\n",
    "prediction_code = prediction_code.replace('cos(', 'cosf(')\n",
    "prediction_code = prediction_code.replace('tan(', 'tanf(')\n",
    "prediction_code = prediction_code.replace('pow(', 'powf(')\n",
    "update_code = update_code.replace('sin(', 'sinf(')\n",
    "update_code = update_code.replace('cos(', 'cosf(')\n",
    "update_code = update_code.replace('tan(', 'tanf(')\n",
    "update_code = update_code.replace('pow(', 'powf(')\n",
    "\n",
    "print('Prediction code:')\n",
    "print(prediction_code)\n",
    "print('Update code:')\n",
    "print(update_code)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Write C code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create c_code folder (if it doesn't exist)\n",
    "import os\n",
    "if not os.path.exists('c_code'):\n",
    "    os.makedirs('c_code')\n",
    "    \n",
    "header = f'''\n",
    "///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////\n",
    "// This file is automatically generated in \"Generate Kalman Filter C Code.ipynb\" from https://github.com/tudelft/kalman_filter   //\n",
    "///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////\n",
    "\n",
    "#ifndef EKF_CALC_H\n",
    "#define EKF_CALC_H\n",
    "\n",
    "#include <stdint.h>\n",
    "\n",
    "#define N_STATES {len(X)}\n",
    "#define N_INPUTS {len(U)}\n",
    "#define N_MEASUREMENTS {len(h)}\n",
    "\n",
    "// set to 1 to use phi, theta, psi measurements\n",
    "float ekf_use_phi;\n",
    "float ekf_use_theta;\n",
    "float ekf_use_psi;\n",
    "\n",
    "// getters\n",
    "float* ekf_get_X();     // get state vector\n",
    "float* ekf_get_P();     // get covariance matrix (lower diagonal)\n",
    "\n",
    "#define ekf_P_index(i,j) ((i>=j) ? ekf_get_P()[i*(i+1)/2+j] : ekf_get_P()[j*(j+1)/2+i])\n",
    "#define ekf_X_index(i) ekf_get_X()[i]\n",
    "\n",
    "// setters\n",
    "void ekf_set_Q(float Q[N_INPUTS]);                    // set process noise covariance matrix diagonal\n",
    "void ekf_set_R(float R[N_MEASUREMENTS]);              // set measurement noise covariance matrix diagonal\n",
    "void ekf_set_X(float X0[N_STATES]);                   // set state vector\n",
    "void ekf_set_P_diag(float P_diag[N_STATES]);          // set covariance matrix diagonal\n",
    "void ekf_set_P(float P0[N_STATES*(N_STATES+1)/2]);    // set covariance matrix (lower diagonal)\n",
    "\n",
    "// prediction and update functions\n",
    "void ekf_predict(float U[N_INPUTS], float dt);\n",
    "void ekf_update(float Z[N_MEASUREMENTS]);\n",
    "\n",
    "\n",
    "#endif // EKF_CALC_H\n",
    "'''\n",
    "\n",
    "prediction_code_ = prediction_code.replace('\\n','\\n\\t')\n",
    "update_code_ = update_code.replace('\\n','\\n\\t')\n",
    "\n",
    "source = f'''\n",
    "///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////\n",
    "// This file is automatically generated in \"Generate Kalman Filter C Code.ipynb\" from https://github.com/tudelft/kalman_filter   //\n",
    "///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////\n",
    "\n",
    "#include \"ekf_calc.h\"\n",
    "#include <math.h>\n",
    "\n",
    "float ekf_use_phi = 1;\n",
    "float ekf_use_theta = 1;\n",
    "float ekf_use_psi = 1;\n",
    "\n",
    "float ekf_Q[N_INPUTS];         // Kalman filter process noise covariance matrix (diagonal)\n",
    "float ekf_R[N_MEASUREMENTS];   // Kalman filter measurement noise covariance matrix (diagonal)\n",
    "\n",
    "// state\n",
    "float ekf_X[N_STATES];\n",
    "float ekf_X_new[N_STATES];\n",
    "\n",
    "// covariance matrix (lower diagonal) P[i,j] = P_lower_diagonal[i*(i+1)/2+j] (if i>=j)\n",
    "float ekf_P_lower_diagonal[N_STATES*(N_STATES+1)/2];\n",
    "float ekf_P_lower_diagonal_new[N_STATES*(N_STATES+1)/2];\n",
    "\n",
    "// temporary variables\n",
    "float tmp[{max(prediction_code_N_tmps,update_code_N_tmps)}];\n",
    "\n",
    "// pointers\n",
    "float *X = ekf_X;\n",
    "float *X_new = ekf_X_new;\n",
    "\n",
    "float *P = ekf_P_lower_diagonal;\n",
    "float *P_new = ekf_P_lower_diagonal_new;\n",
    "\n",
    "// renaming\n",
    "float *Q = ekf_Q;\n",
    "float *R = ekf_R;\n",
    "\n",
    "// pointer for swapping\n",
    "float *swap_ptr;\n",
    "\n",
    "float* ekf_get_X() {{\n",
    "    return X;\n",
    "}}\n",
    "\n",
    "float* ekf_get_P() {{\n",
    "    return P;\n",
    "}}\n",
    "\n",
    "void ekf_set_Q(float Q[N_INPUTS]) {{\n",
    "    for (int i=0; i<N_INPUTS; i++) {{\n",
    "        ekf_Q[i] = Q[i];\n",
    "    }}\n",
    "}}\n",
    "\n",
    "void ekf_set_R(float R[N_MEASUREMENTS]) {{\n",
    "    for (int i=0; i<N_MEASUREMENTS; i++) {{\n",
    "        ekf_R[i] = R[i];\n",
    "    }}\n",
    "}}\n",
    "\n",
    "void ekf_set_X(float X0[N_STATES]) {{\n",
    "    for (int i=0; i<N_STATES; i++) {{\n",
    "        X[i] = X0[i];\n",
    "    }}\n",
    "}}\n",
    "\n",
    "void ekf_set_P_diag(float P_diag[N_STATES]) {{\n",
    "    // set P to zeros\n",
    "    for (int i=0; i<N_STATES*(N_STATES+1)/2; i++) {{\n",
    "        P[i] = 0.;\n",
    "    }}\n",
    "    // set diagonal\n",
    "    for (int i=0; i<N_STATES; i++) {{\n",
    "        P[i*(i+1)/2+i] = P_diag[i];\n",
    "    }}\n",
    "}}\n",
    "\n",
    "void ekf_set_P(float P0[N_STATES*(N_STATES+1)/2]) {{\n",
    "    for (int i=0; i<N_STATES*(N_STATES+1)/2; i++) {{\n",
    "        P[i] = P0[i];\n",
    "    }}\n",
    "}}\n",
    "\n",
    "void ekf_predict(float U[N_INPUTS], float dt) {{\n",
    "    // PREDICTION STEP X_new, P_new = ...\n",
    "    {prediction_code_}\n",
    "\n",
    "    // swap X, X_new and P, P_new pointers\n",
    "    swap_ptr = X;\n",
    "    X = X_new;\n",
    "    X_new = swap_ptr;\n",
    "\n",
    "    swap_ptr = P;\n",
    "    P = P_new;\n",
    "    P_new = swap_ptr;\n",
    "}}\n",
    "\n",
    "void ekf_update(float Z[N_MEASUREMENTS]) {{\n",
    "    // UPDATE STEP X_new, P_new = ...\n",
    "    {update_code_}\n",
    "\n",
    "    // swap X, X_new and P, P_new pointers\n",
    "    swap_ptr = X;\n",
    "    X = X_new;\n",
    "    X_new = swap_ptr;\n",
    "\n",
    "    swap_ptr = P;\n",
    "    P = P_new;\n",
    "    P_new = swap_ptr;\n",
    "}}\n",
    "'''\n",
    "\n",
    "with open('c_code/ekf_calc.h','w') as file:\n",
    "    file.write(header)\n",
    "with open('c_code/ekf_calc.c','w') as file:\n",
    "    file.write(source)\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Compile C Code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import subprocess\n",
    "import ctypes\n",
    "import numpy as np\n",
    "import importlib\n",
    "importlib.reload(ctypes)\n",
    "importlib.reload(subprocess)\n",
    "\n",
    "# Compile C code and create library\n",
    "\n",
    "# https://cu7ious.medium.com/how-to-use-dynamic-libraries-in-c-46a0f9b98270\n",
    "path = os.path.abspath('c_code')\n",
    "# Remove old library\n",
    "subprocess.call('rm *.so', shell=True, cwd=path)\n",
    "# Compile object files\n",
    "subprocess.call('gcc -fPIC -c *.c', shell=True, cwd=path)\n",
    "# Create library\n",
    "subprocess.call('gcc -shared -Wl,-soname,libtools.so -o libtools.so *.o', shell=True, cwd=path)\n",
    "# Remove object files\n",
    "# subprocess.call('rm *.o', shell=True, cwd=path)\n",
    "\n",
    "lib_path = os.path.abspath(\"c_code/libtools.so\")\n",
    "lib = ctypes.CDLL(lib_path)\n",
    "\n",
    "# define argument types \n",
    "lib.ekf_set_Q.argtypes = [ctypes.POINTER(ctypes.c_float)]\n",
    "lib.ekf_set_R.argtypes = [ctypes.POINTER(ctypes.c_float)]\n",
    "lib.ekf_set_X.argtypes = [ctypes.POINTER(ctypes.c_float)]\n",
    "lib.ekf_set_P_diag.argtypes = [ctypes.POINTER(ctypes.c_float)]\n",
    "\n",
    "lib.ekf_get_X.restype = ctypes.POINTER(ctypes.c_float)\n",
    "lib.ekf_get_P.restype = ctypes.POINTER(ctypes.c_float)\n",
    "\n",
    "lib.ekf_predict.argtypes = [ctypes.POINTER(ctypes.c_float), ctypes.c_float]\n",
    "lib.ekf_update.argtypes = [ctypes.POINTER(ctypes.c_float)]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Test C code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "import csv\n",
    "import numpy as np\n",
    "\n",
    "def load_flight_data(file_name):\n",
    "    with open(file_name) as file:\n",
    "        reader = csv.reader(file)\n",
    "        data = list(reader)\n",
    "        \n",
    "        # separate header and data\n",
    "        header = [d for d in data if len(d)==2]\n",
    "        header = {d[0]: d[1] for d in header}\n",
    "        \n",
    "        data = [d for d in data if len(d)>2]\n",
    "        keys = data[0]\n",
    "        data = data[1:]\n",
    "        data = np.array([[float(d) if d else np.nan for d in row] for row in data]) # set empty strings to nan\n",
    "        data = dict(zip(keys,data.T))\n",
    "        \n",
    "        # renaming keys\n",
    "        data['t'] = data.pop('time')*1e-6 # us to s\n",
    "        data['t'] -= data['t'][0] # start at 0\n",
    "\n",
    "        # state estimate\n",
    "        data['x'] = data.pop('pos[0]')*1e-3 # mm to m\n",
    "        data['y'] = data.pop('pos[1]')*1e-3 # mm to m\n",
    "        data['z'] = data.pop('pos[2]')*1e-3 # mm to m\n",
    "        \n",
    "        data['vx'] = data.pop('vel[0]')*1e-2 # cm/s to m/s\n",
    "        data['vy'] = data.pop('vel[1]')*1e-2 # cm/s to m/s\n",
    "        data['vz'] = data.pop('vel[2]')*1e-2 # cm/s to m/s\n",
    "        \n",
    "        quat_scaling = ((127 << 6) - 1) # float from -1 to +1 to VB that takes up 2 bytes max\n",
    "        data['qw'] = data.pop('quat[0]')/quat_scaling\n",
    "        data['qx'] = data.pop('quat[1]')/quat_scaling\n",
    "        data['qy'] = data.pop('quat[2]')/quat_scaling\n",
    "        data['qz'] = data.pop('quat[3]')/quat_scaling\n",
    "        \n",
    "        # quat to eulers\n",
    "        data['phi'] = np.arctan2(2*(data['qw']*data['qx'] + data['qy']*data['qz']), 1 - 2*(data['qx']**2 + data['qy']**2))\n",
    "        data['theta'] = np.arcsin(2*(data['qw']*data['qy'] - data['qz']*data['qx']))\n",
    "        data['psi'] = np.arctan2(2*(data['qw']*data['qz'] + data['qx']*data['qy']), 1 - 2*(data['qy']**2 + data['qz']**2))\n",
    "        \n",
    "        \n",
    "        # optitrack\n",
    "        data['x_opti'] = data.pop('extPos[0]')*1e-3 # mm to m\n",
    "        data['y_opti'] = data.pop('extPos[1]')*1e-3 # mm to m\n",
    "        data['z_opti'] = data.pop('extPos[2]')*1e-3 # mm to m\n",
    "        \n",
    "        data['vx_opti'] = data.pop('extVel[0]')*1e-2 # cm/s to m/s\n",
    "        data['vy_opti'] = data.pop('extVel[1]')*1e-2 # cm/s to m/s\n",
    "        data['vz_opti'] = data.pop('extVel[2]')*1e-2 # cm/s to m/s\n",
    "        \n",
    "        data['phi_opti'] = data.pop('extAtt[0]')/1000 # mrad to rad\n",
    "        data['theta_opti'] = data.pop('extAtt[1]')/1000 # mrad to rad\n",
    "        data['psi_opti'] = data.pop('extAtt[2]')/1000 # mrad to rad\n",
    "        \n",
    "        # imu (from FLU to FRD)\n",
    "        gyro_scale = np.pi/180 # deg/s to rad/s\n",
    "        if bool(float(header['blackbox_high_resolution'])):\n",
    "            gyro_scale /= 10\n",
    "        data['p'] = data.pop('gyroADC[0]')*gyro_scale\n",
    "        data['q'] =-data.pop('gyroADC[1]')*gyro_scale\n",
    "        data['r'] =-data.pop('gyroADC[2]')*gyro_scale\n",
    "        \n",
    "        acc_scale = 9.81/float(header['acc_1G'])\n",
    "        data['ax'] = data.pop('accSmooth[0]')*acc_scale\n",
    "        data['ay'] =-data.pop('accSmooth[1]')*acc_scale\n",
    "        data['az'] =-data.pop('accSmooth[2]')*acc_scale\n",
    "        \n",
    "        # onboard ekf\n",
    "        data['ekf_x'] = data.pop('ekf_pos[0]')*1e-3 # mm to m\n",
    "        data['ekf_y'] = data.pop('ekf_pos[1]')*1e-3 # mm to m\n",
    "        data['ekf_z'] = data.pop('ekf_pos[2]')*1e-3 # mm to m\n",
    "        data['ekf_vx'] = data.pop('ekf_vel[0]')*1e-2 # cm/s to m/s\n",
    "        data['ekf_vy'] = data.pop('ekf_vel[1]')*1e-2 # cm/s to m/s\n",
    "        data['ekf_vz'] = data.pop('ekf_vel[2]')*1e-2 # cm/s to m/s\n",
    "        data['ekf_phi'] = data.pop('ekf_att[0]')/1000 # mrad to rad\n",
    "        data['ekf_theta'] = data.pop('ekf_att[1]')/1000 # mrad to rad\n",
    "        data['ekf_psi'] = data.pop('ekf_att[2]')/1000 # mrad to rad\n",
    "        data['ekf_acc_b_x'] = data.pop('ekf_acc_b[0]')/1000 # mm/s^2 to m/s^2\n",
    "        data['ekf_acc_b_y'] = data.pop('ekf_acc_b[1]')/1000 # mm/s^2 to m/s^2\n",
    "        data['ekf_acc_b_z'] = data.pop('ekf_acc_b[2]')/1000 # mm/s^2 to m/s^2\n",
    "        data['ekf_gyro_b_x'] = data.pop('ekf_gyro_b[0]')*np.pi/180\n",
    "        data['ekf_gyro_b_y'] = data.pop('ekf_gyro_b[1]')*np.pi/180\n",
    "        data['ekf_gyro_b_z'] = data.pop('ekf_gyro_b[2]')*np.pi/180\n",
    "                \n",
    "        return data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 3065  3066  3067 ... 11933 11934 11935]\n",
      "6.0631620000000055\n",
      "logging frequency: 505.68133262252906 Hz\n",
      "optitrack frequency: 38.656266928223545 Hz\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "data = np.load('ekf_data_6may_8ms_delay.npz')\n",
    "    \n",
    "# logging frequency\n",
    "freq = 1/np.mean(np.diff(data['t']))\n",
    "print('logging frequency:',freq,'Hz')\n",
    "\n",
    "# optitrack frequency\n",
    "opti_updates = (np.gradient(data['x_opti'])!=0) | (np.gradient(data['y_opti'])!=0) | (np.gradient(data['z_opti'])!=0)\n",
    "opti_freq = 1/np.mean(np.diff(data['t'][opti_updates]))\n",
    "print('optitrack frequency:',opti_freq,'Hz')\n",
    "\n",
    "# plot ax, ay, az\n",
    "# clos all plots\n",
    "%matplotlib inline\n",
    "plt.close('all')\n",
    "plt.plot(data['t'],data['ax'],label='ax')\n",
    "plt.plot(data['t'],data['ay'],label='ay')\n",
    "plt.plot(data['t'],data['az'],label='az')\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3\n",
      "delay: 0.005932589760938203 s\n"
     ]
    }
   ],
   "source": [
    "# EKF INPUT: U = [ax, ay, az, p, q, r] (IMU)\n",
    "U_data = np.array([\n",
    "    data['ax'], data['ay'], data['az'], data['p'], data['q'], data['r']\n",
    "]).T\n",
    "\n",
    "# EKF MEASUREMENT: Z = [x, y, z, phi, theta, psi] (Optitrack)\n",
    "Z_data = np.array([\n",
    "    data['x_opti'], data['y_opti'], data['z_opti'], data['phi_opti'], data['theta_opti'], data['psi_opti']\n",
    "]).T\n",
    "\n",
    "# SHIFT Z data by 3 samples\n",
    "shift = 3\n",
    "Z_data_shift = np.roll(Z_data, -shift, axis=0)\n",
    "# Z_data = Z_data_shift\n",
    "\n",
    "print(shift)\n",
    "\n",
    "\n",
    "t = data['t']\n",
    "delay = np.mean(t[shift:]-t[:-shift])\n",
    "print('delay:',delay,'s')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.463      -0.042      -0.13500001  0.          0.          0.\n",
      "  0.          0.          0.          0.          0.          0.\n",
      "  0.          0.          0.        ]\n"
     ]
    }
   ],
   "source": [
    "# INITIALIZE EKF\n",
    "# initial state\n",
    "X_init = np.zeros(len(X))\n",
    "X_init[:3] = Z_data[0,:3]\n",
    "# X_init[12:15] = [p_bias, q_bias, r_bias]\n",
    "\n",
    "\n",
    "# initial covariance\n",
    "P_init = np.array([1.]*len(X))\n",
    "# P_init[-6:] = 0. # set covariance of IMU bias to 0\n",
    "# print(P_init)\n",
    "\n",
    "# process noise covariance\n",
    "Q_diag = np.array([0.5, 0.5, 0.5, 0.01, 0.01, 0.01])\n",
    "\n",
    "# measurement noise covariance\n",
    "R_diag = np.array([0.001, 0.001, 0.001, .01, .01, .01])\n",
    "\n",
    "lib.ekf_set_Q((ctypes.c_float*len(Q_diag))(*Q_diag))\n",
    "lib.ekf_set_R((ctypes.c_float*len(R_diag))(*R_diag))\n",
    "lib.ekf_set_X((ctypes.c_float*len(X_init))(*X_init))\n",
    "lib.ekf_set_P_diag((ctypes.c_float*len(P_init))(*P_init))\n",
    "\n",
    "# test ekf_get_state\n",
    "x = lib.ekf_get_X()\n",
    "x = np.array([x[i] for i in range(len(X))])\n",
    "print(x)\n",
    "\n",
    "# RUN EKF\n",
    "X_ekf = [X_init]\n",
    "# Pdiag_ekf = [P_init]\n",
    "\n",
    "X_sampled = []\n",
    "X_delayed = []\n",
    "X_shift =[]\n",
    "m_idx =[]\n",
    "\n",
    "for i in range(1, len(t)):\n",
    "    # PREDICT\n",
    "    lib.ekf_predict(\n",
    "        (ctypes.c_float*len(U_data[i]))(*U_data[i]),\n",
    "        ctypes.c_float(t[i]-t[i-1])\n",
    "    )\n",
    "    \n",
    "    # UPDATE\n",
    "    # only update when there is a new measurement\n",
    "    if np.any(Z_data[i]-Z_data[i-1] != 0):\n",
    "        m_idx.append(i)\n",
    "        \n",
    "        # fix yaw\n",
    "        while Z_data[i,5]-Z_data[i-1,5] > np.pi:\n",
    "            Z_data[i,5] -= 2*np.pi\n",
    "        while Z_data[i,5]-Z_data[i-1,5] < -np.pi:\n",
    "            Z_data[i,5] += 2*np.pi\n",
    "            \n",
    "        # # fix delay issue\n",
    "        # time_delay = 0.008\n",
    "        # # message time\n",
    "        # m_time = t[i] - 0.008\n",
    "        \n",
    "        # # backwards predict n steps\n",
    "        # j = 0\n",
    "        # while t[i-j] > m_time and i-j-1 > 0:\n",
    "        #     lib.ekf_predict(\n",
    "        #         (ctypes.c_float*len(U_data[i-j]))(*U_data[i-j]),\n",
    "        #         ctypes.c_float(-(t[i-j]-t[i-j-1]))                  # negative time step\n",
    "        #     )\n",
    "        #     j += 1\n",
    "        \n",
    "        # # backwards predict remainder\n",
    "        # lib.ekf_predict(\n",
    "        #     (ctypes.c_float*len(U_data[i-j]))(*U_data[i-j]),\n",
    "        #     ctypes.c_float(-(m_time-t[i-j]))\n",
    "        # )\n",
    "        \n",
    "        # backwards predict\n",
    "        # lib.ekf_predict(\n",
    "        #     (ctypes.c_float*len(U_data[i]))(*U_data[i]),\n",
    "        #     ctypes.c_float(-delay)\n",
    "        # )\n",
    "        \n",
    "        # save X delayed\n",
    "        # x = lib.ekf_get_X()\n",
    "        # x = np.array([x[i] for i in range(len(X))])\n",
    "        # X_delayed.append(x)\n",
    "        \n",
    "        \n",
    "        # update\n",
    "        lib.ekf_update(\n",
    "            (ctypes.c_float*len(Z_data[i]))(*Z_data[i])\n",
    "        )\n",
    "        \n",
    "        # forward predict\n",
    "        # lib.ekf_predict(\n",
    "        #     (ctypes.c_float*len(U_data[i]))(*U_data[i]),\n",
    "        #     ctypes.c_float(delay)\n",
    "        # )\n",
    "        \n",
    "        # forward predict remainder\n",
    "        # lib.ekf_predict(\n",
    "        #     (ctypes.c_float*len(U_data[i-j-1]))(*U_data[i-j-1]),\n",
    "        #     ctypes.c_float(-(m_time-t[i-j]))\n",
    "        # )\n",
    "        \n",
    "        # # forward predict n steps\n",
    "        # for k in range(j):\n",
    "        #     lib.ekf_predict(\n",
    "        #         (ctypes.c_float*len(U_data[i-j+k]))(*U_data[i-j+k]),\n",
    "        #         ctypes.c_float(t[i-j+k+1]-t[i-j+k])\n",
    "        #     )\n",
    "        \n",
    "        \n",
    "        \n",
    "    # get state\n",
    "    x = lib.ekf_get_X()\n",
    "    x = np.array([x[i] for i in range(len(X))])\n",
    "    X_ekf.append(x)\n",
    "    \n",
    "    # get Pdiag\n",
    "    # P_diag = (ctypes.c_float*len(X))()\n",
    "    # lib.ekf_get_Pdiag(P_diag)\n",
    "    # P_diag = np.array(P_diag[:])\n",
    "    # Pdiag_ekf.append(P_diag)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [],
   "source": [
    "X_ekf_corrected =  np.array(X_ekf)\n",
    "X_ekf_normal = np.array(X_ekf)\n",
    "X_ekf_Zshift = np.array(X_ekf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 68,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Using matplotlib backend: TkAgg\n"
     ]
    }
   ],
   "source": [
    "# plot trajectory\n",
    "%matplotlib\n",
    "X_ekf = np.array(X_ekf)\n",
    "# Pdiag_ekf = np.array(Pdiag_ekf)\n",
    "\n",
    "\n",
    "# 5x3 subplots\n",
    "fig, axs = plt.subplots(5, 3, figsize=(20,20), tight_layout=True, sharex=True)\n",
    "axs = axs.flatten()\n",
    "\n",
    "# position\n",
    "axs[0].plot(t, data['x_opti'], label='x opti')\n",
    "axs[0].plot(t, X_ekf_normal[:,0], label='x ekf normal')\n",
    "# axs[0].plot(t, X_ekf_Zshift[:,0], label='x ekf Zshift')\n",
    "# axs[0].plot(t, X_ekf_corrected[:,0], label='x ekf corrected')\n",
    "axs[0].plot(t, data['ekf_x'], label='x ekf onboard')\n",
    "axs[0].legend()\n",
    "\n",
    "\n",
    "axs[1].plot(t, data['y_opti'], label='y opti')\n",
    "axs[1].plot(t, X_ekf_normal[:,1], label='y ekf normal')\n",
    "# axs[1].plot(t, X_ekf_Zshift[:,1], label='y ekf Zshift')\n",
    "# axs[1].plot(t, X_ekf_corrected[:,1], label='y ekf corrected')\n",
    "axs[1].plot(t, data['ekf_y'], label='y ekf onboard')\n",
    "axs[1].legend()\n",
    "\n",
    "axs[2].plot(t, data['z_opti'], label='z opti')\n",
    "axs[2].plot(t, X_ekf_normal[:,2], label='z ekf normal')\n",
    "# axs[2].plot(t, X_ekf_Zshift[:,2], label='z ekf Zshift')\n",
    "# axs[2].plot(t, X_ekf_corrected[:,2], label='z ekf corrected')\n",
    "axs[2].plot(t, data['ekf_z'], label='z ekf onboard')\n",
    "axs[2].legend()\n",
    "\n",
    "# velocity\n",
    "axs[3].plot(t, X_ekf_normal[:,3], label='vx ekf normal')\n",
    "# axs[3].plot(t, X_ekf_Zshift[:,3], label='vx ekf Zshift')\n",
    "# axs[3].plot(t, X_ekf_corrected[:,3], label='vx ekf corrected')\n",
    "axs[3].plot(t, data['ekf_vx'], label='vx ekf onboard')\n",
    "axs[3].legend()\n",
    "\n",
    "axs[4].plot(t, X_ekf_normal[:,4], label='vy ekf normal')\n",
    "# axs[4].plot(t, X_ekf_Zshift[:,4], label='vy ekf Zshift')\n",
    "# axs[4].plot(t, X_ekf_corrected[:,4], label='vy ekf corrected')\n",
    "axs[4].plot(t, data['ekf_vy'], label='vy ekf onboard')\n",
    "axs[4].legend()\n",
    "\n",
    "axs[5].plot(t, X_ekf_normal[:,5], label='vz ekf normal')\n",
    "# axs[5].plot(t, X_ekf_Zshift[:,5], label='vz ekf Zshift')\n",
    "# axs[5].plot(t, X_ekf_corrected[:,5], label='vz ekf corrected')\n",
    "axs[5].plot(t, data['ekf_vz'], label='vz ekf onboard')\n",
    "axs[5].legend()\n",
    "\n",
    "# attitude\n",
    "rad_to_deg = 180/np.pi\n",
    "axs[6].plot(t, rad_to_deg*data['phi_opti'], label='phi opti')\n",
    "axs[6].plot(t, rad_to_deg*X_ekf_normal[:,6], label='phi ekf normal')\n",
    "# axs[6].plot(t, rad_to_deg*X_ekf_Zshift[:,6], label='phi ekf Zshift')\n",
    "# axs[6].plot(t, rad_to_deg*X_ekf_corrected[:,6], label='phi ekf corrected')\n",
    "axs[6].plot(t, rad_to_deg*data['ekf_phi'], label='phi ekf onboard')\n",
    "axs[6].legend()\n",
    "\n",
    "axs[7].plot(t, rad_to_deg*data['theta_opti'], label='theta opti')\n",
    "axs[7].plot(t, rad_to_deg*X_ekf_normal[:,7], label='theta ekf normal')\n",
    "# axs[7].plot(t, rad_to_deg*X_ekf_Zshift[:,7], label='theta ekf Zshift')\n",
    "# axs[7].plot(t, rad_to_deg*X_ekf_corrected[:,7], label='theta ekf corrected')\n",
    "axs[7].plot(t, rad_to_deg*data['ekf_theta'], label='theta ekf onboard')\n",
    "axs[7].legend()\n",
    "\n",
    "axs[8].plot(t, rad_to_deg*data['psi_opti'], label='psi opti')\n",
    "axs[8].plot(t, rad_to_deg*X_ekf_normal[:,8], label='psi ekf normal')\n",
    "# axs[8].plot(t, rad_to_deg*X_ekf_Zshift[:,8], label='psi ekf Zshift')\n",
    "# axs[8].plot(t, rad_to_deg*X_ekf_corrected[:,8], label='psi ekf corrected')\n",
    "axs[8].plot(t, rad_to_deg*data['ekf_psi'], label='psi ekf onboard')\n",
    "axs[8].legend()\n",
    "\n",
    "# biases\n",
    "axs[9].plot(t, X_ekf_normal[:,9], label='lx ekf normal')\n",
    "# axs[9].plot(t, X_ekf_Zshift[:,9], label='lx ekf Zshift')\n",
    "# axs[9].plot(t, X_ekf_corrected[:,9], label='lx ekf corrected')\n",
    "axs[9].plot(t, data['ekf_acc_b_x'], label='lx ekf onboard')\n",
    "axs[9].legend()\n",
    "\n",
    "axs[10].plot(t, X_ekf_normal[:,10], label='ly ekf normal')\n",
    "# axs[10].plot(t, X_ekf_Zshift[:,10], label='ly ekf Zshift')\n",
    "# axs[10].plot(t, X_ekf_corrected[:,10], label='ly ekf corrected')\n",
    "axs[10].plot(t, data['ekf_acc_b_y'], label='ly ekf onboard')\n",
    "axs[10].legend()\n",
    "\n",
    "axs[11].plot(t, X_ekf_normal[:,11], label='lz ekf normal')\n",
    "# axs[11].plot(t, X_ekf_Zshift[:,11], label='lz ekf Zshift')\n",
    "# axs[11].plot(t, X_ekf_corrected[:,11], label='lz ekf corrected')\n",
    "axs[11].plot(t, data['ekf_acc_b_z'], label='lz ekf onboard')\n",
    "axs[11].legend()\n",
    "\n",
    "axs[12].plot(t, rad_to_deg*X_ekf_normal[:,12], label='lp ekf normal')\n",
    "# axs[12].plot(t, rad_to_deg*X_ekf_Zshift[:,12], label='lp ekf Zshift')\n",
    "# axs[12].plot(t, rad_to_deg*X_ekf_corrected[:,12], label='lp ekf corrected')\n",
    "axs[12].plot(t, rad_to_deg*data['ekf_gyro_b_x'], label='lp ekf onboard')\n",
    "axs[12].legend()\n",
    "\n",
    "axs[13].plot(t, rad_to_deg*X_ekf_normal[:,13], label='lq ekf normal')\n",
    "# axs[13].plot(t, rad_to_deg*X_ekf_Zshift[:,13], label='lq ekf Zshift')\n",
    "# axs[13].plot(t, rad_to_deg*X_ekf_corrected[:,13], label='lq ekf corrected')\n",
    "axs[13].plot(t, rad_to_deg*data['ekf_gyro_b_y'], label='lq ekf onboard')\n",
    "axs[13].legend()\n",
    "\n",
    "axs[14].plot(t, rad_to_deg*X_ekf_normal[:,14], label='lr ekf normal')\n",
    "# axs[14].plot(t, rad_to_deg*X_ekf_Zshift[:,14], label='lr ekf Zshift')\n",
    "# axs[14].plot(t, rad_to_deg*X_ekf_corrected[:,14], label='lr ekf corrected')\n",
    "axs[14].plot(t, rad_to_deg*data['ekf_gyro_b_z'], label='lr ekf onboard')\n",
    "axs[14].legend()\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 124,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.close('all')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "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.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}