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