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