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