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