#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.14.0 * * * * See http://www.sympy.org/ for more information. * * * * This file is part of 'ekf' * ******************************************************************************/ void H(double *in_vec, double *out_5015980593157513767) { out_5015980593157513767[0] = 0; out_5015980593157513767[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_5015980593157513767[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; out_5015980593157513767[3] = cos(in_vec[1])*cos(in_vec[2]); out_5015980593157513767[4] = sin(in_vec[2])*cos(in_vec[1]); out_5015980593157513767[5] = -sin(in_vec[1]); out_5015980593157513767[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_5015980593157513767[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_5015980593157513767[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_5015980593157513767[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); out_5015980593157513767[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); out_5015980593157513767[11] = sin(in_vec[0])*cos(in_vec[1]); out_5015980593157513767[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_5015980593157513767[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_5015980593157513767[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_5015980593157513767[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); out_5015980593157513767[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); out_5015980593157513767[17] = cos(in_vec[0])*cos(in_vec[1]); } void err_fun(double *nom_x, double *delta_x, double *out_1860005180627469095) { out_1860005180627469095[0] = delta_x[0] + nom_x[0]; out_1860005180627469095[1] = delta_x[1] + nom_x[1]; out_1860005180627469095[2] = delta_x[2] + nom_x[2]; out_1860005180627469095[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_1860005180627469095[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_1860005180627469095[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_1860005180627469095[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_1860005180627469095[7] = delta_x[6] + nom_x[7]; out_1860005180627469095[8] = delta_x[7] + nom_x[8]; out_1860005180627469095[9] = delta_x[8] + nom_x[9]; out_1860005180627469095[10] = delta_x[9] + nom_x[10]; out_1860005180627469095[11] = delta_x[10] + nom_x[11]; out_1860005180627469095[12] = delta_x[11] + nom_x[12]; out_1860005180627469095[13] = delta_x[12] + nom_x[13]; out_1860005180627469095[14] = delta_x[13] + nom_x[14]; out_1860005180627469095[15] = delta_x[14] + nom_x[15]; out_1860005180627469095[16] = delta_x[15] + nom_x[16]; out_1860005180627469095[17] = delta_x[16] + nom_x[17]; out_1860005180627469095[18] = delta_x[17] + nom_x[18]; out_1860005180627469095[19] = delta_x[18] + nom_x[19]; out_1860005180627469095[20] = delta_x[19] + nom_x[20]; out_1860005180627469095[21] = delta_x[20] + nom_x[21]; } void inv_err_fun(double *nom_x, double *true_x, double *out_1902382254042256432) { out_1902382254042256432[0] = -nom_x[0] + true_x[0]; out_1902382254042256432[1] = -nom_x[1] + true_x[1]; out_1902382254042256432[2] = -nom_x[2] + true_x[2]; out_1902382254042256432[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_1902382254042256432[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_1902382254042256432[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_1902382254042256432[6] = -nom_x[7] + true_x[7]; out_1902382254042256432[7] = -nom_x[8] + true_x[8]; out_1902382254042256432[8] = -nom_x[9] + true_x[9]; out_1902382254042256432[9] = -nom_x[10] + true_x[10]; out_1902382254042256432[10] = -nom_x[11] + true_x[11]; out_1902382254042256432[11] = -nom_x[12] + true_x[12]; out_1902382254042256432[12] = -nom_x[13] + true_x[13]; out_1902382254042256432[13] = -nom_x[14] + true_x[14]; out_1902382254042256432[14] = -nom_x[15] + true_x[15]; out_1902382254042256432[15] = -nom_x[16] + true_x[16]; out_1902382254042256432[16] = -nom_x[17] + true_x[17]; out_1902382254042256432[17] = -nom_x[18] + true_x[18]; out_1902382254042256432[18] = -nom_x[19] + true_x[19]; out_1902382254042256432[19] = -nom_x[20] + true_x[20]; out_1902382254042256432[20] = -nom_x[21] + true_x[21]; } void H_mod_fun(double *state, double *out_8394854480302840345) { out_8394854480302840345[0] = 1.0; out_8394854480302840345[1] = 0; out_8394854480302840345[2] = 0; out_8394854480302840345[3] = 0.0; out_8394854480302840345[4] = 0.0; out_8394854480302840345[5] = 0.0; out_8394854480302840345[6] = 0.0; out_8394854480302840345[7] = 0.0; out_8394854480302840345[8] = 0.0; out_8394854480302840345[9] = 0.0; out_8394854480302840345[10] = 0.0; out_8394854480302840345[11] = 0.0; out_8394854480302840345[12] = 0.0; out_8394854480302840345[13] = 0.0; out_8394854480302840345[14] = 0.0; out_8394854480302840345[15] = 0.0; out_8394854480302840345[16] = 0.0; out_8394854480302840345[17] = 0.0; out_8394854480302840345[18] = 0.0; out_8394854480302840345[19] = 0.0; out_8394854480302840345[20] = 0.0; out_8394854480302840345[21] = 0; out_8394854480302840345[22] = 1.0; out_8394854480302840345[23] = 0; out_8394854480302840345[24] = 0.0; out_8394854480302840345[25] = 0.0; out_8394854480302840345[26] = 0.0; out_8394854480302840345[27] = 0.0; out_8394854480302840345[28] = 0.0; out_8394854480302840345[29] = 0.0; out_8394854480302840345[30] = 0.0; out_8394854480302840345[31] = 0.0; out_8394854480302840345[32] = 0.0; out_8394854480302840345[33] = 0.0; out_8394854480302840345[34] = 0.0; out_8394854480302840345[35] = 0.0; out_8394854480302840345[36] = 0.0; out_8394854480302840345[37] = 0.0; out_8394854480302840345[38] = 0.0; out_8394854480302840345[39] = 0.0; out_8394854480302840345[40] = 0.0; out_8394854480302840345[41] = 0.0; out_8394854480302840345[42] = 0; out_8394854480302840345[43] = 0; out_8394854480302840345[44] = 1.0; out_8394854480302840345[45] = 0.0; out_8394854480302840345[46] = 0.0; out_8394854480302840345[47] = 0.0; out_8394854480302840345[48] = 0.0; out_8394854480302840345[49] = 0.0; out_8394854480302840345[50] = 0.0; out_8394854480302840345[51] = 0.0; out_8394854480302840345[52] = 0.0; out_8394854480302840345[53] = 0.0; out_8394854480302840345[54] = 0.0; out_8394854480302840345[55] = 0.0; out_8394854480302840345[56] = 0.0; out_8394854480302840345[57] = 0.0; out_8394854480302840345[58] = 0.0; out_8394854480302840345[59] = 0.0; out_8394854480302840345[60] = 0.0; out_8394854480302840345[61] = 0.0; out_8394854480302840345[62] = 0.0; out_8394854480302840345[63] = 0.0; out_8394854480302840345[64] = 0.0; out_8394854480302840345[65] = 0.0; out_8394854480302840345[66] = -0.5*state[4]; out_8394854480302840345[67] = -0.5*state[5]; out_8394854480302840345[68] = -0.5*state[6]; out_8394854480302840345[69] = 0.0; out_8394854480302840345[70] = 0.0; out_8394854480302840345[71] = 0.0; out_8394854480302840345[72] = 0.0; out_8394854480302840345[73] = 0.0; out_8394854480302840345[74] = 0.0; out_8394854480302840345[75] = 0.0; out_8394854480302840345[76] = 0.0; out_8394854480302840345[77] = 0.0; out_8394854480302840345[78] = 0.0; out_8394854480302840345[79] = 0.0; out_8394854480302840345[80] = 0.0; out_8394854480302840345[81] = 0.0; out_8394854480302840345[82] = 0.0; out_8394854480302840345[83] = 0.0; out_8394854480302840345[84] = 0.0; out_8394854480302840345[85] = 0.0; out_8394854480302840345[86] = 0.0; out_8394854480302840345[87] = 0.5*state[3]; out_8394854480302840345[88] = 0.5*state[6]; out_8394854480302840345[89] = -0.5*state[5]; out_8394854480302840345[90] = 0.0; out_8394854480302840345[91] = 0.0; out_8394854480302840345[92] = 0.0; out_8394854480302840345[93] = 0.0; out_8394854480302840345[94] = 0.0; out_8394854480302840345[95] = 0.0; out_8394854480302840345[96] = 0.0; out_8394854480302840345[97] = 0.0; out_8394854480302840345[98] = 0.0; out_8394854480302840345[99] = 0.0; out_8394854480302840345[100] = 0.0; out_8394854480302840345[101] = 0.0; out_8394854480302840345[102] = 0.0; out_8394854480302840345[103] = 0.0; out_8394854480302840345[104] = 0.0; out_8394854480302840345[105] = 0.0; out_8394854480302840345[106] = 0.0; out_8394854480302840345[107] = 0.0; out_8394854480302840345[108] = -0.5*state[6]; out_8394854480302840345[109] = 0.5*state[3]; out_8394854480302840345[110] = 0.5*state[4]; out_8394854480302840345[111] = 0.0; out_8394854480302840345[112] = 0.0; out_8394854480302840345[113] = 0.0; out_8394854480302840345[114] = 0.0; out_8394854480302840345[115] = 0.0; out_8394854480302840345[116] = 0.0; out_8394854480302840345[117] = 0.0; out_8394854480302840345[118] = 0.0; out_8394854480302840345[119] = 0.0; out_8394854480302840345[120] = 0.0; out_8394854480302840345[121] = 0.0; out_8394854480302840345[122] = 0.0; out_8394854480302840345[123] = 0.0; out_8394854480302840345[124] = 0.0; out_8394854480302840345[125] = 0.0; out_8394854480302840345[126] = 0.0; out_8394854480302840345[127] = 0.0; out_8394854480302840345[128] = 0.0; out_8394854480302840345[129] = 0.5*state[5]; out_8394854480302840345[130] = -0.5*state[4]; out_8394854480302840345[131] = 0.5*state[3]; out_8394854480302840345[132] = 0.0; out_8394854480302840345[133] = 0.0; out_8394854480302840345[134] = 0.0; out_8394854480302840345[135] = 0.0; out_8394854480302840345[136] = 0.0; out_8394854480302840345[137] = 0.0; out_8394854480302840345[138] = 0.0; out_8394854480302840345[139] = 0.0; out_8394854480302840345[140] = 0.0; out_8394854480302840345[141] = 0.0; out_8394854480302840345[142] = 0.0; out_8394854480302840345[143] = 0.0; out_8394854480302840345[144] = 0.0; out_8394854480302840345[145] = 0.0; out_8394854480302840345[146] = 0.0; out_8394854480302840345[147] = 0.0; out_8394854480302840345[148] = 0.0; out_8394854480302840345[149] = 0.0; out_8394854480302840345[150] = 0.0; out_8394854480302840345[151] = 0.0; out_8394854480302840345[152] = 0.0; out_8394854480302840345[153] = 1.0; out_8394854480302840345[154] = 0; out_8394854480302840345[155] = 0; out_8394854480302840345[156] = 0; out_8394854480302840345[157] = 0; out_8394854480302840345[158] = 0; out_8394854480302840345[159] = 0; out_8394854480302840345[160] = 0; out_8394854480302840345[161] = 0; out_8394854480302840345[162] = 0; out_8394854480302840345[163] = 0; out_8394854480302840345[164] = 0; out_8394854480302840345[165] = 0; out_8394854480302840345[166] = 0; out_8394854480302840345[167] = 0; out_8394854480302840345[168] = 0.0; out_8394854480302840345[169] = 0.0; out_8394854480302840345[170] = 0.0; out_8394854480302840345[171] = 0.0; out_8394854480302840345[172] = 0.0; out_8394854480302840345[173] = 0.0; out_8394854480302840345[174] = 0; out_8394854480302840345[175] = 1.0; out_8394854480302840345[176] = 0; out_8394854480302840345[177] = 0; out_8394854480302840345[178] = 0; out_8394854480302840345[179] = 0; out_8394854480302840345[180] = 0; out_8394854480302840345[181] = 0; out_8394854480302840345[182] = 0; out_8394854480302840345[183] = 0; out_8394854480302840345[184] = 0; out_8394854480302840345[185] = 0; out_8394854480302840345[186] = 0; out_8394854480302840345[187] = 0; out_8394854480302840345[188] = 0; out_8394854480302840345[189] = 0.0; out_8394854480302840345[190] = 0.0; out_8394854480302840345[191] = 0.0; out_8394854480302840345[192] = 0.0; out_8394854480302840345[193] = 0.0; out_8394854480302840345[194] = 0.0; out_8394854480302840345[195] = 0; out_8394854480302840345[196] = 0; out_8394854480302840345[197] = 1.0; out_8394854480302840345[198] = 0; out_8394854480302840345[199] = 0; out_8394854480302840345[200] = 0; out_8394854480302840345[201] = 0; out_8394854480302840345[202] = 0; out_8394854480302840345[203] = 0; out_8394854480302840345[204] = 0; out_8394854480302840345[205] = 0; out_8394854480302840345[206] = 0; out_8394854480302840345[207] = 0; out_8394854480302840345[208] = 0; out_8394854480302840345[209] = 0; out_8394854480302840345[210] = 0.0; out_8394854480302840345[211] = 0.0; out_8394854480302840345[212] = 0.0; out_8394854480302840345[213] = 0.0; out_8394854480302840345[214] = 0.0; out_8394854480302840345[215] = 0.0; out_8394854480302840345[216] = 0; out_8394854480302840345[217] = 0; out_8394854480302840345[218] = 0; out_8394854480302840345[219] = 1.0; out_8394854480302840345[220] = 0; out_8394854480302840345[221] = 0; out_8394854480302840345[222] = 0; out_8394854480302840345[223] = 0; out_8394854480302840345[224] = 0; out_8394854480302840345[225] = 0; out_8394854480302840345[226] = 0; out_8394854480302840345[227] = 0; out_8394854480302840345[228] = 0; out_8394854480302840345[229] = 0; out_8394854480302840345[230] = 0; out_8394854480302840345[231] = 0.0; out_8394854480302840345[232] = 0.0; out_8394854480302840345[233] = 0.0; out_8394854480302840345[234] = 0.0; out_8394854480302840345[235] = 0.0; out_8394854480302840345[236] = 0.0; out_8394854480302840345[237] = 0; out_8394854480302840345[238] = 0; out_8394854480302840345[239] = 0; out_8394854480302840345[240] = 0; out_8394854480302840345[241] = 1.0; out_8394854480302840345[242] = 0; out_8394854480302840345[243] = 0; out_8394854480302840345[244] = 0; out_8394854480302840345[245] = 0; out_8394854480302840345[246] = 0; out_8394854480302840345[247] = 0; out_8394854480302840345[248] = 0; out_8394854480302840345[249] = 0; out_8394854480302840345[250] = 0; out_8394854480302840345[251] = 0; out_8394854480302840345[252] = 0.0; out_8394854480302840345[253] = 0.0; out_8394854480302840345[254] = 0.0; out_8394854480302840345[255] = 0.0; out_8394854480302840345[256] = 0.0; out_8394854480302840345[257] = 0.0; out_8394854480302840345[258] = 0; out_8394854480302840345[259] = 0; out_8394854480302840345[260] = 0; out_8394854480302840345[261] = 0; out_8394854480302840345[262] = 0; out_8394854480302840345[263] = 1.0; out_8394854480302840345[264] = 0; out_8394854480302840345[265] = 0; out_8394854480302840345[266] = 0; out_8394854480302840345[267] = 0; out_8394854480302840345[268] = 0; out_8394854480302840345[269] = 0; out_8394854480302840345[270] = 0; out_8394854480302840345[271] = 0; out_8394854480302840345[272] = 0; out_8394854480302840345[273] = 0.0; out_8394854480302840345[274] = 0.0; out_8394854480302840345[275] = 0.0; out_8394854480302840345[276] = 0.0; out_8394854480302840345[277] = 0.0; out_8394854480302840345[278] = 0.0; out_8394854480302840345[279] = 0; out_8394854480302840345[280] = 0; out_8394854480302840345[281] = 0; out_8394854480302840345[282] = 0; out_8394854480302840345[283] = 0; out_8394854480302840345[284] = 0; out_8394854480302840345[285] = 1.0; out_8394854480302840345[286] = 0; out_8394854480302840345[287] = 0; out_8394854480302840345[288] = 0; out_8394854480302840345[289] = 0; out_8394854480302840345[290] = 0; out_8394854480302840345[291] = 0; out_8394854480302840345[292] = 0; out_8394854480302840345[293] = 0; out_8394854480302840345[294] = 0.0; out_8394854480302840345[295] = 0.0; out_8394854480302840345[296] = 0.0; out_8394854480302840345[297] = 0.0; out_8394854480302840345[298] = 0.0; out_8394854480302840345[299] = 0.0; out_8394854480302840345[300] = 0; out_8394854480302840345[301] = 0; out_8394854480302840345[302] = 0; out_8394854480302840345[303] = 0; out_8394854480302840345[304] = 0; out_8394854480302840345[305] = 0; out_8394854480302840345[306] = 0; out_8394854480302840345[307] = 1.0; out_8394854480302840345[308] = 0; out_8394854480302840345[309] = 0; out_8394854480302840345[310] = 0; out_8394854480302840345[311] = 0; out_8394854480302840345[312] = 0; out_8394854480302840345[313] = 0; out_8394854480302840345[314] = 0; out_8394854480302840345[315] = 0.0; out_8394854480302840345[316] = 0.0; out_8394854480302840345[317] = 0.0; out_8394854480302840345[318] = 0.0; out_8394854480302840345[319] = 0.0; out_8394854480302840345[320] = 0.0; out_8394854480302840345[321] = 0; out_8394854480302840345[322] = 0; out_8394854480302840345[323] = 0; out_8394854480302840345[324] = 0; out_8394854480302840345[325] = 0; out_8394854480302840345[326] = 0; out_8394854480302840345[327] = 0; out_8394854480302840345[328] = 0; out_8394854480302840345[329] = 1.0; out_8394854480302840345[330] = 0; out_8394854480302840345[331] = 0; out_8394854480302840345[332] = 0; out_8394854480302840345[333] = 0; out_8394854480302840345[334] = 0; out_8394854480302840345[335] = 0; out_8394854480302840345[336] = 0.0; out_8394854480302840345[337] = 0.0; out_8394854480302840345[338] = 0.0; out_8394854480302840345[339] = 0.0; out_8394854480302840345[340] = 0.0; out_8394854480302840345[341] = 0.0; out_8394854480302840345[342] = 0; out_8394854480302840345[343] = 0; out_8394854480302840345[344] = 0; out_8394854480302840345[345] = 0; out_8394854480302840345[346] = 0; out_8394854480302840345[347] = 0; out_8394854480302840345[348] = 0; out_8394854480302840345[349] = 0; out_8394854480302840345[350] = 0; out_8394854480302840345[351] = 1.0; out_8394854480302840345[352] = 0; out_8394854480302840345[353] = 0; out_8394854480302840345[354] = 0; out_8394854480302840345[355] = 0; out_8394854480302840345[356] = 0; out_8394854480302840345[357] = 0.0; out_8394854480302840345[358] = 0.0; out_8394854480302840345[359] = 0.0; out_8394854480302840345[360] = 0.0; out_8394854480302840345[361] = 0.0; out_8394854480302840345[362] = 0.0; out_8394854480302840345[363] = 0; out_8394854480302840345[364] = 0; out_8394854480302840345[365] = 0; out_8394854480302840345[366] = 0; out_8394854480302840345[367] = 0; out_8394854480302840345[368] = 0; out_8394854480302840345[369] = 0; out_8394854480302840345[370] = 0; out_8394854480302840345[371] = 0; out_8394854480302840345[372] = 0; out_8394854480302840345[373] = 1.0; out_8394854480302840345[374] = 0; out_8394854480302840345[375] = 0; out_8394854480302840345[376] = 0; out_8394854480302840345[377] = 0; out_8394854480302840345[378] = 0.0; out_8394854480302840345[379] = 0.0; out_8394854480302840345[380] = 0.0; out_8394854480302840345[381] = 0.0; out_8394854480302840345[382] = 0.0; out_8394854480302840345[383] = 0.0; out_8394854480302840345[384] = 0; out_8394854480302840345[385] = 0; out_8394854480302840345[386] = 0; out_8394854480302840345[387] = 0; out_8394854480302840345[388] = 0; out_8394854480302840345[389] = 0; out_8394854480302840345[390] = 0; out_8394854480302840345[391] = 0; out_8394854480302840345[392] = 0; out_8394854480302840345[393] = 0; out_8394854480302840345[394] = 0; out_8394854480302840345[395] = 1.0; out_8394854480302840345[396] = 0; out_8394854480302840345[397] = 0; out_8394854480302840345[398] = 0; out_8394854480302840345[399] = 0.0; out_8394854480302840345[400] = 0.0; out_8394854480302840345[401] = 0.0; out_8394854480302840345[402] = 0.0; out_8394854480302840345[403] = 0.0; out_8394854480302840345[404] = 0.0; out_8394854480302840345[405] = 0; out_8394854480302840345[406] = 0; out_8394854480302840345[407] = 0; out_8394854480302840345[408] = 0; out_8394854480302840345[409] = 0; out_8394854480302840345[410] = 0; out_8394854480302840345[411] = 0; out_8394854480302840345[412] = 0; out_8394854480302840345[413] = 0; out_8394854480302840345[414] = 0; out_8394854480302840345[415] = 0; out_8394854480302840345[416] = 0; out_8394854480302840345[417] = 1.0; out_8394854480302840345[418] = 0; out_8394854480302840345[419] = 0; out_8394854480302840345[420] = 0.0; out_8394854480302840345[421] = 0.0; out_8394854480302840345[422] = 0.0; out_8394854480302840345[423] = 0.0; out_8394854480302840345[424] = 0.0; out_8394854480302840345[425] = 0.0; out_8394854480302840345[426] = 0; out_8394854480302840345[427] = 0; out_8394854480302840345[428] = 0; out_8394854480302840345[429] = 0; out_8394854480302840345[430] = 0; out_8394854480302840345[431] = 0; out_8394854480302840345[432] = 0; out_8394854480302840345[433] = 0; out_8394854480302840345[434] = 0; out_8394854480302840345[435] = 0; out_8394854480302840345[436] = 0; out_8394854480302840345[437] = 0; out_8394854480302840345[438] = 0; out_8394854480302840345[439] = 1.0; out_8394854480302840345[440] = 0; out_8394854480302840345[441] = 0.0; out_8394854480302840345[442] = 0.0; out_8394854480302840345[443] = 0.0; out_8394854480302840345[444] = 0.0; out_8394854480302840345[445] = 0.0; out_8394854480302840345[446] = 0.0; out_8394854480302840345[447] = 0; out_8394854480302840345[448] = 0; out_8394854480302840345[449] = 0; out_8394854480302840345[450] = 0; out_8394854480302840345[451] = 0; out_8394854480302840345[452] = 0; out_8394854480302840345[453] = 0; out_8394854480302840345[454] = 0; out_8394854480302840345[455] = 0; out_8394854480302840345[456] = 0; out_8394854480302840345[457] = 0; out_8394854480302840345[458] = 0; out_8394854480302840345[459] = 0; out_8394854480302840345[460] = 0; out_8394854480302840345[461] = 1.0; } void f_fun(double *state, double dt, double *out_8170256821180437676) { out_8170256821180437676[0] = dt*state[7] + state[0]; out_8170256821180437676[1] = dt*state[8] + state[1]; out_8170256821180437676[2] = dt*state[9] + state[2]; out_8170256821180437676[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; out_8170256821180437676[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; out_8170256821180437676[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; out_8170256821180437676[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; out_8170256821180437676[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_8170256821180437676[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_8170256821180437676[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_8170256821180437676[10] = state[10]; out_8170256821180437676[11] = state[11]; out_8170256821180437676[12] = state[12]; out_8170256821180437676[13] = state[13]; out_8170256821180437676[14] = state[14]; out_8170256821180437676[15] = state[15]; out_8170256821180437676[16] = state[16]; out_8170256821180437676[17] = state[17]; out_8170256821180437676[18] = state[18]; out_8170256821180437676[19] = state[19]; out_8170256821180437676[20] = state[20]; out_8170256821180437676[21] = state[21]; } void F_fun(double *state, double dt, double *out_6156410520571324808) { out_6156410520571324808[0] = 1; out_6156410520571324808[1] = 0; out_6156410520571324808[2] = 0; out_6156410520571324808[3] = 0; out_6156410520571324808[4] = 0; out_6156410520571324808[5] = 0; out_6156410520571324808[6] = dt; out_6156410520571324808[7] = 0; out_6156410520571324808[8] = 0; out_6156410520571324808[9] = 0; out_6156410520571324808[10] = 0; out_6156410520571324808[11] = 0; out_6156410520571324808[12] = 0; out_6156410520571324808[13] = 0; out_6156410520571324808[14] = 0; out_6156410520571324808[15] = 0; out_6156410520571324808[16] = 0; out_6156410520571324808[17] = 0; out_6156410520571324808[18] = 0; out_6156410520571324808[19] = 0; out_6156410520571324808[20] = 0; out_6156410520571324808[21] = 0; out_6156410520571324808[22] = 1; out_6156410520571324808[23] = 0; out_6156410520571324808[24] = 0; out_6156410520571324808[25] = 0; out_6156410520571324808[26] = 0; out_6156410520571324808[27] = 0; out_6156410520571324808[28] = dt; out_6156410520571324808[29] = 0; out_6156410520571324808[30] = 0; out_6156410520571324808[31] = 0; out_6156410520571324808[32] = 0; out_6156410520571324808[33] = 0; out_6156410520571324808[34] = 0; out_6156410520571324808[35] = 0; out_6156410520571324808[36] = 0; out_6156410520571324808[37] = 0; out_6156410520571324808[38] = 0; out_6156410520571324808[39] = 0; out_6156410520571324808[40] = 0; out_6156410520571324808[41] = 0; out_6156410520571324808[42] = 0; out_6156410520571324808[43] = 0; out_6156410520571324808[44] = 1; out_6156410520571324808[45] = 0; out_6156410520571324808[46] = 0; out_6156410520571324808[47] = 0; out_6156410520571324808[48] = 0; out_6156410520571324808[49] = 0; out_6156410520571324808[50] = dt; out_6156410520571324808[51] = 0; out_6156410520571324808[52] = 0; out_6156410520571324808[53] = 0; out_6156410520571324808[54] = 0; out_6156410520571324808[55] = 0; out_6156410520571324808[56] = 0; out_6156410520571324808[57] = 0; out_6156410520571324808[58] = 0; out_6156410520571324808[59] = 0; out_6156410520571324808[60] = 0; out_6156410520571324808[61] = 0; out_6156410520571324808[62] = 0; out_6156410520571324808[63] = 0; out_6156410520571324808[64] = 0; out_6156410520571324808[65] = 0; out_6156410520571324808[66] = 1; out_6156410520571324808[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_6156410520571324808[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_6156410520571324808[69] = 0; out_6156410520571324808[70] = 0; out_6156410520571324808[71] = 0; out_6156410520571324808[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); out_6156410520571324808[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); out_6156410520571324808[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); out_6156410520571324808[75] = 0; out_6156410520571324808[76] = 0; out_6156410520571324808[77] = 0; out_6156410520571324808[78] = 0; out_6156410520571324808[79] = 0; out_6156410520571324808[80] = 0; out_6156410520571324808[81] = 0; out_6156410520571324808[82] = 0; out_6156410520571324808[83] = 0; out_6156410520571324808[84] = 0; out_6156410520571324808[85] = 0; out_6156410520571324808[86] = 0; out_6156410520571324808[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_6156410520571324808[88] = 1; out_6156410520571324808[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_6156410520571324808[90] = 0; out_6156410520571324808[91] = 0; out_6156410520571324808[92] = 0; out_6156410520571324808[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); out_6156410520571324808[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); out_6156410520571324808[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); out_6156410520571324808[96] = 0; out_6156410520571324808[97] = 0; out_6156410520571324808[98] = 0; out_6156410520571324808[99] = 0; out_6156410520571324808[100] = 0; out_6156410520571324808[101] = 0; out_6156410520571324808[102] = 0; out_6156410520571324808[103] = 0; out_6156410520571324808[104] = 0; out_6156410520571324808[105] = 0; out_6156410520571324808[106] = 0; out_6156410520571324808[107] = 0; out_6156410520571324808[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_6156410520571324808[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_6156410520571324808[110] = 1; out_6156410520571324808[111] = 0; out_6156410520571324808[112] = 0; out_6156410520571324808[113] = 0; out_6156410520571324808[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); out_6156410520571324808[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); out_6156410520571324808[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); out_6156410520571324808[117] = 0; out_6156410520571324808[118] = 0; out_6156410520571324808[119] = 0; out_6156410520571324808[120] = 0; out_6156410520571324808[121] = 0; out_6156410520571324808[122] = 0; out_6156410520571324808[123] = 0; out_6156410520571324808[124] = 0; out_6156410520571324808[125] = 0; out_6156410520571324808[126] = 0; out_6156410520571324808[127] = 0; out_6156410520571324808[128] = 0; out_6156410520571324808[129] = 0; out_6156410520571324808[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_6156410520571324808[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_6156410520571324808[132] = 1; out_6156410520571324808[133] = 0; out_6156410520571324808[134] = 0; out_6156410520571324808[135] = 0; out_6156410520571324808[136] = 0; out_6156410520571324808[137] = 0; out_6156410520571324808[138] = 0; out_6156410520571324808[139] = 0; out_6156410520571324808[140] = 0; out_6156410520571324808[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); out_6156410520571324808[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); out_6156410520571324808[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); out_6156410520571324808[144] = 0; out_6156410520571324808[145] = 0; out_6156410520571324808[146] = 0; out_6156410520571324808[147] = 0; out_6156410520571324808[148] = 0; out_6156410520571324808[149] = 0; out_6156410520571324808[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_6156410520571324808[151] = 0; out_6156410520571324808[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_6156410520571324808[153] = 0; out_6156410520571324808[154] = 1; out_6156410520571324808[155] = 0; out_6156410520571324808[156] = 0; out_6156410520571324808[157] = 0; out_6156410520571324808[158] = 0; out_6156410520571324808[159] = 0; out_6156410520571324808[160] = 0; out_6156410520571324808[161] = 0; out_6156410520571324808[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); out_6156410520571324808[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); out_6156410520571324808[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); out_6156410520571324808[165] = 0; out_6156410520571324808[166] = 0; out_6156410520571324808[167] = 0; out_6156410520571324808[168] = 0; out_6156410520571324808[169] = 0; out_6156410520571324808[170] = 0; out_6156410520571324808[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_6156410520571324808[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_6156410520571324808[173] = 0; out_6156410520571324808[174] = 0; out_6156410520571324808[175] = 0; out_6156410520571324808[176] = 1; out_6156410520571324808[177] = 0; out_6156410520571324808[178] = 0; out_6156410520571324808[179] = 0; out_6156410520571324808[180] = 0; out_6156410520571324808[181] = 0; out_6156410520571324808[182] = 0; out_6156410520571324808[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); out_6156410520571324808[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); out_6156410520571324808[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); out_6156410520571324808[186] = 0; out_6156410520571324808[187] = 0; out_6156410520571324808[188] = 0; out_6156410520571324808[189] = 0; out_6156410520571324808[190] = 0; out_6156410520571324808[191] = 0; out_6156410520571324808[192] = 0; out_6156410520571324808[193] = 0; out_6156410520571324808[194] = 0; out_6156410520571324808[195] = 0; out_6156410520571324808[196] = 0; out_6156410520571324808[197] = 0; out_6156410520571324808[198] = 1; out_6156410520571324808[199] = 0; out_6156410520571324808[200] = 0; out_6156410520571324808[201] = 0; out_6156410520571324808[202] = 0; out_6156410520571324808[203] = 0; out_6156410520571324808[204] = 0; out_6156410520571324808[205] = 0; out_6156410520571324808[206] = 0; out_6156410520571324808[207] = 0; out_6156410520571324808[208] = 0; out_6156410520571324808[209] = 0; out_6156410520571324808[210] = 0; out_6156410520571324808[211] = 0; out_6156410520571324808[212] = 0; out_6156410520571324808[213] = 0; out_6156410520571324808[214] = 0; out_6156410520571324808[215] = 0; out_6156410520571324808[216] = 0; out_6156410520571324808[217] = 0; out_6156410520571324808[218] = 0; out_6156410520571324808[219] = 0; out_6156410520571324808[220] = 1; out_6156410520571324808[221] = 0; out_6156410520571324808[222] = 0; out_6156410520571324808[223] = 0; out_6156410520571324808[224] = 0; out_6156410520571324808[225] = 0; out_6156410520571324808[226] = 0; out_6156410520571324808[227] = 0; out_6156410520571324808[228] = 0; out_6156410520571324808[229] = 0; out_6156410520571324808[230] = 0; out_6156410520571324808[231] = 0; out_6156410520571324808[232] = 0; out_6156410520571324808[233] = 0; out_6156410520571324808[234] = 0; out_6156410520571324808[235] = 0; out_6156410520571324808[236] = 0; out_6156410520571324808[237] = 0; out_6156410520571324808[238] = 0; out_6156410520571324808[239] = 0; out_6156410520571324808[240] = 0; out_6156410520571324808[241] = 0; out_6156410520571324808[242] = 1; out_6156410520571324808[243] = 0; out_6156410520571324808[244] = 0; out_6156410520571324808[245] = 0; out_6156410520571324808[246] = 0; out_6156410520571324808[247] = 0; out_6156410520571324808[248] = 0; out_6156410520571324808[249] = 0; out_6156410520571324808[250] = 0; out_6156410520571324808[251] = 0; out_6156410520571324808[252] = 0; out_6156410520571324808[253] = 0; out_6156410520571324808[254] = 0; out_6156410520571324808[255] = 0; out_6156410520571324808[256] = 0; out_6156410520571324808[257] = 0; out_6156410520571324808[258] = 0; out_6156410520571324808[259] = 0; out_6156410520571324808[260] = 0; out_6156410520571324808[261] = 0; out_6156410520571324808[262] = 0; out_6156410520571324808[263] = 0; out_6156410520571324808[264] = 1; out_6156410520571324808[265] = 0; out_6156410520571324808[266] = 0; out_6156410520571324808[267] = 0; out_6156410520571324808[268] = 0; out_6156410520571324808[269] = 0; out_6156410520571324808[270] = 0; out_6156410520571324808[271] = 0; out_6156410520571324808[272] = 0; out_6156410520571324808[273] = 0; out_6156410520571324808[274] = 0; out_6156410520571324808[275] = 0; out_6156410520571324808[276] = 0; out_6156410520571324808[277] = 0; out_6156410520571324808[278] = 0; out_6156410520571324808[279] = 0; out_6156410520571324808[280] = 0; out_6156410520571324808[281] = 0; out_6156410520571324808[282] = 0; out_6156410520571324808[283] = 0; out_6156410520571324808[284] = 0; out_6156410520571324808[285] = 0; out_6156410520571324808[286] = 1; out_6156410520571324808[287] = 0; out_6156410520571324808[288] = 0; out_6156410520571324808[289] = 0; out_6156410520571324808[290] = 0; out_6156410520571324808[291] = 0; out_6156410520571324808[292] = 0; out_6156410520571324808[293] = 0; out_6156410520571324808[294] = 0; out_6156410520571324808[295] = 0; out_6156410520571324808[296] = 0; out_6156410520571324808[297] = 0; out_6156410520571324808[298] = 0; out_6156410520571324808[299] = 0; out_6156410520571324808[300] = 0; out_6156410520571324808[301] = 0; out_6156410520571324808[302] = 0; out_6156410520571324808[303] = 0; out_6156410520571324808[304] = 0; out_6156410520571324808[305] = 0; out_6156410520571324808[306] = 0; out_6156410520571324808[307] = 0; out_6156410520571324808[308] = 1; out_6156410520571324808[309] = 0; out_6156410520571324808[310] = 0; out_6156410520571324808[311] = 0; out_6156410520571324808[312] = 0; out_6156410520571324808[313] = 0; out_6156410520571324808[314] = 0; out_6156410520571324808[315] = 0; out_6156410520571324808[316] = 0; out_6156410520571324808[317] = 0; out_6156410520571324808[318] = 0; out_6156410520571324808[319] = 0; out_6156410520571324808[320] = 0; out_6156410520571324808[321] = 0; out_6156410520571324808[322] = 0; out_6156410520571324808[323] = 0; out_6156410520571324808[324] = 0; out_6156410520571324808[325] = 0; out_6156410520571324808[326] = 0; out_6156410520571324808[327] = 0; out_6156410520571324808[328] = 0; out_6156410520571324808[329] = 0; out_6156410520571324808[330] = 1; out_6156410520571324808[331] = 0; out_6156410520571324808[332] = 0; out_6156410520571324808[333] = 0; out_6156410520571324808[334] = 0; out_6156410520571324808[335] = 0; out_6156410520571324808[336] = 0; out_6156410520571324808[337] = 0; out_6156410520571324808[338] = 0; out_6156410520571324808[339] = 0; out_6156410520571324808[340] = 0; out_6156410520571324808[341] = 0; out_6156410520571324808[342] = 0; out_6156410520571324808[343] = 0; out_6156410520571324808[344] = 0; out_6156410520571324808[345] = 0; out_6156410520571324808[346] = 0; out_6156410520571324808[347] = 0; out_6156410520571324808[348] = 0; out_6156410520571324808[349] = 0; out_6156410520571324808[350] = 0; out_6156410520571324808[351] = 0; out_6156410520571324808[352] = 1; out_6156410520571324808[353] = 0; out_6156410520571324808[354] = 0; out_6156410520571324808[355] = 0; out_6156410520571324808[356] = 0; out_6156410520571324808[357] = 0; out_6156410520571324808[358] = 0; out_6156410520571324808[359] = 0; out_6156410520571324808[360] = 0; out_6156410520571324808[361] = 0; out_6156410520571324808[362] = 0; out_6156410520571324808[363] = 0; out_6156410520571324808[364] = 0; out_6156410520571324808[365] = 0; out_6156410520571324808[366] = 0; out_6156410520571324808[367] = 0; out_6156410520571324808[368] = 0; out_6156410520571324808[369] = 0; out_6156410520571324808[370] = 0; out_6156410520571324808[371] = 0; out_6156410520571324808[372] = 0; out_6156410520571324808[373] = 0; out_6156410520571324808[374] = 1; out_6156410520571324808[375] = 0; out_6156410520571324808[376] = 0; out_6156410520571324808[377] = 0; out_6156410520571324808[378] = 0; out_6156410520571324808[379] = 0; out_6156410520571324808[380] = 0; out_6156410520571324808[381] = 0; out_6156410520571324808[382] = 0; out_6156410520571324808[383] = 0; out_6156410520571324808[384] = 0; out_6156410520571324808[385] = 0; out_6156410520571324808[386] = 0; out_6156410520571324808[387] = 0; out_6156410520571324808[388] = 0; out_6156410520571324808[389] = 0; out_6156410520571324808[390] = 0; out_6156410520571324808[391] = 0; out_6156410520571324808[392] = 0; out_6156410520571324808[393] = 0; out_6156410520571324808[394] = 0; out_6156410520571324808[395] = 0; out_6156410520571324808[396] = 1; out_6156410520571324808[397] = 0; out_6156410520571324808[398] = 0; out_6156410520571324808[399] = 0; out_6156410520571324808[400] = 0; out_6156410520571324808[401] = 0; out_6156410520571324808[402] = 0; out_6156410520571324808[403] = 0; out_6156410520571324808[404] = 0; out_6156410520571324808[405] = 0; out_6156410520571324808[406] = 0; out_6156410520571324808[407] = 0; out_6156410520571324808[408] = 0; out_6156410520571324808[409] = 0; out_6156410520571324808[410] = 0; out_6156410520571324808[411] = 0; out_6156410520571324808[412] = 0; out_6156410520571324808[413] = 0; out_6156410520571324808[414] = 0; out_6156410520571324808[415] = 0; out_6156410520571324808[416] = 0; out_6156410520571324808[417] = 0; out_6156410520571324808[418] = 1; out_6156410520571324808[419] = 0; out_6156410520571324808[420] = 0; out_6156410520571324808[421] = 0; out_6156410520571324808[422] = 0; out_6156410520571324808[423] = 0; out_6156410520571324808[424] = 0; out_6156410520571324808[425] = 0; out_6156410520571324808[426] = 0; out_6156410520571324808[427] = 0; out_6156410520571324808[428] = 0; out_6156410520571324808[429] = 0; out_6156410520571324808[430] = 0; out_6156410520571324808[431] = 0; out_6156410520571324808[432] = 0; out_6156410520571324808[433] = 0; out_6156410520571324808[434] = 0; out_6156410520571324808[435] = 0; out_6156410520571324808[436] = 0; out_6156410520571324808[437] = 0; out_6156410520571324808[438] = 0; out_6156410520571324808[439] = 0; out_6156410520571324808[440] = 1; } void h_4(double *state, double *unused, double *out_4490985437783499760) { out_4490985437783499760[0] = state[10] + state[13]; out_4490985437783499760[1] = state[11] + state[14]; out_4490985437783499760[2] = state[12] + state[15]; } void H_4(double *state, double *unused, double *out_8374258154538532265) { out_8374258154538532265[0] = 0; out_8374258154538532265[1] = 0; out_8374258154538532265[2] = 0; out_8374258154538532265[3] = 0; out_8374258154538532265[4] = 0; out_8374258154538532265[5] = 0; out_8374258154538532265[6] = 0; out_8374258154538532265[7] = 0; out_8374258154538532265[8] = 0; out_8374258154538532265[9] = 0; out_8374258154538532265[10] = 1; out_8374258154538532265[11] = 0; out_8374258154538532265[12] = 0; out_8374258154538532265[13] = 1; out_8374258154538532265[14] = 0; out_8374258154538532265[15] = 0; out_8374258154538532265[16] = 0; out_8374258154538532265[17] = 0; out_8374258154538532265[18] = 0; out_8374258154538532265[19] = 0; out_8374258154538532265[20] = 0; out_8374258154538532265[21] = 0; out_8374258154538532265[22] = 0; out_8374258154538532265[23] = 0; out_8374258154538532265[24] = 0; out_8374258154538532265[25] = 0; out_8374258154538532265[26] = 0; out_8374258154538532265[27] = 0; out_8374258154538532265[28] = 0; out_8374258154538532265[29] = 0; out_8374258154538532265[30] = 0; out_8374258154538532265[31] = 0; out_8374258154538532265[32] = 0; out_8374258154538532265[33] = 1; out_8374258154538532265[34] = 0; out_8374258154538532265[35] = 0; out_8374258154538532265[36] = 1; out_8374258154538532265[37] = 0; out_8374258154538532265[38] = 0; out_8374258154538532265[39] = 0; out_8374258154538532265[40] = 0; out_8374258154538532265[41] = 0; out_8374258154538532265[42] = 0; out_8374258154538532265[43] = 0; out_8374258154538532265[44] = 0; out_8374258154538532265[45] = 0; out_8374258154538532265[46] = 0; out_8374258154538532265[47] = 0; out_8374258154538532265[48] = 0; out_8374258154538532265[49] = 0; out_8374258154538532265[50] = 0; out_8374258154538532265[51] = 0; out_8374258154538532265[52] = 0; out_8374258154538532265[53] = 0; out_8374258154538532265[54] = 0; out_8374258154538532265[55] = 0; out_8374258154538532265[56] = 1; out_8374258154538532265[57] = 0; out_8374258154538532265[58] = 0; out_8374258154538532265[59] = 1; out_8374258154538532265[60] = 0; out_8374258154538532265[61] = 0; out_8374258154538532265[62] = 0; out_8374258154538532265[63] = 0; out_8374258154538532265[64] = 0; out_8374258154538532265[65] = 0; } void h_9(double *state, double *unused, double *out_4760385502323129369) { out_4760385502323129369[0] = state[10]; out_4760385502323129369[1] = state[11]; out_4760385502323129369[2] = state[12]; } void H_9(double *state, double *unused, double *out_2785266983906571881) { out_2785266983906571881[0] = 0; out_2785266983906571881[1] = 0; out_2785266983906571881[2] = 0; out_2785266983906571881[3] = 0; out_2785266983906571881[4] = 0; out_2785266983906571881[5] = 0; out_2785266983906571881[6] = 0; out_2785266983906571881[7] = 0; out_2785266983906571881[8] = 0; out_2785266983906571881[9] = 0; out_2785266983906571881[10] = 1; out_2785266983906571881[11] = 0; out_2785266983906571881[12] = 0; out_2785266983906571881[13] = 0; out_2785266983906571881[14] = 0; out_2785266983906571881[15] = 0; out_2785266983906571881[16] = 0; out_2785266983906571881[17] = 0; out_2785266983906571881[18] = 0; out_2785266983906571881[19] = 0; out_2785266983906571881[20] = 0; out_2785266983906571881[21] = 0; out_2785266983906571881[22] = 0; out_2785266983906571881[23] = 0; out_2785266983906571881[24] = 0; out_2785266983906571881[25] = 0; out_2785266983906571881[26] = 0; out_2785266983906571881[27] = 0; out_2785266983906571881[28] = 0; out_2785266983906571881[29] = 0; out_2785266983906571881[30] = 0; out_2785266983906571881[31] = 0; out_2785266983906571881[32] = 0; out_2785266983906571881[33] = 1; out_2785266983906571881[34] = 0; out_2785266983906571881[35] = 0; out_2785266983906571881[36] = 0; out_2785266983906571881[37] = 0; out_2785266983906571881[38] = 0; out_2785266983906571881[39] = 0; out_2785266983906571881[40] = 0; out_2785266983906571881[41] = 0; out_2785266983906571881[42] = 0; out_2785266983906571881[43] = 0; out_2785266983906571881[44] = 0; out_2785266983906571881[45] = 0; out_2785266983906571881[46] = 0; out_2785266983906571881[47] = 0; out_2785266983906571881[48] = 0; out_2785266983906571881[49] = 0; out_2785266983906571881[50] = 0; out_2785266983906571881[51] = 0; out_2785266983906571881[52] = 0; out_2785266983906571881[53] = 0; out_2785266983906571881[54] = 0; out_2785266983906571881[55] = 0; out_2785266983906571881[56] = 1; out_2785266983906571881[57] = 0; out_2785266983906571881[58] = 0; out_2785266983906571881[59] = 0; out_2785266983906571881[60] = 0; out_2785266983906571881[61] = 0; out_2785266983906571881[62] = 0; out_2785266983906571881[63] = 0; out_2785266983906571881[64] = 0; out_2785266983906571881[65] = 0; } void h_10(double *state, double *unused, double *out_1378114642409751093) { out_1378114642409751093[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_1378114642409751093[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_1378114642409751093[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_116278907094796186) { out_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[7] = 0; out_116278907094796186[8] = 0; out_116278907094796186[9] = 0; out_116278907094796186[10] = 0; out_116278907094796186[11] = 0; out_116278907094796186[12] = 0; out_116278907094796186[13] = 0; out_116278907094796186[14] = 0; out_116278907094796186[15] = 0; out_116278907094796186[16] = 1; out_116278907094796186[17] = 0; out_116278907094796186[18] = 0; out_116278907094796186[19] = 1; out_116278907094796186[20] = 0; out_116278907094796186[21] = 0; out_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[29] = 0; out_116278907094796186[30] = 0; out_116278907094796186[31] = 0; out_116278907094796186[32] = 0; out_116278907094796186[33] = 0; out_116278907094796186[34] = 0; out_116278907094796186[35] = 0; out_116278907094796186[36] = 0; out_116278907094796186[37] = 0; out_116278907094796186[38] = 0; out_116278907094796186[39] = 1; out_116278907094796186[40] = 0; out_116278907094796186[41] = 0; out_116278907094796186[42] = 1; out_116278907094796186[43] = 0; out_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[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_116278907094796186[51] = 0; out_116278907094796186[52] = 0; out_116278907094796186[53] = 0; out_116278907094796186[54] = 0; out_116278907094796186[55] = 0; out_116278907094796186[56] = 0; out_116278907094796186[57] = 0; out_116278907094796186[58] = 0; out_116278907094796186[59] = 0; out_116278907094796186[60] = 0; out_116278907094796186[61] = 0; out_116278907094796186[62] = 1; out_116278907094796186[63] = 0; out_116278907094796186[64] = 0; out_116278907094796186[65] = 1; } void h_12(double *state, double *unused, double *out_6738077689390158393) { out_6738077689390158393[0] = state[0]; out_6738077689390158393[1] = state[1]; out_6738077689390158393[2] = state[2]; } void H_12(double *state, double *unused, double *out_5053029511139057556) { out_5053029511139057556[0] = 1; out_5053029511139057556[1] = 0; out_5053029511139057556[2] = 0; out_5053029511139057556[3] = 0; out_5053029511139057556[4] = 0; out_5053029511139057556[5] = 0; out_5053029511139057556[6] = 0; out_5053029511139057556[7] = 0; out_5053029511139057556[8] = 0; out_5053029511139057556[9] = 0; out_5053029511139057556[10] = 0; out_5053029511139057556[11] = 0; out_5053029511139057556[12] = 0; out_5053029511139057556[13] = 0; out_5053029511139057556[14] = 0; out_5053029511139057556[15] = 0; out_5053029511139057556[16] = 0; out_5053029511139057556[17] = 0; out_5053029511139057556[18] = 0; out_5053029511139057556[19] = 0; out_5053029511139057556[20] = 0; out_5053029511139057556[21] = 0; out_5053029511139057556[22] = 0; out_5053029511139057556[23] = 1; out_5053029511139057556[24] = 0; out_5053029511139057556[25] = 0; out_5053029511139057556[26] = 0; out_5053029511139057556[27] = 0; out_5053029511139057556[28] = 0; out_5053029511139057556[29] = 0; out_5053029511139057556[30] = 0; out_5053029511139057556[31] = 0; out_5053029511139057556[32] = 0; out_5053029511139057556[33] = 0; out_5053029511139057556[34] = 0; out_5053029511139057556[35] = 0; out_5053029511139057556[36] = 0; out_5053029511139057556[37] = 0; out_5053029511139057556[38] = 0; out_5053029511139057556[39] = 0; out_5053029511139057556[40] = 0; out_5053029511139057556[41] = 0; out_5053029511139057556[42] = 0; out_5053029511139057556[43] = 0; out_5053029511139057556[44] = 0; out_5053029511139057556[45] = 0; out_5053029511139057556[46] = 1; out_5053029511139057556[47] = 0; out_5053029511139057556[48] = 0; out_5053029511139057556[49] = 0; out_5053029511139057556[50] = 0; out_5053029511139057556[51] = 0; out_5053029511139057556[52] = 0; out_5053029511139057556[53] = 0; out_5053029511139057556[54] = 0; out_5053029511139057556[55] = 0; out_5053029511139057556[56] = 0; out_5053029511139057556[57] = 0; out_5053029511139057556[58] = 0; out_5053029511139057556[59] = 0; out_5053029511139057556[60] = 0; out_5053029511139057556[61] = 0; out_5053029511139057556[62] = 0; out_5053029511139057556[63] = 0; out_5053029511139057556[64] = 0; out_5053029511139057556[65] = 0; } void h_35(double *state, double *unused, double *out_5147545742760872875) { out_5147545742760872875[0] = state[7]; out_5147545742760872875[1] = state[8]; out_5147545742760872875[2] = state[9]; } void H_35(double *state, double *unused, double *out_6705823861798411975) { out_6705823861798411975[0] = 0; out_6705823861798411975[1] = 0; out_6705823861798411975[2] = 0; out_6705823861798411975[3] = 0; out_6705823861798411975[4] = 0; out_6705823861798411975[5] = 0; out_6705823861798411975[6] = 0; out_6705823861798411975[7] = 1; out_6705823861798411975[8] = 0; out_6705823861798411975[9] = 0; out_6705823861798411975[10] = 0; out_6705823861798411975[11] = 0; out_6705823861798411975[12] = 0; out_6705823861798411975[13] = 0; out_6705823861798411975[14] = 0; out_6705823861798411975[15] = 0; out_6705823861798411975[16] = 0; out_6705823861798411975[17] = 0; out_6705823861798411975[18] = 0; out_6705823861798411975[19] = 0; out_6705823861798411975[20] = 0; out_6705823861798411975[21] = 0; out_6705823861798411975[22] = 0; out_6705823861798411975[23] = 0; out_6705823861798411975[24] = 0; out_6705823861798411975[25] = 0; out_6705823861798411975[26] = 0; out_6705823861798411975[27] = 0; out_6705823861798411975[28] = 0; out_6705823861798411975[29] = 0; out_6705823861798411975[30] = 1; out_6705823861798411975[31] = 0; out_6705823861798411975[32] = 0; out_6705823861798411975[33] = 0; out_6705823861798411975[34] = 0; out_6705823861798411975[35] = 0; out_6705823861798411975[36] = 0; out_6705823861798411975[37] = 0; out_6705823861798411975[38] = 0; out_6705823861798411975[39] = 0; out_6705823861798411975[40] = 0; out_6705823861798411975[41] = 0; out_6705823861798411975[42] = 0; out_6705823861798411975[43] = 0; out_6705823861798411975[44] = 0; out_6705823861798411975[45] = 0; out_6705823861798411975[46] = 0; out_6705823861798411975[47] = 0; out_6705823861798411975[48] = 0; out_6705823861798411975[49] = 0; out_6705823861798411975[50] = 0; out_6705823861798411975[51] = 0; out_6705823861798411975[52] = 0; out_6705823861798411975[53] = 1; out_6705823861798411975[54] = 0; out_6705823861798411975[55] = 0; out_6705823861798411975[56] = 0; out_6705823861798411975[57] = 0; out_6705823861798411975[58] = 0; out_6705823861798411975[59] = 0; out_6705823861798411975[60] = 0; out_6705823861798411975[61] = 0; out_6705823861798411975[62] = 0; out_6705823861798411975[63] = 0; out_6705823861798411975[64] = 0; out_6705823861798411975[65] = 0; } void h_32(double *state, double *unused, double *out_354434862447744727) { out_354434862447744727[0] = state[3]; out_354434862447744727[1] = state[4]; out_354434862447744727[2] = state[5]; out_354434862447744727[3] = state[6]; } void H_32(double *state, double *unused, double *out_6574659076857671506) { out_6574659076857671506[0] = 0; out_6574659076857671506[1] = 0; out_6574659076857671506[2] = 0; out_6574659076857671506[3] = 1; out_6574659076857671506[4] = 0; out_6574659076857671506[5] = 0; out_6574659076857671506[6] = 0; out_6574659076857671506[7] = 0; out_6574659076857671506[8] = 0; out_6574659076857671506[9] = 0; out_6574659076857671506[10] = 0; out_6574659076857671506[11] = 0; out_6574659076857671506[12] = 0; out_6574659076857671506[13] = 0; out_6574659076857671506[14] = 0; out_6574659076857671506[15] = 0; out_6574659076857671506[16] = 0; out_6574659076857671506[17] = 0; out_6574659076857671506[18] = 0; out_6574659076857671506[19] = 0; out_6574659076857671506[20] = 0; out_6574659076857671506[21] = 0; out_6574659076857671506[22] = 0; out_6574659076857671506[23] = 0; out_6574659076857671506[24] = 0; out_6574659076857671506[25] = 0; out_6574659076857671506[26] = 1; out_6574659076857671506[27] = 0; out_6574659076857671506[28] = 0; out_6574659076857671506[29] = 0; out_6574659076857671506[30] = 0; out_6574659076857671506[31] = 0; out_6574659076857671506[32] = 0; out_6574659076857671506[33] = 0; out_6574659076857671506[34] = 0; out_6574659076857671506[35] = 0; out_6574659076857671506[36] = 0; out_6574659076857671506[37] = 0; out_6574659076857671506[38] = 0; out_6574659076857671506[39] = 0; out_6574659076857671506[40] = 0; out_6574659076857671506[41] = 0; out_6574659076857671506[42] = 0; out_6574659076857671506[43] = 0; out_6574659076857671506[44] = 0; out_6574659076857671506[45] = 0; out_6574659076857671506[46] = 0; out_6574659076857671506[47] = 0; out_6574659076857671506[48] = 0; out_6574659076857671506[49] = 1; out_6574659076857671506[50] = 0; out_6574659076857671506[51] = 0; out_6574659076857671506[52] = 0; out_6574659076857671506[53] = 0; out_6574659076857671506[54] = 0; out_6574659076857671506[55] = 0; out_6574659076857671506[56] = 0; out_6574659076857671506[57] = 0; out_6574659076857671506[58] = 0; out_6574659076857671506[59] = 0; out_6574659076857671506[60] = 0; out_6574659076857671506[61] = 0; out_6574659076857671506[62] = 0; out_6574659076857671506[63] = 0; out_6574659076857671506[64] = 0; out_6574659076857671506[65] = 0; out_6574659076857671506[66] = 0; out_6574659076857671506[67] = 0; out_6574659076857671506[68] = 0; out_6574659076857671506[69] = 0; out_6574659076857671506[70] = 0; out_6574659076857671506[71] = 0; out_6574659076857671506[72] = 1; out_6574659076857671506[73] = 0; out_6574659076857671506[74] = 0; out_6574659076857671506[75] = 0; out_6574659076857671506[76] = 0; out_6574659076857671506[77] = 0; out_6574659076857671506[78] = 0; out_6574659076857671506[79] = 0; out_6574659076857671506[80] = 0; out_6574659076857671506[81] = 0; out_6574659076857671506[82] = 0; out_6574659076857671506[83] = 0; out_6574659076857671506[84] = 0; out_6574659076857671506[85] = 0; out_6574659076857671506[86] = 0; out_6574659076857671506[87] = 0; } void h_13(double *state, double *unused, double *out_5810751130783272681) { out_5810751130783272681[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_5810751130783272681[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_5810751130783272681[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_1515222710564246501) { out_1515222710564246501[0] = 0; out_1515222710564246501[1] = 0; out_1515222710564246501[2] = 0; out_1515222710564246501[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; out_1515222710564246501[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_1515222710564246501[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; out_1515222710564246501[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; out_1515222710564246501[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); out_1515222710564246501[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; out_1515222710564246501[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; out_1515222710564246501[10] = 0; out_1515222710564246501[11] = 0; out_1515222710564246501[12] = 0; out_1515222710564246501[13] = 0; out_1515222710564246501[14] = 0; out_1515222710564246501[15] = 0; out_1515222710564246501[16] = 0; out_1515222710564246501[17] = 0; out_1515222710564246501[18] = 0; out_1515222710564246501[19] = 0; out_1515222710564246501[20] = 0; out_1515222710564246501[21] = 0; out_1515222710564246501[22] = 0; out_1515222710564246501[23] = 0; out_1515222710564246501[24] = 0; out_1515222710564246501[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; out_1515222710564246501[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; out_1515222710564246501[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_1515222710564246501[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; out_1515222710564246501[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; out_1515222710564246501[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); out_1515222710564246501[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; out_1515222710564246501[32] = 0; out_1515222710564246501[33] = 0; out_1515222710564246501[34] = 0; out_1515222710564246501[35] = 0; out_1515222710564246501[36] = 0; out_1515222710564246501[37] = 0; out_1515222710564246501[38] = 0; out_1515222710564246501[39] = 0; out_1515222710564246501[40] = 0; out_1515222710564246501[41] = 0; out_1515222710564246501[42] = 0; out_1515222710564246501[43] = 0; out_1515222710564246501[44] = 0; out_1515222710564246501[45] = 0; out_1515222710564246501[46] = 0; out_1515222710564246501[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; out_1515222710564246501[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; out_1515222710564246501[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; out_1515222710564246501[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; out_1515222710564246501[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; out_1515222710564246501[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; out_1515222710564246501[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); out_1515222710564246501[54] = 0; out_1515222710564246501[55] = 0; out_1515222710564246501[56] = 0; out_1515222710564246501[57] = 0; out_1515222710564246501[58] = 0; out_1515222710564246501[59] = 0; out_1515222710564246501[60] = 0; out_1515222710564246501[61] = 0; out_1515222710564246501[62] = 0; out_1515222710564246501[63] = 0; out_1515222710564246501[64] = 0; out_1515222710564246501[65] = 0; } void h_14(double *state, double *unused, double *out_4760385502323129369) { out_4760385502323129369[0] = state[10]; out_4760385502323129369[1] = state[11]; out_4760385502323129369[2] = state[12]; } void H_14(double *state, double *unused, double *out_2785266983906571881) { out_2785266983906571881[0] = 0; out_2785266983906571881[1] = 0; out_2785266983906571881[2] = 0; out_2785266983906571881[3] = 0; out_2785266983906571881[4] = 0; out_2785266983906571881[5] = 0; out_2785266983906571881[6] = 0; out_2785266983906571881[7] = 0; out_2785266983906571881[8] = 0; out_2785266983906571881[9] = 0; out_2785266983906571881[10] = 1; out_2785266983906571881[11] = 0; out_2785266983906571881[12] = 0; out_2785266983906571881[13] = 0; out_2785266983906571881[14] = 0; out_2785266983906571881[15] = 0; out_2785266983906571881[16] = 0; out_2785266983906571881[17] = 0; out_2785266983906571881[18] = 0; out_2785266983906571881[19] = 0; out_2785266983906571881[20] = 0; out_2785266983906571881[21] = 0; out_2785266983906571881[22] = 0; out_2785266983906571881[23] = 0; out_2785266983906571881[24] = 0; out_2785266983906571881[25] = 0; out_2785266983906571881[26] = 0; out_2785266983906571881[27] = 0; out_2785266983906571881[28] = 0; out_2785266983906571881[29] = 0; out_2785266983906571881[30] = 0; out_2785266983906571881[31] = 0; out_2785266983906571881[32] = 0; out_2785266983906571881[33] = 1; out_2785266983906571881[34] = 0; out_2785266983906571881[35] = 0; out_2785266983906571881[36] = 0; out_2785266983906571881[37] = 0; out_2785266983906571881[38] = 0; out_2785266983906571881[39] = 0; out_2785266983906571881[40] = 0; out_2785266983906571881[41] = 0; out_2785266983906571881[42] = 0; out_2785266983906571881[43] = 0; out_2785266983906571881[44] = 0; out_2785266983906571881[45] = 0; out_2785266983906571881[46] = 0; out_2785266983906571881[47] = 0; out_2785266983906571881[48] = 0; out_2785266983906571881[49] = 0; out_2785266983906571881[50] = 0; out_2785266983906571881[51] = 0; out_2785266983906571881[52] = 0; out_2785266983906571881[53] = 0; out_2785266983906571881[54] = 0; out_2785266983906571881[55] = 0; out_2785266983906571881[56] = 1; out_2785266983906571881[57] = 0; out_2785266983906571881[58] = 0; out_2785266983906571881[59] = 0; out_2785266983906571881[60] = 0; out_2785266983906571881[61] = 0; out_2785266983906571881[62] = 0; out_2785266983906571881[63] = 0; out_2785266983906571881[64] = 0; out_2785266983906571881[65] = 0; } void h_33(double *state, double *unused, double *out_1923132504004862756) { out_1923132504004862756[0] = state[16]; out_1923132504004862756[1] = state[17]; out_1923132504004862756[2] = state[18]; } void H_33(double *state, double *unused, double *out_3555266857159554371) { out_3555266857159554371[0] = 0; out_3555266857159554371[1] = 0; out_3555266857159554371[2] = 0; out_3555266857159554371[3] = 0; out_3555266857159554371[4] = 0; out_3555266857159554371[5] = 0; out_3555266857159554371[6] = 0; out_3555266857159554371[7] = 0; out_3555266857159554371[8] = 0; out_3555266857159554371[9] = 0; out_3555266857159554371[10] = 0; out_3555266857159554371[11] = 0; out_3555266857159554371[12] = 0; out_3555266857159554371[13] = 0; out_3555266857159554371[14] = 0; out_3555266857159554371[15] = 0; out_3555266857159554371[16] = 1; out_3555266857159554371[17] = 0; out_3555266857159554371[18] = 0; out_3555266857159554371[19] = 0; out_3555266857159554371[20] = 0; out_3555266857159554371[21] = 0; out_3555266857159554371[22] = 0; out_3555266857159554371[23] = 0; out_3555266857159554371[24] = 0; out_3555266857159554371[25] = 0; out_3555266857159554371[26] = 0; out_3555266857159554371[27] = 0; out_3555266857159554371[28] = 0; out_3555266857159554371[29] = 0; out_3555266857159554371[30] = 0; out_3555266857159554371[31] = 0; out_3555266857159554371[32] = 0; out_3555266857159554371[33] = 0; out_3555266857159554371[34] = 0; out_3555266857159554371[35] = 0; out_3555266857159554371[36] = 0; out_3555266857159554371[37] = 0; out_3555266857159554371[38] = 0; out_3555266857159554371[39] = 1; out_3555266857159554371[40] = 0; out_3555266857159554371[41] = 0; out_3555266857159554371[42] = 0; out_3555266857159554371[43] = 0; out_3555266857159554371[44] = 0; out_3555266857159554371[45] = 0; out_3555266857159554371[46] = 0; out_3555266857159554371[47] = 0; out_3555266857159554371[48] = 0; out_3555266857159554371[49] = 0; out_3555266857159554371[50] = 0; out_3555266857159554371[51] = 0; out_3555266857159554371[52] = 0; out_3555266857159554371[53] = 0; out_3555266857159554371[54] = 0; out_3555266857159554371[55] = 0; out_3555266857159554371[56] = 0; out_3555266857159554371[57] = 0; out_3555266857159554371[58] = 0; out_3555266857159554371[59] = 0; out_3555266857159554371[60] = 0; out_3555266857159554371[61] = 0; out_3555266857159554371[62] = 1; out_3555266857159554371[63] = 0; out_3555266857159554371[64] = 0; out_3555266857159554371[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_5015980593157513767) { H(in_vec, out_5015980593157513767); } void live_err_fun(double *nom_x, double *delta_x, double *out_1860005180627469095) { err_fun(nom_x, delta_x, out_1860005180627469095); } void live_inv_err_fun(double *nom_x, double *true_x, double *out_1902382254042256432) { inv_err_fun(nom_x, true_x, out_1902382254042256432); } void live_H_mod_fun(double *state, double *out_8394854480302840345) { H_mod_fun(state, out_8394854480302840345); } void live_f_fun(double *state, double dt, double *out_8170256821180437676) { f_fun(state, dt, out_8170256821180437676); } void live_F_fun(double *state, double dt, double *out_6156410520571324808) { F_fun(state, dt, out_6156410520571324808); } void live_h_4(double *state, double *unused, double *out_4490985437783499760) { h_4(state, unused, out_4490985437783499760); } void live_H_4(double *state, double *unused, double *out_8374258154538532265) { H_4(state, unused, out_8374258154538532265); } void live_h_9(double *state, double *unused, double *out_4760385502323129369) { h_9(state, unused, out_4760385502323129369); } void live_H_9(double *state, double *unused, double *out_2785266983906571881) { H_9(state, unused, out_2785266983906571881); } void live_h_10(double *state, double *unused, double *out_1378114642409751093) { h_10(state, unused, out_1378114642409751093); } void live_H_10(double *state, double *unused, double *out_116278907094796186) { H_10(state, unused, out_116278907094796186); } void live_h_12(double *state, double *unused, double *out_6738077689390158393) { h_12(state, unused, out_6738077689390158393); } void live_H_12(double *state, double *unused, double *out_5053029511139057556) { H_12(state, unused, out_5053029511139057556); } void live_h_35(double *state, double *unused, double *out_5147545742760872875) { h_35(state, unused, out_5147545742760872875); } void live_H_35(double *state, double *unused, double *out_6705823861798411975) { H_35(state, unused, out_6705823861798411975); } void live_h_32(double *state, double *unused, double *out_354434862447744727) { h_32(state, unused, out_354434862447744727); } void live_H_32(double *state, double *unused, double *out_6574659076857671506) { H_32(state, unused, out_6574659076857671506); } void live_h_13(double *state, double *unused, double *out_5810751130783272681) { h_13(state, unused, out_5810751130783272681); } void live_H_13(double *state, double *unused, double *out_1515222710564246501) { H_13(state, unused, out_1515222710564246501); } void live_h_14(double *state, double *unused, double *out_4760385502323129369) { h_14(state, unused, out_4760385502323129369); } void live_H_14(double *state, double *unused, double *out_2785266983906571881) { H_14(state, unused, out_2785266983906571881); } void live_h_33(double *state, double *unused, double *out_1923132504004862756) { h_33(state, unused, out_1923132504004862756); } void live_H_33(double *state, double *unused, double *out_3555266857159554371) { H_33(state, unused, out_3555266857159554371); } 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)