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