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