mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
71d02f36106dcbe2d3df717f74e268c69b3b5184
onepilot/selfdrive/locationd/models/generated/live.cpp
T
FrogAi 71d02f3610 Compile FrogPilot
2024-03-19 18:41:15 -07:00

1988 lines
90 KiB
C++
Raw Blame History

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