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