#include "live.h" namespace { #define DIM 22 #define EDIM 21 #define MEDIM 21 typedef void (*Hfun)(double *, double *, double *); const static double MAHA_THRESH_4 = 7.814727903251177; const static double MAHA_THRESH_9 = 7.814727903251177; const static double MAHA_THRESH_10 = 7.814727903251177; const static double MAHA_THRESH_12 = 7.814727903251177; const static double MAHA_THRESH_35 = 7.814727903251177; const static double MAHA_THRESH_32 = 9.487729036781154; const static double MAHA_THRESH_13 = 7.814727903251177; const static double MAHA_THRESH_14 = 7.814727903251177; const static double MAHA_THRESH_33 = 7.814727903251177; /****************************************************************************** * Code generated with SymPy 1.12 * * * * See http://www.sympy.org/ for more information. * * * * This file is part of 'ekf' * ******************************************************************************/ void H(double *in_vec, double *out_8945704586164018613) { out_8945704586164018613[0] = 0; out_8945704586164018613[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; out_8945704586164018613[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; out_8945704586164018613[3] = cos(in_vec[1])*cos(in_vec[2]); out_8945704586164018613[4] = sin(in_vec[2])*cos(in_vec[1]); out_8945704586164018613[5] = -sin(in_vec[1]); out_8945704586164018613[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; out_8945704586164018613[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; out_8945704586164018613[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; out_8945704586164018613[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); out_8945704586164018613[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); out_8945704586164018613[11] = sin(in_vec[0])*cos(in_vec[1]); out_8945704586164018613[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; out_8945704586164018613[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; out_8945704586164018613[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; out_8945704586164018613[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); out_8945704586164018613[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); out_8945704586164018613[17] = cos(in_vec[0])*cos(in_vec[1]); } void err_fun(double *nom_x, double *delta_x, double *out_8370097340763738192) { out_8370097340763738192[0] = delta_x[0] + nom_x[0]; out_8370097340763738192[1] = delta_x[1] + nom_x[1]; out_8370097340763738192[2] = delta_x[2] + nom_x[2]; out_8370097340763738192[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; out_8370097340763738192[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; out_8370097340763738192[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; out_8370097340763738192[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; out_8370097340763738192[7] = delta_x[6] + nom_x[7]; out_8370097340763738192[8] = delta_x[7] + nom_x[8]; out_8370097340763738192[9] = delta_x[8] + nom_x[9]; out_8370097340763738192[10] = delta_x[9] + nom_x[10]; out_8370097340763738192[11] = delta_x[10] + nom_x[11]; out_8370097340763738192[12] = delta_x[11] + nom_x[12]; out_8370097340763738192[13] = delta_x[12] + nom_x[13]; out_8370097340763738192[14] = delta_x[13] + nom_x[14]; out_8370097340763738192[15] = delta_x[14] + nom_x[15]; out_8370097340763738192[16] = delta_x[15] + nom_x[16]; out_8370097340763738192[17] = delta_x[16] + nom_x[17]; out_8370097340763738192[18] = delta_x[17] + nom_x[18]; out_8370097340763738192[19] = delta_x[18] + nom_x[19]; out_8370097340763738192[20] = delta_x[19] + nom_x[20]; out_8370097340763738192[21] = delta_x[20] + nom_x[21]; } void inv_err_fun(double *nom_x, double *true_x, double *out_1561714653313430456) { out_1561714653313430456[0] = -nom_x[0] + true_x[0]; out_1561714653313430456[1] = -nom_x[1] + true_x[1]; out_1561714653313430456[2] = -nom_x[2] + true_x[2]; out_1561714653313430456[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; out_1561714653313430456[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; out_1561714653313430456[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; out_1561714653313430456[6] = -nom_x[7] + true_x[7]; out_1561714653313430456[7] = -nom_x[8] + true_x[8]; out_1561714653313430456[8] = -nom_x[9] + true_x[9]; out_1561714653313430456[9] = -nom_x[10] + true_x[10]; out_1561714653313430456[10] = -nom_x[11] + true_x[11]; out_1561714653313430456[11] = -nom_x[12] + true_x[12]; out_1561714653313430456[12] = -nom_x[13] + true_x[13]; out_1561714653313430456[13] = -nom_x[14] + true_x[14]; out_1561714653313430456[14] = -nom_x[15] + true_x[15]; out_1561714653313430456[15] = -nom_x[16] + true_x[16]; out_1561714653313430456[16] = -nom_x[17] + true_x[17]; out_1561714653313430456[17] = -nom_x[18] + true_x[18]; out_1561714653313430456[18] = -nom_x[19] + true_x[19]; out_1561714653313430456[19] = -nom_x[20] + true_x[20]; out_1561714653313430456[20] = -nom_x[21] + true_x[21]; } void H_mod_fun(double *state, double *out_619228651928883812) { out_619228651928883812[0] = 1.0; out_619228651928883812[1] = 0; out_619228651928883812[2] = 0; out_619228651928883812[3] = 0; out_619228651928883812[4] = 0; out_619228651928883812[5] = 0; out_619228651928883812[6] = 0; out_619228651928883812[7] = 0; out_619228651928883812[8] = 0; out_619228651928883812[9] = 0; out_619228651928883812[10] = 0; out_619228651928883812[11] = 0; out_619228651928883812[12] = 0; out_619228651928883812[13] = 0; out_619228651928883812[14] = 0; out_619228651928883812[15] = 0; out_619228651928883812[16] = 0; out_619228651928883812[17] = 0; out_619228651928883812[18] = 0; out_619228651928883812[19] = 0; out_619228651928883812[20] = 0; out_619228651928883812[21] = 0; out_619228651928883812[22] = 1.0; out_619228651928883812[23] = 0; out_619228651928883812[24] = 0; out_619228651928883812[25] = 0; out_619228651928883812[26] = 0; out_619228651928883812[27] = 0; out_619228651928883812[28] = 0; out_619228651928883812[29] = 0; out_619228651928883812[30] = 0; out_619228651928883812[31] = 0; out_619228651928883812[32] = 0; out_619228651928883812[33] = 0; out_619228651928883812[34] = 0; out_619228651928883812[35] = 0; out_619228651928883812[36] = 0; out_619228651928883812[37] = 0; out_619228651928883812[38] = 0; out_619228651928883812[39] = 0; out_619228651928883812[40] = 0; out_619228651928883812[41] = 0; out_619228651928883812[42] = 0; out_619228651928883812[43] = 0; out_619228651928883812[44] = 1.0; out_619228651928883812[45] = 0; out_619228651928883812[46] = 0; out_619228651928883812[47] = 0; out_619228651928883812[48] = 0; out_619228651928883812[49] = 0; out_619228651928883812[50] = 0; out_619228651928883812[51] = 0; out_619228651928883812[52] = 0; out_619228651928883812[53] = 0; out_619228651928883812[54] = 0; out_619228651928883812[55] = 0; out_619228651928883812[56] = 0; out_619228651928883812[57] = 0; out_619228651928883812[58] = 0; out_619228651928883812[59] = 0; out_619228651928883812[60] = 0; out_619228651928883812[61] = 0; out_619228651928883812[62] = 0; out_619228651928883812[63] = 0; out_619228651928883812[64] = 0; out_619228651928883812[65] = 0; out_619228651928883812[66] = -0.5*state[4]; out_619228651928883812[67] = -0.5*state[5]; out_619228651928883812[68] = -0.5*state[6]; out_619228651928883812[69] = 0; out_619228651928883812[70] = 0; out_619228651928883812[71] = 0; out_619228651928883812[72] = 0; out_619228651928883812[73] = 0; out_619228651928883812[74] = 0; out_619228651928883812[75] = 0; out_619228651928883812[76] = 0; out_619228651928883812[77] = 0; out_619228651928883812[78] = 0; out_619228651928883812[79] = 0; out_619228651928883812[80] = 0; out_619228651928883812[81] = 0; out_619228651928883812[82] = 0; out_619228651928883812[83] = 0; out_619228651928883812[84] = 0; out_619228651928883812[85] = 0; out_619228651928883812[86] = 0; out_619228651928883812[87] = 0.5*state[3]; out_619228651928883812[88] = 0.5*state[6]; out_619228651928883812[89] = -0.5*state[5]; out_619228651928883812[90] = 0; out_619228651928883812[91] = 0; out_619228651928883812[92] = 0; out_619228651928883812[93] = 0; out_619228651928883812[94] = 0; out_619228651928883812[95] = 0; out_619228651928883812[96] = 0; out_619228651928883812[97] = 0; out_619228651928883812[98] = 0; out_619228651928883812[99] = 0; out_619228651928883812[100] = 0; out_619228651928883812[101] = 0; out_619228651928883812[102] = 0; out_619228651928883812[103] = 0; out_619228651928883812[104] = 0; out_619228651928883812[105] = 0; out_619228651928883812[106] = 0; out_619228651928883812[107] = 0; out_619228651928883812[108] = -0.5*state[6]; out_619228651928883812[109] = 0.5*state[3]; out_619228651928883812[110] = 0.5*state[4]; out_619228651928883812[111] = 0; out_619228651928883812[112] = 0; out_619228651928883812[113] = 0; out_619228651928883812[114] = 0; out_619228651928883812[115] = 0; out_619228651928883812[116] = 0; out_619228651928883812[117] = 0; out_619228651928883812[118] = 0; out_619228651928883812[119] = 0; out_619228651928883812[120] = 0; out_619228651928883812[121] = 0; out_619228651928883812[122] = 0; out_619228651928883812[123] = 0; out_619228651928883812[124] = 0; out_619228651928883812[125] = 0; out_619228651928883812[126] = 0; out_619228651928883812[127] = 0; out_619228651928883812[128] = 0; out_619228651928883812[129] = 0.5*state[5]; out_619228651928883812[130] = -0.5*state[4]; out_619228651928883812[131] = 0.5*state[3]; out_619228651928883812[132] = 0; out_619228651928883812[133] = 0; out_619228651928883812[134] = 0; out_619228651928883812[135] = 0; out_619228651928883812[136] = 0; out_619228651928883812[137] = 0; out_619228651928883812[138] = 0; out_619228651928883812[139] = 0; out_619228651928883812[140] = 0; out_619228651928883812[141] = 0; out_619228651928883812[142] = 0; out_619228651928883812[143] = 0; out_619228651928883812[144] = 0; out_619228651928883812[145] = 0; out_619228651928883812[146] = 0; out_619228651928883812[147] = 0; out_619228651928883812[148] = 0; out_619228651928883812[149] = 0; out_619228651928883812[150] = 0; out_619228651928883812[151] = 0; out_619228651928883812[152] = 0; out_619228651928883812[153] = 1.0; out_619228651928883812[154] = 0; out_619228651928883812[155] = 0; out_619228651928883812[156] = 0; out_619228651928883812[157] = 0; out_619228651928883812[158] = 0; out_619228651928883812[159] = 0; out_619228651928883812[160] = 0; out_619228651928883812[161] = 0; out_619228651928883812[162] = 0; out_619228651928883812[163] = 0; out_619228651928883812[164] = 0; out_619228651928883812[165] = 0; out_619228651928883812[166] = 0; out_619228651928883812[167] = 0; out_619228651928883812[168] = 0; out_619228651928883812[169] = 0; out_619228651928883812[170] = 0; out_619228651928883812[171] = 0; out_619228651928883812[172] = 0; out_619228651928883812[173] = 0; out_619228651928883812[174] = 0; out_619228651928883812[175] = 1.0; out_619228651928883812[176] = 0; out_619228651928883812[177] = 0; out_619228651928883812[178] = 0; out_619228651928883812[179] = 0; out_619228651928883812[180] = 0; out_619228651928883812[181] = 0; out_619228651928883812[182] = 0; out_619228651928883812[183] = 0; out_619228651928883812[184] = 0; out_619228651928883812[185] = 0; out_619228651928883812[186] = 0; out_619228651928883812[187] = 0; out_619228651928883812[188] = 0; out_619228651928883812[189] = 0; out_619228651928883812[190] = 0; out_619228651928883812[191] = 0; out_619228651928883812[192] = 0; out_619228651928883812[193] = 0; out_619228651928883812[194] = 0; out_619228651928883812[195] = 0; out_619228651928883812[196] = 0; out_619228651928883812[197] = 1.0; out_619228651928883812[198] = 0; out_619228651928883812[199] = 0; out_619228651928883812[200] = 0; out_619228651928883812[201] = 0; out_619228651928883812[202] = 0; out_619228651928883812[203] = 0; out_619228651928883812[204] = 0; out_619228651928883812[205] = 0; out_619228651928883812[206] = 0; out_619228651928883812[207] = 0; out_619228651928883812[208] = 0; out_619228651928883812[209] = 0; out_619228651928883812[210] = 0; out_619228651928883812[211] = 0; out_619228651928883812[212] = 0; out_619228651928883812[213] = 0; out_619228651928883812[214] = 0; out_619228651928883812[215] = 0; out_619228651928883812[216] = 0; out_619228651928883812[217] = 0; out_619228651928883812[218] = 0; out_619228651928883812[219] = 1.0; out_619228651928883812[220] = 0; out_619228651928883812[221] = 0; out_619228651928883812[222] = 0; out_619228651928883812[223] = 0; out_619228651928883812[224] = 0; out_619228651928883812[225] = 0; out_619228651928883812[226] = 0; out_619228651928883812[227] = 0; out_619228651928883812[228] = 0; out_619228651928883812[229] = 0; out_619228651928883812[230] = 0; out_619228651928883812[231] = 0; out_619228651928883812[232] = 0; out_619228651928883812[233] = 0; out_619228651928883812[234] = 0; out_619228651928883812[235] = 0; out_619228651928883812[236] = 0; out_619228651928883812[237] = 0; out_619228651928883812[238] = 0; out_619228651928883812[239] = 0; out_619228651928883812[240] = 0; out_619228651928883812[241] = 1.0; out_619228651928883812[242] = 0; out_619228651928883812[243] = 0; out_619228651928883812[244] = 0; out_619228651928883812[245] = 0; out_619228651928883812[246] = 0; out_619228651928883812[247] = 0; out_619228651928883812[248] = 0; out_619228651928883812[249] = 0; out_619228651928883812[250] = 0; out_619228651928883812[251] = 0; out_619228651928883812[252] = 0; out_619228651928883812[253] = 0; out_619228651928883812[254] = 0; out_619228651928883812[255] = 0; out_619228651928883812[256] = 0; out_619228651928883812[257] = 0; out_619228651928883812[258] = 0; out_619228651928883812[259] = 0; out_619228651928883812[260] = 0; out_619228651928883812[261] = 0; out_619228651928883812[262] = 0; out_619228651928883812[263] = 1.0; out_619228651928883812[264] = 0; out_619228651928883812[265] = 0; out_619228651928883812[266] = 0; out_619228651928883812[267] = 0; out_619228651928883812[268] = 0; out_619228651928883812[269] = 0; out_619228651928883812[270] = 0; out_619228651928883812[271] = 0; out_619228651928883812[272] = 0; out_619228651928883812[273] = 0; out_619228651928883812[274] = 0; out_619228651928883812[275] = 0; out_619228651928883812[276] = 0; out_619228651928883812[277] = 0; out_619228651928883812[278] = 0; out_619228651928883812[279] = 0; out_619228651928883812[280] = 0; out_619228651928883812[281] = 0; out_619228651928883812[282] = 0; out_619228651928883812[283] = 0; out_619228651928883812[284] = 0; out_619228651928883812[285] = 1.0; out_619228651928883812[286] = 0; out_619228651928883812[287] = 0; out_619228651928883812[288] = 0; out_619228651928883812[289] = 0; out_619228651928883812[290] = 0; out_619228651928883812[291] = 0; out_619228651928883812[292] = 0; out_619228651928883812[293] = 0; out_619228651928883812[294] = 0; out_619228651928883812[295] = 0; out_619228651928883812[296] = 0; out_619228651928883812[297] = 0; out_619228651928883812[298] = 0; out_619228651928883812[299] = 0; out_619228651928883812[300] = 0; out_619228651928883812[301] = 0; out_619228651928883812[302] = 0; out_619228651928883812[303] = 0; out_619228651928883812[304] = 0; out_619228651928883812[305] = 0; out_619228651928883812[306] = 0; out_619228651928883812[307] = 1.0; out_619228651928883812[308] = 0; out_619228651928883812[309] = 0; out_619228651928883812[310] = 0; out_619228651928883812[311] = 0; out_619228651928883812[312] = 0; out_619228651928883812[313] = 0; out_619228651928883812[314] = 0; out_619228651928883812[315] = 0; out_619228651928883812[316] = 0; out_619228651928883812[317] = 0; out_619228651928883812[318] = 0; out_619228651928883812[319] = 0; out_619228651928883812[320] = 0; out_619228651928883812[321] = 0; out_619228651928883812[322] = 0; out_619228651928883812[323] = 0; out_619228651928883812[324] = 0; out_619228651928883812[325] = 0; out_619228651928883812[326] = 0; out_619228651928883812[327] = 0; out_619228651928883812[328] = 0; out_619228651928883812[329] = 1.0; out_619228651928883812[330] = 0; out_619228651928883812[331] = 0; out_619228651928883812[332] = 0; out_619228651928883812[333] = 0; out_619228651928883812[334] = 0; out_619228651928883812[335] = 0; out_619228651928883812[336] = 0; out_619228651928883812[337] = 0; out_619228651928883812[338] = 0; out_619228651928883812[339] = 0; out_619228651928883812[340] = 0; out_619228651928883812[341] = 0; out_619228651928883812[342] = 0; out_619228651928883812[343] = 0; out_619228651928883812[344] = 0; out_619228651928883812[345] = 0; out_619228651928883812[346] = 0; out_619228651928883812[347] = 0; out_619228651928883812[348] = 0; out_619228651928883812[349] = 0; out_619228651928883812[350] = 0; out_619228651928883812[351] = 1.0; out_619228651928883812[352] = 0; out_619228651928883812[353] = 0; out_619228651928883812[354] = 0; out_619228651928883812[355] = 0; out_619228651928883812[356] = 0; out_619228651928883812[357] = 0; out_619228651928883812[358] = 0; out_619228651928883812[359] = 0; out_619228651928883812[360] = 0; out_619228651928883812[361] = 0; out_619228651928883812[362] = 0; out_619228651928883812[363] = 0; out_619228651928883812[364] = 0; out_619228651928883812[365] = 0; out_619228651928883812[366] = 0; out_619228651928883812[367] = 0; out_619228651928883812[368] = 0; out_619228651928883812[369] = 0; out_619228651928883812[370] = 0; out_619228651928883812[371] = 0; out_619228651928883812[372] = 0; out_619228651928883812[373] = 1.0; out_619228651928883812[374] = 0; out_619228651928883812[375] = 0; out_619228651928883812[376] = 0; out_619228651928883812[377] = 0; out_619228651928883812[378] = 0; out_619228651928883812[379] = 0; out_619228651928883812[380] = 0; out_619228651928883812[381] = 0; out_619228651928883812[382] = 0; out_619228651928883812[383] = 0; out_619228651928883812[384] = 0; out_619228651928883812[385] = 0; out_619228651928883812[386] = 0; out_619228651928883812[387] = 0; out_619228651928883812[388] = 0; out_619228651928883812[389] = 0; out_619228651928883812[390] = 0; out_619228651928883812[391] = 0; out_619228651928883812[392] = 0; out_619228651928883812[393] = 0; out_619228651928883812[394] = 0; out_619228651928883812[395] = 1.0; out_619228651928883812[396] = 0; out_619228651928883812[397] = 0; out_619228651928883812[398] = 0; out_619228651928883812[399] = 0; out_619228651928883812[400] = 0; out_619228651928883812[401] = 0; out_619228651928883812[402] = 0; out_619228651928883812[403] = 0; out_619228651928883812[404] = 0; out_619228651928883812[405] = 0; out_619228651928883812[406] = 0; out_619228651928883812[407] = 0; out_619228651928883812[408] = 0; out_619228651928883812[409] = 0; out_619228651928883812[410] = 0; out_619228651928883812[411] = 0; out_619228651928883812[412] = 0; out_619228651928883812[413] = 0; out_619228651928883812[414] = 0; out_619228651928883812[415] = 0; out_619228651928883812[416] = 0; out_619228651928883812[417] = 1.0; out_619228651928883812[418] = 0; out_619228651928883812[419] = 0; out_619228651928883812[420] = 0; out_619228651928883812[421] = 0; out_619228651928883812[422] = 0; out_619228651928883812[423] = 0; out_619228651928883812[424] = 0; out_619228651928883812[425] = 0; out_619228651928883812[426] = 0; out_619228651928883812[427] = 0; out_619228651928883812[428] = 0; out_619228651928883812[429] = 0; out_619228651928883812[430] = 0; out_619228651928883812[431] = 0; out_619228651928883812[432] = 0; out_619228651928883812[433] = 0; out_619228651928883812[434] = 0; out_619228651928883812[435] = 0; out_619228651928883812[436] = 0; out_619228651928883812[437] = 0; out_619228651928883812[438] = 0; out_619228651928883812[439] = 1.0; out_619228651928883812[440] = 0; out_619228651928883812[441] = 0; out_619228651928883812[442] = 0; out_619228651928883812[443] = 0; out_619228651928883812[444] = 0; out_619228651928883812[445] = 0; out_619228651928883812[446] = 0; out_619228651928883812[447] = 0; out_619228651928883812[448] = 0; out_619228651928883812[449] = 0; out_619228651928883812[450] = 0; out_619228651928883812[451] = 0; out_619228651928883812[452] = 0; out_619228651928883812[453] = 0; out_619228651928883812[454] = 0; out_619228651928883812[455] = 0; out_619228651928883812[456] = 0; out_619228651928883812[457] = 0; out_619228651928883812[458] = 0; out_619228651928883812[459] = 0; out_619228651928883812[460] = 0; out_619228651928883812[461] = 1.0; } void f_fun(double *state, double dt, double *out_7848720022958012510) { out_7848720022958012510[0] = dt*state[7] + state[0]; out_7848720022958012510[1] = dt*state[8] + state[1]; out_7848720022958012510[2] = dt*state[9] + state[2]; out_7848720022958012510[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; out_7848720022958012510[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; out_7848720022958012510[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; out_7848720022958012510[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; out_7848720022958012510[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; out_7848720022958012510[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; out_7848720022958012510[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; out_7848720022958012510[10] = state[10]; out_7848720022958012510[11] = state[11]; out_7848720022958012510[12] = state[12]; out_7848720022958012510[13] = state[13]; out_7848720022958012510[14] = state[14]; out_7848720022958012510[15] = state[15]; out_7848720022958012510[16] = state[16]; out_7848720022958012510[17] = state[17]; out_7848720022958012510[18] = state[18]; out_7848720022958012510[19] = state[19]; out_7848720022958012510[20] = state[20]; out_7848720022958012510[21] = state[21]; } void F_fun(double *state, double dt, double *out_350033378085274248) { out_350033378085274248[0] = 1; out_350033378085274248[1] = 0; out_350033378085274248[2] = 0; out_350033378085274248[3] = 0; out_350033378085274248[4] = 0; out_350033378085274248[5] = 0; out_350033378085274248[6] = dt; out_350033378085274248[7] = 0; out_350033378085274248[8] = 0; out_350033378085274248[9] = 0; out_350033378085274248[10] = 0; out_350033378085274248[11] = 0; out_350033378085274248[12] = 0; out_350033378085274248[13] = 0; out_350033378085274248[14] = 0; out_350033378085274248[15] = 0; out_350033378085274248[16] = 0; out_350033378085274248[17] = 0; out_350033378085274248[18] = 0; out_350033378085274248[19] = 0; out_350033378085274248[20] = 0; out_350033378085274248[21] = 0; out_350033378085274248[22] = 1; out_350033378085274248[23] = 0; out_350033378085274248[24] = 0; out_350033378085274248[25] = 0; out_350033378085274248[26] = 0; out_350033378085274248[27] = 0; out_350033378085274248[28] = dt; out_350033378085274248[29] = 0; out_350033378085274248[30] = 0; out_350033378085274248[31] = 0; out_350033378085274248[32] = 0; out_350033378085274248[33] = 0; out_350033378085274248[34] = 0; out_350033378085274248[35] = 0; out_350033378085274248[36] = 0; out_350033378085274248[37] = 0; out_350033378085274248[38] = 0; out_350033378085274248[39] = 0; out_350033378085274248[40] = 0; out_350033378085274248[41] = 0; out_350033378085274248[42] = 0; out_350033378085274248[43] = 0; out_350033378085274248[44] = 1; out_350033378085274248[45] = 0; out_350033378085274248[46] = 0; out_350033378085274248[47] = 0; out_350033378085274248[48] = 0; out_350033378085274248[49] = 0; out_350033378085274248[50] = dt; out_350033378085274248[51] = 0; out_350033378085274248[52] = 0; out_350033378085274248[53] = 0; out_350033378085274248[54] = 0; out_350033378085274248[55] = 0; out_350033378085274248[56] = 0; out_350033378085274248[57] = 0; out_350033378085274248[58] = 0; out_350033378085274248[59] = 0; out_350033378085274248[60] = 0; out_350033378085274248[61] = 0; out_350033378085274248[62] = 0; out_350033378085274248[63] = 0; out_350033378085274248[64] = 0; out_350033378085274248[65] = 0; out_350033378085274248[66] = 1; out_350033378085274248[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); out_350033378085274248[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); out_350033378085274248[69] = 0; out_350033378085274248[70] = 0; out_350033378085274248[71] = 0; out_350033378085274248[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); out_350033378085274248[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); out_350033378085274248[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); out_350033378085274248[75] = 0; out_350033378085274248[76] = 0; out_350033378085274248[77] = 0; out_350033378085274248[78] = 0; out_350033378085274248[79] = 0; out_350033378085274248[80] = 0; out_350033378085274248[81] = 0; out_350033378085274248[82] = 0; out_350033378085274248[83] = 0; out_350033378085274248[84] = 0; out_350033378085274248[85] = 0; out_350033378085274248[86] = 0; out_350033378085274248[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); out_350033378085274248[88] = 1; out_350033378085274248[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); out_350033378085274248[90] = 0; out_350033378085274248[91] = 0; out_350033378085274248[92] = 0; out_350033378085274248[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); out_350033378085274248[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); out_350033378085274248[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); out_350033378085274248[96] = 0; out_350033378085274248[97] = 0; out_350033378085274248[98] = 0; out_350033378085274248[99] = 0; out_350033378085274248[100] = 0; out_350033378085274248[101] = 0; out_350033378085274248[102] = 0; out_350033378085274248[103] = 0; out_350033378085274248[104] = 0; out_350033378085274248[105] = 0; out_350033378085274248[106] = 0; out_350033378085274248[107] = 0; out_350033378085274248[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); out_350033378085274248[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); out_350033378085274248[110] = 1; out_350033378085274248[111] = 0; out_350033378085274248[112] = 0; out_350033378085274248[113] = 0; out_350033378085274248[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); out_350033378085274248[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); out_350033378085274248[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); out_350033378085274248[117] = 0; out_350033378085274248[118] = 0; out_350033378085274248[119] = 0; out_350033378085274248[120] = 0; out_350033378085274248[121] = 0; out_350033378085274248[122] = 0; out_350033378085274248[123] = 0; out_350033378085274248[124] = 0; out_350033378085274248[125] = 0; out_350033378085274248[126] = 0; out_350033378085274248[127] = 0; out_350033378085274248[128] = 0; out_350033378085274248[129] = 0; out_350033378085274248[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); out_350033378085274248[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); out_350033378085274248[132] = 1; out_350033378085274248[133] = 0; out_350033378085274248[134] = 0; out_350033378085274248[135] = 0; out_350033378085274248[136] = 0; out_350033378085274248[137] = 0; out_350033378085274248[138] = 0; out_350033378085274248[139] = 0; out_350033378085274248[140] = 0; out_350033378085274248[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); out_350033378085274248[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); out_350033378085274248[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); out_350033378085274248[144] = 0; out_350033378085274248[145] = 0; out_350033378085274248[146] = 0; out_350033378085274248[147] = 0; out_350033378085274248[148] = 0; out_350033378085274248[149] = 0; out_350033378085274248[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); out_350033378085274248[151] = 0; out_350033378085274248[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); out_350033378085274248[153] = 0; out_350033378085274248[154] = 1; out_350033378085274248[155] = 0; out_350033378085274248[156] = 0; out_350033378085274248[157] = 0; out_350033378085274248[158] = 0; out_350033378085274248[159] = 0; out_350033378085274248[160] = 0; out_350033378085274248[161] = 0; out_350033378085274248[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); out_350033378085274248[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); out_350033378085274248[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); out_350033378085274248[165] = 0; out_350033378085274248[166] = 0; out_350033378085274248[167] = 0; out_350033378085274248[168] = 0; out_350033378085274248[169] = 0; out_350033378085274248[170] = 0; out_350033378085274248[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); out_350033378085274248[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); out_350033378085274248[173] = 0; out_350033378085274248[174] = 0; out_350033378085274248[175] = 0; out_350033378085274248[176] = 1; out_350033378085274248[177] = 0; out_350033378085274248[178] = 0; out_350033378085274248[179] = 0; out_350033378085274248[180] = 0; out_350033378085274248[181] = 0; out_350033378085274248[182] = 0; out_350033378085274248[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); out_350033378085274248[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); out_350033378085274248[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); out_350033378085274248[186] = 0; out_350033378085274248[187] = 0; out_350033378085274248[188] = 0; out_350033378085274248[189] = 0; out_350033378085274248[190] = 0; out_350033378085274248[191] = 0; out_350033378085274248[192] = 0; out_350033378085274248[193] = 0; out_350033378085274248[194] = 0; out_350033378085274248[195] = 0; out_350033378085274248[196] = 0; out_350033378085274248[197] = 0; out_350033378085274248[198] = 1; out_350033378085274248[199] = 0; out_350033378085274248[200] = 0; out_350033378085274248[201] = 0; out_350033378085274248[202] = 0; out_350033378085274248[203] = 0; out_350033378085274248[204] = 0; out_350033378085274248[205] = 0; out_350033378085274248[206] = 0; out_350033378085274248[207] = 0; out_350033378085274248[208] = 0; out_350033378085274248[209] = 0; out_350033378085274248[210] = 0; out_350033378085274248[211] = 0; out_350033378085274248[212] = 0; out_350033378085274248[213] = 0; out_350033378085274248[214] = 0; out_350033378085274248[215] = 0; out_350033378085274248[216] = 0; out_350033378085274248[217] = 0; out_350033378085274248[218] = 0; out_350033378085274248[219] = 0; out_350033378085274248[220] = 1; out_350033378085274248[221] = 0; out_350033378085274248[222] = 0; out_350033378085274248[223] = 0; out_350033378085274248[224] = 0; out_350033378085274248[225] = 0; out_350033378085274248[226] = 0; out_350033378085274248[227] = 0; out_350033378085274248[228] = 0; out_350033378085274248[229] = 0; out_350033378085274248[230] = 0; out_350033378085274248[231] = 0; out_350033378085274248[232] = 0; out_350033378085274248[233] = 0; out_350033378085274248[234] = 0; out_350033378085274248[235] = 0; out_350033378085274248[236] = 0; out_350033378085274248[237] = 0; out_350033378085274248[238] = 0; out_350033378085274248[239] = 0; out_350033378085274248[240] = 0; out_350033378085274248[241] = 0; out_350033378085274248[242] = 1; out_350033378085274248[243] = 0; out_350033378085274248[244] = 0; out_350033378085274248[245] = 0; out_350033378085274248[246] = 0; out_350033378085274248[247] = 0; out_350033378085274248[248] = 0; out_350033378085274248[249] = 0; out_350033378085274248[250] = 0; out_350033378085274248[251] = 0; out_350033378085274248[252] = 0; out_350033378085274248[253] = 0; out_350033378085274248[254] = 0; out_350033378085274248[255] = 0; out_350033378085274248[256] = 0; out_350033378085274248[257] = 0; out_350033378085274248[258] = 0; out_350033378085274248[259] = 0; out_350033378085274248[260] = 0; out_350033378085274248[261] = 0; out_350033378085274248[262] = 0; out_350033378085274248[263] = 0; out_350033378085274248[264] = 1; out_350033378085274248[265] = 0; out_350033378085274248[266] = 0; out_350033378085274248[267] = 0; out_350033378085274248[268] = 0; out_350033378085274248[269] = 0; out_350033378085274248[270] = 0; out_350033378085274248[271] = 0; out_350033378085274248[272] = 0; out_350033378085274248[273] = 0; out_350033378085274248[274] = 0; out_350033378085274248[275] = 0; out_350033378085274248[276] = 0; out_350033378085274248[277] = 0; out_350033378085274248[278] = 0; out_350033378085274248[279] = 0; out_350033378085274248[280] = 0; out_350033378085274248[281] = 0; out_350033378085274248[282] = 0; out_350033378085274248[283] = 0; out_350033378085274248[284] = 0; out_350033378085274248[285] = 0; out_350033378085274248[286] = 1; out_350033378085274248[287] = 0; out_350033378085274248[288] = 0; out_350033378085274248[289] = 0; out_350033378085274248[290] = 0; out_350033378085274248[291] = 0; out_350033378085274248[292] = 0; out_350033378085274248[293] = 0; out_350033378085274248[294] = 0; out_350033378085274248[295] = 0; out_350033378085274248[296] = 0; out_350033378085274248[297] = 0; out_350033378085274248[298] = 0; out_350033378085274248[299] = 0; out_350033378085274248[300] = 0; out_350033378085274248[301] = 0; out_350033378085274248[302] = 0; out_350033378085274248[303] = 0; out_350033378085274248[304] = 0; out_350033378085274248[305] = 0; out_350033378085274248[306] = 0; out_350033378085274248[307] = 0; out_350033378085274248[308] = 1; out_350033378085274248[309] = 0; out_350033378085274248[310] = 0; out_350033378085274248[311] = 0; out_350033378085274248[312] = 0; out_350033378085274248[313] = 0; out_350033378085274248[314] = 0; out_350033378085274248[315] = 0; out_350033378085274248[316] = 0; out_350033378085274248[317] = 0; out_350033378085274248[318] = 0; out_350033378085274248[319] = 0; out_350033378085274248[320] = 0; out_350033378085274248[321] = 0; out_350033378085274248[322] = 0; out_350033378085274248[323] = 0; out_350033378085274248[324] = 0; out_350033378085274248[325] = 0; out_350033378085274248[326] = 0; out_350033378085274248[327] = 0; out_350033378085274248[328] = 0; out_350033378085274248[329] = 0; out_350033378085274248[330] = 1; out_350033378085274248[331] = 0; out_350033378085274248[332] = 0; out_350033378085274248[333] = 0; out_350033378085274248[334] = 0; out_350033378085274248[335] = 0; out_350033378085274248[336] = 0; out_350033378085274248[337] = 0; out_350033378085274248[338] = 0; out_350033378085274248[339] = 0; out_350033378085274248[340] = 0; out_350033378085274248[341] = 0; out_350033378085274248[342] = 0; out_350033378085274248[343] = 0; out_350033378085274248[344] = 0; out_350033378085274248[345] = 0; out_350033378085274248[346] = 0; out_350033378085274248[347] = 0; out_350033378085274248[348] = 0; out_350033378085274248[349] = 0; out_350033378085274248[350] = 0; out_350033378085274248[351] = 0; out_350033378085274248[352] = 1; out_350033378085274248[353] = 0; out_350033378085274248[354] = 0; out_350033378085274248[355] = 0; out_350033378085274248[356] = 0; out_350033378085274248[357] = 0; out_350033378085274248[358] = 0; out_350033378085274248[359] = 0; out_350033378085274248[360] = 0; out_350033378085274248[361] = 0; out_350033378085274248[362] = 0; out_350033378085274248[363] = 0; out_350033378085274248[364] = 0; out_350033378085274248[365] = 0; out_350033378085274248[366] = 0; out_350033378085274248[367] = 0; out_350033378085274248[368] = 0; out_350033378085274248[369] = 0; out_350033378085274248[370] = 0; out_350033378085274248[371] = 0; out_350033378085274248[372] = 0; out_350033378085274248[373] = 0; out_350033378085274248[374] = 1; out_350033378085274248[375] = 0; out_350033378085274248[376] = 0; out_350033378085274248[377] = 0; out_350033378085274248[378] = 0; out_350033378085274248[379] = 0; out_350033378085274248[380] = 0; out_350033378085274248[381] = 0; out_350033378085274248[382] = 0; out_350033378085274248[383] = 0; out_350033378085274248[384] = 0; out_350033378085274248[385] = 0; out_350033378085274248[386] = 0; out_350033378085274248[387] = 0; out_350033378085274248[388] = 0; out_350033378085274248[389] = 0; out_350033378085274248[390] = 0; out_350033378085274248[391] = 0; out_350033378085274248[392] = 0; out_350033378085274248[393] = 0; out_350033378085274248[394] = 0; out_350033378085274248[395] = 0; out_350033378085274248[396] = 1; out_350033378085274248[397] = 0; out_350033378085274248[398] = 0; out_350033378085274248[399] = 0; out_350033378085274248[400] = 0; out_350033378085274248[401] = 0; out_350033378085274248[402] = 0; out_350033378085274248[403] = 0; out_350033378085274248[404] = 0; out_350033378085274248[405] = 0; out_350033378085274248[406] = 0; out_350033378085274248[407] = 0; out_350033378085274248[408] = 0; out_350033378085274248[409] = 0; out_350033378085274248[410] = 0; out_350033378085274248[411] = 0; out_350033378085274248[412] = 0; out_350033378085274248[413] = 0; out_350033378085274248[414] = 0; out_350033378085274248[415] = 0; out_350033378085274248[416] = 0; out_350033378085274248[417] = 0; out_350033378085274248[418] = 1; out_350033378085274248[419] = 0; out_350033378085274248[420] = 0; out_350033378085274248[421] = 0; out_350033378085274248[422] = 0; out_350033378085274248[423] = 0; out_350033378085274248[424] = 0; out_350033378085274248[425] = 0; out_350033378085274248[426] = 0; out_350033378085274248[427] = 0; out_350033378085274248[428] = 0; out_350033378085274248[429] = 0; out_350033378085274248[430] = 0; out_350033378085274248[431] = 0; out_350033378085274248[432] = 0; out_350033378085274248[433] = 0; out_350033378085274248[434] = 0; out_350033378085274248[435] = 0; out_350033378085274248[436] = 0; out_350033378085274248[437] = 0; out_350033378085274248[438] = 0; out_350033378085274248[439] = 0; out_350033378085274248[440] = 1; } void h_4(double *state, double *unused, double *out_6806577712686989055) { out_6806577712686989055[0] = state[10] + state[13]; out_6806577712686989055[1] = state[11] + state[14]; out_6806577712686989055[2] = state[12] + state[15]; } void H_4(double *state, double *unused, double *out_5600576627277883540) { out_5600576627277883540[0] = 0; out_5600576627277883540[1] = 0; out_5600576627277883540[2] = 0; out_5600576627277883540[3] = 0; out_5600576627277883540[4] = 0; out_5600576627277883540[5] = 0; out_5600576627277883540[6] = 0; out_5600576627277883540[7] = 0; out_5600576627277883540[8] = 0; out_5600576627277883540[9] = 0; out_5600576627277883540[10] = 1; out_5600576627277883540[11] = 0; out_5600576627277883540[12] = 0; out_5600576627277883540[13] = 1; out_5600576627277883540[14] = 0; out_5600576627277883540[15] = 0; out_5600576627277883540[16] = 0; out_5600576627277883540[17] = 0; out_5600576627277883540[18] = 0; out_5600576627277883540[19] = 0; out_5600576627277883540[20] = 0; out_5600576627277883540[21] = 0; out_5600576627277883540[22] = 0; out_5600576627277883540[23] = 0; out_5600576627277883540[24] = 0; out_5600576627277883540[25] = 0; out_5600576627277883540[26] = 0; out_5600576627277883540[27] = 0; out_5600576627277883540[28] = 0; out_5600576627277883540[29] = 0; out_5600576627277883540[30] = 0; out_5600576627277883540[31] = 0; out_5600576627277883540[32] = 0; out_5600576627277883540[33] = 1; out_5600576627277883540[34] = 0; out_5600576627277883540[35] = 0; out_5600576627277883540[36] = 1; out_5600576627277883540[37] = 0; out_5600576627277883540[38] = 0; out_5600576627277883540[39] = 0; out_5600576627277883540[40] = 0; out_5600576627277883540[41] = 0; out_5600576627277883540[42] = 0; out_5600576627277883540[43] = 0; out_5600576627277883540[44] = 0; out_5600576627277883540[45] = 0; out_5600576627277883540[46] = 0; out_5600576627277883540[47] = 0; out_5600576627277883540[48] = 0; out_5600576627277883540[49] = 0; out_5600576627277883540[50] = 0; out_5600576627277883540[51] = 0; out_5600576627277883540[52] = 0; out_5600576627277883540[53] = 0; out_5600576627277883540[54] = 0; out_5600576627277883540[55] = 0; out_5600576627277883540[56] = 1; out_5600576627277883540[57] = 0; out_5600576627277883540[58] = 0; out_5600576627277883540[59] = 1; out_5600576627277883540[60] = 0; out_5600576627277883540[61] = 0; out_5600576627277883540[62] = 0; out_5600576627277883540[63] = 0; out_5600576627277883540[64] = 0; out_5600576627277883540[65] = 0; } void h_9(double *state, double *unused, double *out_8013092661027487392) { out_8013092661027487392[0] = state[10]; out_8013092661027487392[1] = state[11]; out_8013092661027487392[2] = state[12]; } void H_9(double *state, double *unused, double *out_5558948511167220606) { out_5558948511167220606[0] = 0; out_5558948511167220606[1] = 0; out_5558948511167220606[2] = 0; out_5558948511167220606[3] = 0; out_5558948511167220606[4] = 0; out_5558948511167220606[5] = 0; out_5558948511167220606[6] = 0; out_5558948511167220606[7] = 0; out_5558948511167220606[8] = 0; out_5558948511167220606[9] = 0; out_5558948511167220606[10] = 1; out_5558948511167220606[11] = 0; out_5558948511167220606[12] = 0; out_5558948511167220606[13] = 0; out_5558948511167220606[14] = 0; out_5558948511167220606[15] = 0; out_5558948511167220606[16] = 0; out_5558948511167220606[17] = 0; out_5558948511167220606[18] = 0; out_5558948511167220606[19] = 0; out_5558948511167220606[20] = 0; out_5558948511167220606[21] = 0; out_5558948511167220606[22] = 0; out_5558948511167220606[23] = 0; out_5558948511167220606[24] = 0; out_5558948511167220606[25] = 0; out_5558948511167220606[26] = 0; out_5558948511167220606[27] = 0; out_5558948511167220606[28] = 0; out_5558948511167220606[29] = 0; out_5558948511167220606[30] = 0; out_5558948511167220606[31] = 0; out_5558948511167220606[32] = 0; out_5558948511167220606[33] = 1; out_5558948511167220606[34] = 0; out_5558948511167220606[35] = 0; out_5558948511167220606[36] = 0; out_5558948511167220606[37] = 0; out_5558948511167220606[38] = 0; out_5558948511167220606[39] = 0; out_5558948511167220606[40] = 0; out_5558948511167220606[41] = 0; out_5558948511167220606[42] = 0; out_5558948511167220606[43] = 0; out_5558948511167220606[44] = 0; out_5558948511167220606[45] = 0; out_5558948511167220606[46] = 0; out_5558948511167220606[47] = 0; out_5558948511167220606[48] = 0; out_5558948511167220606[49] = 0; out_5558948511167220606[50] = 0; out_5558948511167220606[51] = 0; out_5558948511167220606[52] = 0; out_5558948511167220606[53] = 0; out_5558948511167220606[54] = 0; out_5558948511167220606[55] = 0; out_5558948511167220606[56] = 1; out_5558948511167220606[57] = 0; out_5558948511167220606[58] = 0; out_5558948511167220606[59] = 0; out_5558948511167220606[60] = 0; out_5558948511167220606[61] = 0; out_5558948511167220606[62] = 0; out_5558948511167220606[63] = 0; out_5558948511167220606[64] = 0; out_5558948511167220606[65] = 0; } void h_10(double *state, double *unused, double *out_3244487682903101191) { out_3244487682903101191[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; out_3244487682903101191[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; out_3244487682903101191[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; } void H_10(double *state, double *unused, double *out_8030959225542197370) { out_8030959225542197370[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); out_8030959225542197370[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; out_8030959225542197370[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; out_8030959225542197370[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; out_8030959225542197370[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; out_8030959225542197370[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; out_8030959225542197370[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; out_8030959225542197370[7] = 0; out_8030959225542197370[8] = 0; out_8030959225542197370[9] = 0; out_8030959225542197370[10] = 0; out_8030959225542197370[11] = 0; out_8030959225542197370[12] = 0; out_8030959225542197370[13] = 0; out_8030959225542197370[14] = 0; out_8030959225542197370[15] = 0; out_8030959225542197370[16] = 1; out_8030959225542197370[17] = 0; out_8030959225542197370[18] = 0; out_8030959225542197370[19] = 1; out_8030959225542197370[20] = 0; out_8030959225542197370[21] = 0; out_8030959225542197370[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; out_8030959225542197370[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); out_8030959225542197370[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; out_8030959225542197370[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; out_8030959225542197370[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; out_8030959225542197370[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; out_8030959225542197370[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; out_8030959225542197370[29] = 0; out_8030959225542197370[30] = 0; out_8030959225542197370[31] = 0; out_8030959225542197370[32] = 0; out_8030959225542197370[33] = 0; out_8030959225542197370[34] = 0; out_8030959225542197370[35] = 0; out_8030959225542197370[36] = 0; out_8030959225542197370[37] = 0; out_8030959225542197370[38] = 0; out_8030959225542197370[39] = 1; out_8030959225542197370[40] = 0; out_8030959225542197370[41] = 0; out_8030959225542197370[42] = 1; out_8030959225542197370[43] = 0; out_8030959225542197370[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; out_8030959225542197370[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; out_8030959225542197370[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); out_8030959225542197370[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; out_8030959225542197370[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; out_8030959225542197370[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; out_8030959225542197370[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; out_8030959225542197370[51] = 0; out_8030959225542197370[52] = 0; out_8030959225542197370[53] = 0; out_8030959225542197370[54] = 0; out_8030959225542197370[55] = 0; out_8030959225542197370[56] = 0; out_8030959225542197370[57] = 0; out_8030959225542197370[58] = 0; out_8030959225542197370[59] = 0; out_8030959225542197370[60] = 0; out_8030959225542197370[61] = 0; out_8030959225542197370[62] = 1; out_8030959225542197370[63] = 0; out_8030959225542197370[64] = 0; out_8030959225542197370[65] = 1; } void h_12(double *state, double *unused, double *out_2391008662114364333) { out_2391008662114364333[0] = state[0]; out_2391008662114364333[1] = state[1]; out_2391008662114364333[2] = state[2]; } void H_12(double *state, double *unused, double *out_7826711038399706281) { out_7826711038399706281[0] = 1; out_7826711038399706281[1] = 0; out_7826711038399706281[2] = 0; out_7826711038399706281[3] = 0; out_7826711038399706281[4] = 0; out_7826711038399706281[5] = 0; out_7826711038399706281[6] = 0; out_7826711038399706281[7] = 0; out_7826711038399706281[8] = 0; out_7826711038399706281[9] = 0; out_7826711038399706281[10] = 0; out_7826711038399706281[11] = 0; out_7826711038399706281[12] = 0; out_7826711038399706281[13] = 0; out_7826711038399706281[14] = 0; out_7826711038399706281[15] = 0; out_7826711038399706281[16] = 0; out_7826711038399706281[17] = 0; out_7826711038399706281[18] = 0; out_7826711038399706281[19] = 0; out_7826711038399706281[20] = 0; out_7826711038399706281[21] = 0; out_7826711038399706281[22] = 0; out_7826711038399706281[23] = 1; out_7826711038399706281[24] = 0; out_7826711038399706281[25] = 0; out_7826711038399706281[26] = 0; out_7826711038399706281[27] = 0; out_7826711038399706281[28] = 0; out_7826711038399706281[29] = 0; out_7826711038399706281[30] = 0; out_7826711038399706281[31] = 0; out_7826711038399706281[32] = 0; out_7826711038399706281[33] = 0; out_7826711038399706281[34] = 0; out_7826711038399706281[35] = 0; out_7826711038399706281[36] = 0; out_7826711038399706281[37] = 0; out_7826711038399706281[38] = 0; out_7826711038399706281[39] = 0; out_7826711038399706281[40] = 0; out_7826711038399706281[41] = 0; out_7826711038399706281[42] = 0; out_7826711038399706281[43] = 0; out_7826711038399706281[44] = 0; out_7826711038399706281[45] = 0; out_7826711038399706281[46] = 1; out_7826711038399706281[47] = 0; out_7826711038399706281[48] = 0; out_7826711038399706281[49] = 0; out_7826711038399706281[50] = 0; out_7826711038399706281[51] = 0; out_7826711038399706281[52] = 0; out_7826711038399706281[53] = 0; out_7826711038399706281[54] = 0; out_7826711038399706281[55] = 0; out_7826711038399706281[56] = 0; out_7826711038399706281[57] = 0; out_7826711038399706281[58] = 0; out_7826711038399706281[59] = 0; out_7826711038399706281[60] = 0; out_7826711038399706281[61] = 0; out_7826711038399706281[62] = 0; out_7826711038399706281[63] = 0; out_7826711038399706281[64] = 0; out_7826711038399706281[65] = 0; } void h_35(double *state, double *unused, double *out_7297975473935584261) { out_7297975473935584261[0] = state[7]; out_7297975473935584261[1] = state[8]; out_7297975473935584261[2] = state[9]; } void H_35(double *state, double *unused, double *out_8967238684650490916) { out_8967238684650490916[0] = 0; out_8967238684650490916[1] = 0; out_8967238684650490916[2] = 0; out_8967238684650490916[3] = 0; out_8967238684650490916[4] = 0; out_8967238684650490916[5] = 0; out_8967238684650490916[6] = 0; out_8967238684650490916[7] = 1; out_8967238684650490916[8] = 0; out_8967238684650490916[9] = 0; out_8967238684650490916[10] = 0; out_8967238684650490916[11] = 0; out_8967238684650490916[12] = 0; out_8967238684650490916[13] = 0; out_8967238684650490916[14] = 0; out_8967238684650490916[15] = 0; out_8967238684650490916[16] = 0; out_8967238684650490916[17] = 0; out_8967238684650490916[18] = 0; out_8967238684650490916[19] = 0; out_8967238684650490916[20] = 0; out_8967238684650490916[21] = 0; out_8967238684650490916[22] = 0; out_8967238684650490916[23] = 0; out_8967238684650490916[24] = 0; out_8967238684650490916[25] = 0; out_8967238684650490916[26] = 0; out_8967238684650490916[27] = 0; out_8967238684650490916[28] = 0; out_8967238684650490916[29] = 0; out_8967238684650490916[30] = 1; out_8967238684650490916[31] = 0; out_8967238684650490916[32] = 0; out_8967238684650490916[33] = 0; out_8967238684650490916[34] = 0; out_8967238684650490916[35] = 0; out_8967238684650490916[36] = 0; out_8967238684650490916[37] = 0; out_8967238684650490916[38] = 0; out_8967238684650490916[39] = 0; out_8967238684650490916[40] = 0; out_8967238684650490916[41] = 0; out_8967238684650490916[42] = 0; out_8967238684650490916[43] = 0; out_8967238684650490916[44] = 0; out_8967238684650490916[45] = 0; out_8967238684650490916[46] = 0; out_8967238684650490916[47] = 0; out_8967238684650490916[48] = 0; out_8967238684650490916[49] = 0; out_8967238684650490916[50] = 0; out_8967238684650490916[51] = 0; out_8967238684650490916[52] = 0; out_8967238684650490916[53] = 1; out_8967238684650490916[54] = 0; out_8967238684650490916[55] = 0; out_8967238684650490916[56] = 0; out_8967238684650490916[57] = 0; out_8967238684650490916[58] = 0; out_8967238684650490916[59] = 0; out_8967238684650490916[60] = 0; out_8967238684650490916[61] = 0; out_8967238684650490916[62] = 0; out_8967238684650490916[63] = 0; out_8967238684650490916[64] = 0; out_8967238684650490916[65] = 0; } void h_32(double *state, double *unused, double *out_5218610459212142195) { out_5218610459212142195[0] = state[3]; out_5218610459212142195[1] = state[4]; out_5218610459212142195[2] = state[5]; out_5218610459212142195[3] = state[6]; } void H_32(double *state, double *unused, double *out_9098403469591231385) { out_9098403469591231385[0] = 0; out_9098403469591231385[1] = 0; out_9098403469591231385[2] = 0; out_9098403469591231385[3] = 1; out_9098403469591231385[4] = 0; out_9098403469591231385[5] = 0; out_9098403469591231385[6] = 0; out_9098403469591231385[7] = 0; out_9098403469591231385[8] = 0; out_9098403469591231385[9] = 0; out_9098403469591231385[10] = 0; out_9098403469591231385[11] = 0; out_9098403469591231385[12] = 0; out_9098403469591231385[13] = 0; out_9098403469591231385[14] = 0; out_9098403469591231385[15] = 0; out_9098403469591231385[16] = 0; out_9098403469591231385[17] = 0; out_9098403469591231385[18] = 0; out_9098403469591231385[19] = 0; out_9098403469591231385[20] = 0; out_9098403469591231385[21] = 0; out_9098403469591231385[22] = 0; out_9098403469591231385[23] = 0; out_9098403469591231385[24] = 0; out_9098403469591231385[25] = 0; out_9098403469591231385[26] = 1; out_9098403469591231385[27] = 0; out_9098403469591231385[28] = 0; out_9098403469591231385[29] = 0; out_9098403469591231385[30] = 0; out_9098403469591231385[31] = 0; out_9098403469591231385[32] = 0; out_9098403469591231385[33] = 0; out_9098403469591231385[34] = 0; out_9098403469591231385[35] = 0; out_9098403469591231385[36] = 0; out_9098403469591231385[37] = 0; out_9098403469591231385[38] = 0; out_9098403469591231385[39] = 0; out_9098403469591231385[40] = 0; out_9098403469591231385[41] = 0; out_9098403469591231385[42] = 0; out_9098403469591231385[43] = 0; out_9098403469591231385[44] = 0; out_9098403469591231385[45] = 0; out_9098403469591231385[46] = 0; out_9098403469591231385[47] = 0; out_9098403469591231385[48] = 0; out_9098403469591231385[49] = 1; out_9098403469591231385[50] = 0; out_9098403469591231385[51] = 0; out_9098403469591231385[52] = 0; out_9098403469591231385[53] = 0; out_9098403469591231385[54] = 0; out_9098403469591231385[55] = 0; out_9098403469591231385[56] = 0; out_9098403469591231385[57] = 0; out_9098403469591231385[58] = 0; out_9098403469591231385[59] = 0; out_9098403469591231385[60] = 0; out_9098403469591231385[61] = 0; out_9098403469591231385[62] = 0; out_9098403469591231385[63] = 0; out_9098403469591231385[64] = 0; out_9098403469591231385[65] = 0; out_9098403469591231385[66] = 0; out_9098403469591231385[67] = 0; out_9098403469591231385[68] = 0; out_9098403469591231385[69] = 0; out_9098403469591231385[70] = 0; out_9098403469591231385[71] = 0; out_9098403469591231385[72] = 1; out_9098403469591231385[73] = 0; out_9098403469591231385[74] = 0; out_9098403469591231385[75] = 0; out_9098403469591231385[76] = 0; out_9098403469591231385[77] = 0; out_9098403469591231385[78] = 0; out_9098403469591231385[79] = 0; out_9098403469591231385[80] = 0; out_9098403469591231385[81] = 0; out_9098403469591231385[82] = 0; out_9098403469591231385[83] = 0; out_9098403469591231385[84] = 0; out_9098403469591231385[85] = 0; out_9098403469591231385[86] = 0; out_9098403469591231385[87] = 0; } void h_13(double *state, double *unused, double *out_7462688905416857194) { out_7462688905416857194[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; out_7462688905416857194[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; out_7462688905416857194[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; } void H_13(double *state, double *unused, double *out_6584715849578254444) { out_6584715849578254444[0] = 0; out_6584715849578254444[1] = 0; out_6584715849578254444[2] = 0; out_6584715849578254444[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; out_6584715849578254444[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_6584715849578254444[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; out_6584715849578254444[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; out_6584715849578254444[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); out_6584715849578254444[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; out_6584715849578254444[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; out_6584715849578254444[10] = 0; out_6584715849578254444[11] = 0; out_6584715849578254444[12] = 0; out_6584715849578254444[13] = 0; out_6584715849578254444[14] = 0; out_6584715849578254444[15] = 0; out_6584715849578254444[16] = 0; out_6584715849578254444[17] = 0; out_6584715849578254444[18] = 0; out_6584715849578254444[19] = 0; out_6584715849578254444[20] = 0; out_6584715849578254444[21] = 0; out_6584715849578254444[22] = 0; out_6584715849578254444[23] = 0; out_6584715849578254444[24] = 0; out_6584715849578254444[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; out_6584715849578254444[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; out_6584715849578254444[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_6584715849578254444[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; out_6584715849578254444[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; out_6584715849578254444[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); out_6584715849578254444[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; out_6584715849578254444[32] = 0; out_6584715849578254444[33] = 0; out_6584715849578254444[34] = 0; out_6584715849578254444[35] = 0; out_6584715849578254444[36] = 0; out_6584715849578254444[37] = 0; out_6584715849578254444[38] = 0; out_6584715849578254444[39] = 0; out_6584715849578254444[40] = 0; out_6584715849578254444[41] = 0; out_6584715849578254444[42] = 0; out_6584715849578254444[43] = 0; out_6584715849578254444[44] = 0; out_6584715849578254444[45] = 0; out_6584715849578254444[46] = 0; out_6584715849578254444[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; out_6584715849578254444[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; out_6584715849578254444[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; out_6584715849578254444[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_6584715849578254444[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; out_6584715849578254444[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; out_6584715849578254444[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); out_6584715849578254444[54] = 0; out_6584715849578254444[55] = 0; out_6584715849578254444[56] = 0; out_6584715849578254444[57] = 0; out_6584715849578254444[58] = 0; out_6584715849578254444[59] = 0; out_6584715849578254444[60] = 0; out_6584715849578254444[61] = 0; out_6584715849578254444[62] = 0; out_6584715849578254444[63] = 0; out_6584715849578254444[64] = 0; out_6584715849578254444[65] = 0; } void h_14(double *state, double *unused, double *out_8013092661027487392) { out_8013092661027487392[0] = state[10]; out_8013092661027487392[1] = state[11]; out_8013092661027487392[2] = state[12]; } void H_14(double *state, double *unused, double *out_5558948511167220606) { out_5558948511167220606[0] = 0; out_5558948511167220606[1] = 0; out_5558948511167220606[2] = 0; out_5558948511167220606[3] = 0; out_5558948511167220606[4] = 0; out_5558948511167220606[5] = 0; out_5558948511167220606[6] = 0; out_5558948511167220606[7] = 0; out_5558948511167220606[8] = 0; out_5558948511167220606[9] = 0; out_5558948511167220606[10] = 1; out_5558948511167220606[11] = 0; out_5558948511167220606[12] = 0; out_5558948511167220606[13] = 0; out_5558948511167220606[14] = 0; out_5558948511167220606[15] = 0; out_5558948511167220606[16] = 0; out_5558948511167220606[17] = 0; out_5558948511167220606[18] = 0; out_5558948511167220606[19] = 0; out_5558948511167220606[20] = 0; out_5558948511167220606[21] = 0; out_5558948511167220606[22] = 0; out_5558948511167220606[23] = 0; out_5558948511167220606[24] = 0; out_5558948511167220606[25] = 0; out_5558948511167220606[26] = 0; out_5558948511167220606[27] = 0; out_5558948511167220606[28] = 0; out_5558948511167220606[29] = 0; out_5558948511167220606[30] = 0; out_5558948511167220606[31] = 0; out_5558948511167220606[32] = 0; out_5558948511167220606[33] = 1; out_5558948511167220606[34] = 0; out_5558948511167220606[35] = 0; out_5558948511167220606[36] = 0; out_5558948511167220606[37] = 0; out_5558948511167220606[38] = 0; out_5558948511167220606[39] = 0; out_5558948511167220606[40] = 0; out_5558948511167220606[41] = 0; out_5558948511167220606[42] = 0; out_5558948511167220606[43] = 0; out_5558948511167220606[44] = 0; out_5558948511167220606[45] = 0; out_5558948511167220606[46] = 0; out_5558948511167220606[47] = 0; out_5558948511167220606[48] = 0; out_5558948511167220606[49] = 0; out_5558948511167220606[50] = 0; out_5558948511167220606[51] = 0; out_5558948511167220606[52] = 0; out_5558948511167220606[53] = 0; out_5558948511167220606[54] = 0; out_5558948511167220606[55] = 0; out_5558948511167220606[56] = 1; out_5558948511167220606[57] = 0; out_5558948511167220606[58] = 0; out_5558948511167220606[59] = 0; out_5558948511167220606[60] = 0; out_5558948511167220606[61] = 0; out_5558948511167220606[62] = 0; out_5558948511167220606[63] = 0; out_5558948511167220606[64] = 0; out_5558948511167220606[65] = 0; } void h_33(double *state, double *unused, double *out_298940871667614646) { out_298940871667614646[0] = state[16]; out_298940871667614646[1] = state[17]; out_298940871667614646[2] = state[18]; } void H_33(double *state, double *unused, double *out_6328948384420203096) { out_6328948384420203096[0] = 0; out_6328948384420203096[1] = 0; out_6328948384420203096[2] = 0; out_6328948384420203096[3] = 0; out_6328948384420203096[4] = 0; out_6328948384420203096[5] = 0; out_6328948384420203096[6] = 0; out_6328948384420203096[7] = 0; out_6328948384420203096[8] = 0; out_6328948384420203096[9] = 0; out_6328948384420203096[10] = 0; out_6328948384420203096[11] = 0; out_6328948384420203096[12] = 0; out_6328948384420203096[13] = 0; out_6328948384420203096[14] = 0; out_6328948384420203096[15] = 0; out_6328948384420203096[16] = 1; out_6328948384420203096[17] = 0; out_6328948384420203096[18] = 0; out_6328948384420203096[19] = 0; out_6328948384420203096[20] = 0; out_6328948384420203096[21] = 0; out_6328948384420203096[22] = 0; out_6328948384420203096[23] = 0; out_6328948384420203096[24] = 0; out_6328948384420203096[25] = 0; out_6328948384420203096[26] = 0; out_6328948384420203096[27] = 0; out_6328948384420203096[28] = 0; out_6328948384420203096[29] = 0; out_6328948384420203096[30] = 0; out_6328948384420203096[31] = 0; out_6328948384420203096[32] = 0; out_6328948384420203096[33] = 0; out_6328948384420203096[34] = 0; out_6328948384420203096[35] = 0; out_6328948384420203096[36] = 0; out_6328948384420203096[37] = 0; out_6328948384420203096[38] = 0; out_6328948384420203096[39] = 1; out_6328948384420203096[40] = 0; out_6328948384420203096[41] = 0; out_6328948384420203096[42] = 0; out_6328948384420203096[43] = 0; out_6328948384420203096[44] = 0; out_6328948384420203096[45] = 0; out_6328948384420203096[46] = 0; out_6328948384420203096[47] = 0; out_6328948384420203096[48] = 0; out_6328948384420203096[49] = 0; out_6328948384420203096[50] = 0; out_6328948384420203096[51] = 0; out_6328948384420203096[52] = 0; out_6328948384420203096[53] = 0; out_6328948384420203096[54] = 0; out_6328948384420203096[55] = 0; out_6328948384420203096[56] = 0; out_6328948384420203096[57] = 0; out_6328948384420203096[58] = 0; out_6328948384420203096[59] = 0; out_6328948384420203096[60] = 0; out_6328948384420203096[61] = 0; out_6328948384420203096[62] = 1; out_6328948384420203096[63] = 0; out_6328948384420203096[64] = 0; out_6328948384420203096[65] = 0; } #include #include typedef Eigen::Matrix DDM; typedef Eigen::Matrix EEM; typedef Eigen::Matrix DEM; void predict(double *in_x, double *in_P, double *in_Q, double dt) { typedef Eigen::Matrix RRM; double nx[DIM] = {0}; double in_F[EDIM*EDIM] = {0}; // functions from sympy f_fun(in_x, dt, nx); F_fun(in_x, dt, in_F); EEM F(in_F); EEM P(in_P); EEM Q(in_Q); RRM F_main = F.topLeftCorner(MEDIM, MEDIM); P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose(); P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM); P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose(); P = P + dt*Q; // copy out state memcpy(in_x, nx, DIM * sizeof(double)); memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); } // note: extra_args dim only correct when null space projecting // otherwise 1 template void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) { typedef Eigen::Matrix ZZM; typedef Eigen::Matrix ZDM; typedef Eigen::Matrix XEM; //typedef Eigen::Matrix EZM; typedef Eigen::Matrix X1M; typedef Eigen::Matrix XXM; double in_hx[ZDIM] = {0}; double in_H[ZDIM * DIM] = {0}; double in_H_mod[EDIM * DIM] = {0}; double delta_x[EDIM] = {0}; double x_new[DIM] = {0}; // state x, P Eigen::Matrix z(in_z); EEM P(in_P); ZZM pre_R(in_R); // functions from sympy h_fun(in_x, in_ea, in_hx); H_fun(in_x, in_ea, in_H); ZDM pre_H(in_H); // get y (y = z - hx) Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y; X1M y; XXM H; XXM R; if (Hea_fun){ typedef Eigen::Matrix ZAM; double in_Hea[ZDIM * EADIM] = {0}; Hea_fun(in_x, in_ea, in_Hea); ZAM Hea(in_Hea); XXM A = Hea.transpose().fullPivLu().kernel(); y = A.transpose() * pre_y; H = A.transpose() * pre_H; R = A.transpose() * pre_R * A; } else { y = pre_y; H = pre_H; R = pre_R; } // get modified H H_mod_fun(in_x, in_H_mod); DEM H_mod(in_H_mod); XEM H_err = H * H_mod; // Do mahalobis distance test if (MAHA_TEST){ XXM a = (H_err * P * H_err.transpose() + R).inverse(); double maha_dist = y.transpose() * a * y; if (maha_dist > MAHA_THRESHOLD){ R = 1.0e16 * R; } } // Outlier resilient weighting double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum()); // kalman gains and I_KH XXM S = ((H_err * P) * H_err.transpose()) + R/weight; XEM KT = S.fullPivLu().solve(H_err * P.transpose()); //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE? //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose(); //std::cout << "Here is the matrix rot:\n" << K << std::endl; EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err); // update state by injecting dx Eigen::Matrix dx(delta_x); dx = (KT.transpose() * y); memcpy(delta_x, dx.data(), EDIM * sizeof(double)); err_fun(in_x, delta_x, x_new); Eigen::Matrix x(x_new); // update cov P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT); // copy out state memcpy(in_x, x.data(), DIM * sizeof(double)); memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); memcpy(in_z, y.data(), y.rows() * sizeof(double)); } } extern "C" { void live_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_4, H_4, NULL, in_z, in_R, in_ea, MAHA_THRESH_4); } void live_update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_9, H_9, NULL, in_z, in_R, in_ea, MAHA_THRESH_9); } void live_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_10, H_10, NULL, in_z, in_R, in_ea, MAHA_THRESH_10); } void live_update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_12, H_12, NULL, in_z, in_R, in_ea, MAHA_THRESH_12); } void live_update_35(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_35, H_35, NULL, in_z, in_R, in_ea, MAHA_THRESH_35); } void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<4, 3, 0>(in_x, in_P, h_32, H_32, NULL, in_z, in_R, in_ea, MAHA_THRESH_32); } void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_13, H_13, NULL, in_z, in_R, in_ea, MAHA_THRESH_13); } void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33); } void live_H(double *in_vec, double *out_8945704586164018613) { H(in_vec, out_8945704586164018613); } void live_err_fun(double *nom_x, double *delta_x, double *out_8370097340763738192) { err_fun(nom_x, delta_x, out_8370097340763738192); } void live_inv_err_fun(double *nom_x, double *true_x, double *out_1561714653313430456) { inv_err_fun(nom_x, true_x, out_1561714653313430456); } void live_H_mod_fun(double *state, double *out_619228651928883812) { H_mod_fun(state, out_619228651928883812); } void live_f_fun(double *state, double dt, double *out_7848720022958012510) { f_fun(state, dt, out_7848720022958012510); } void live_F_fun(double *state, double dt, double *out_350033378085274248) { F_fun(state, dt, out_350033378085274248); } void live_h_4(double *state, double *unused, double *out_6806577712686989055) { h_4(state, unused, out_6806577712686989055); } void live_H_4(double *state, double *unused, double *out_5600576627277883540) { H_4(state, unused, out_5600576627277883540); } void live_h_9(double *state, double *unused, double *out_8013092661027487392) { h_9(state, unused, out_8013092661027487392); } void live_H_9(double *state, double *unused, double *out_5558948511167220606) { H_9(state, unused, out_5558948511167220606); } void live_h_10(double *state, double *unused, double *out_3244487682903101191) { h_10(state, unused, out_3244487682903101191); } void live_H_10(double *state, double *unused, double *out_8030959225542197370) { H_10(state, unused, out_8030959225542197370); } void live_h_12(double *state, double *unused, double *out_2391008662114364333) { h_12(state, unused, out_2391008662114364333); } void live_H_12(double *state, double *unused, double *out_7826711038399706281) { H_12(state, unused, out_7826711038399706281); } void live_h_35(double *state, double *unused, double *out_7297975473935584261) { h_35(state, unused, out_7297975473935584261); } void live_H_35(double *state, double *unused, double *out_8967238684650490916) { H_35(state, unused, out_8967238684650490916); } void live_h_32(double *state, double *unused, double *out_5218610459212142195) { h_32(state, unused, out_5218610459212142195); } void live_H_32(double *state, double *unused, double *out_9098403469591231385) { H_32(state, unused, out_9098403469591231385); } void live_h_13(double *state, double *unused, double *out_7462688905416857194) { h_13(state, unused, out_7462688905416857194); } void live_H_13(double *state, double *unused, double *out_6584715849578254444) { H_13(state, unused, out_6584715849578254444); } void live_h_14(double *state, double *unused, double *out_8013092661027487392) { h_14(state, unused, out_8013092661027487392); } void live_H_14(double *state, double *unused, double *out_5558948511167220606) { H_14(state, unused, out_5558948511167220606); } void live_h_33(double *state, double *unused, double *out_298940871667614646) { h_33(state, unused, out_298940871667614646); } void live_H_33(double *state, double *unused, double *out_6328948384420203096) { H_33(state, unused, out_6328948384420203096); } void live_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); } } const EKF live = { .name = "live", .kinds = { 4, 9, 10, 12, 35, 32, 13, 14, 33 }, .feature_kinds = { }, .f_fun = live_f_fun, .F_fun = live_F_fun, .err_fun = live_err_fun, .inv_err_fun = live_inv_err_fun, .H_mod_fun = live_H_mod_fun, .predict = live_predict, .hs = { { 4, live_h_4 }, { 9, live_h_9 }, { 10, live_h_10 }, { 12, live_h_12 }, { 35, live_h_35 }, { 32, live_h_32 }, { 13, live_h_13 }, { 14, live_h_14 }, { 33, live_h_33 }, }, .Hs = { { 4, live_H_4 }, { 9, live_H_9 }, { 10, live_H_10 }, { 12, live_H_12 }, { 35, live_H_35 }, { 32, live_H_32 }, { 13, live_H_13 }, { 14, live_H_14 }, { 33, live_H_33 }, }, .updates = { { 4, live_update_4 }, { 9, live_update_9 }, { 10, live_update_10 }, { 12, live_update_12 }, { 35, live_update_35 }, { 32, live_update_32 }, { 13, live_update_13 }, { 14, live_update_14 }, { 33, live_update_33 }, }, .Hes = { }, .sets = { }, .extra_routines = { { "H", live_H }, }, }; ekf_lib_init(live)