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