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