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