mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-03-02 10:13:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot 2022-08-17T05:06:08 for EON/C2

Browse Source 
version: dragonpilot v0.8.16 beta for EON/C2
date: 2022-08-18T23:02:08
dp-dev(priv2) master commit: 96dd81dde40b92d0686ec5761cfaaab24300b027
This commit is contained in:
Dragonpilot Team
2022-08-17 05:03:29 +00:00
committed by Comma Device
parent 4d7a40310f
commit b437663252
105 changed files with 9516 additions and 3612 deletions
Download Patch File Download Diff File Expand all files Collapse all files

724
selfdrive/locationd/models/generated/gnss.cpp
View File

@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_4620912923766298580) {
out_4620912923766298580[0] = delta_x[0] + nom_x[0];
out_4620912923766298580[1] = delta_x[1] + nom_x[1];
out_4620912923766298580[2] = delta_x[2] + nom_x[2];
out_4620912923766298580[3] = delta_x[3] + nom_x[3];
out_4620912923766298580[4] = delta_x[4] + nom_x[4];
out_4620912923766298580[5] = delta_x[5] + nom_x[5];
out_4620912923766298580[6] = delta_x[6] + nom_x[6];
out_4620912923766298580[7] = delta_x[7] + nom_x[7];
out_4620912923766298580[8] = delta_x[8] + nom_x[8];
out_4620912923766298580[9] = delta_x[9] + nom_x[9];
out_4620912923766298580[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_80014774094776572) {
out_80014774094776572[0] = delta_x[0] + nom_x[0];
out_80014774094776572[1] = delta_x[1] + nom_x[1];
out_80014774094776572[2] = delta_x[2] + nom_x[2];
out_80014774094776572[3] = delta_x[3] + nom_x[3];
out_80014774094776572[4] = delta_x[4] + nom_x[4];
out_80014774094776572[5] = delta_x[5] + nom_x[5];
out_80014774094776572[6] = delta_x[6] + nom_x[6];
out_80014774094776572[7] = delta_x[7] + nom_x[7];
out_80014774094776572[8] = delta_x[8] + nom_x[8];
out_80014774094776572[9] = delta_x[9] + nom_x[9];
out_80014774094776572[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_9187715529705882522) {
out_9187715529705882522[0] = -nom_x[0] + true_x[0];
out_9187715529705882522[1] = -nom_x[1] + true_x[1];
out_9187715529705882522[2] = -nom_x[2] + true_x[2];
out_9187715529705882522[3] = -nom_x[3] + true_x[3];
out_9187715529705882522[4] = -nom_x[4] + true_x[4];
out_9187715529705882522[5] = -nom_x[5] + true_x[5];
out_9187715529705882522[6] = -nom_x[6] + true_x[6];
out_9187715529705882522[7] = -nom_x[7] + true_x[7];
out_9187715529705882522[8] = -nom_x[8] + true_x[8];
out_9187715529705882522[9] = -nom_x[9] + true_x[9];
out_9187715529705882522[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_9144507820461143592) {
out_9144507820461143592[0] = -nom_x[0] + true_x[0];
out_9144507820461143592[1] = -nom_x[1] + true_x[1];
out_9144507820461143592[2] = -nom_x[2] + true_x[2];
out_9144507820461143592[3] = -nom_x[3] + true_x[3];
out_9144507820461143592[4] = -nom_x[4] + true_x[4];
out_9144507820461143592[5] = -nom_x[5] + true_x[5];
out_9144507820461143592[6] = -nom_x[6] + true_x[6];
out_9144507820461143592[7] = -nom_x[7] + true_x[7];
out_9144507820461143592[8] = -nom_x[8] + true_x[8];
out_9144507820461143592[9] = -nom_x[9] + true_x[9];
out_9144507820461143592[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_922973610943044438) {
out_922973610943044438[0] = 1.0;
out_922973610943044438[1] = 0;
out_922973610943044438[2] = 0;
out_922973610943044438[3] = 0;
out_922973610943044438[4] = 0;
out_922973610943044438[5] = 0;
out_922973610943044438[6] = 0;
out_922973610943044438[7] = 0;
out_922973610943044438[8] = 0;
out_922973610943044438[9] = 0;
out_922973610943044438[10] = 0;
out_922973610943044438[11] = 0;
out_922973610943044438[12] = 1.0;
out_922973610943044438[13] = 0;
out_922973610943044438[14] = 0;
out_922973610943044438[15] = 0;
out_922973610943044438[16] = 0;
out_922973610943044438[17] = 0;
out_922973610943044438[18] = 0;
out_922973610943044438[19] = 0;
out_922973610943044438[20] = 0;
out_922973610943044438[21] = 0;
out_922973610943044438[22] = 0;
out_922973610943044438[23] = 0;
out_922973610943044438[24] = 1.0;
out_922973610943044438[25] = 0;
out_922973610943044438[26] = 0;
out_922973610943044438[27] = 0;
out_922973610943044438[28] = 0;
out_922973610943044438[29] = 0;
out_922973610943044438[30] = 0;
out_922973610943044438[31] = 0;
out_922973610943044438[32] = 0;
out_922973610943044438[33] = 0;
out_922973610943044438[34] = 0;
out_922973610943044438[35] = 0;
out_922973610943044438[36] = 1.0;
out_922973610943044438[37] = 0;
out_922973610943044438[38] = 0;
out_922973610943044438[39] = 0;
out_922973610943044438[40] = 0;
out_922973610943044438[41] = 0;
out_922973610943044438[42] = 0;
out_922973610943044438[43] = 0;
out_922973610943044438[44] = 0;
out_922973610943044438[45] = 0;
out_922973610943044438[46] = 0;
out_922973610943044438[47] = 0;
out_922973610943044438[48] = 1.0;
out_922973610943044438[49] = 0;
out_922973610943044438[50] = 0;
out_922973610943044438[51] = 0;
out_922973610943044438[52] = 0;
out_922973610943044438[53] = 0;
out_922973610943044438[54] = 0;
out_922973610943044438[55] = 0;
out_922973610943044438[56] = 0;
out_922973610943044438[57] = 0;
out_922973610943044438[58] = 0;
out_922973610943044438[59] = 0;
out_922973610943044438[60] = 1.0;
out_922973610943044438[61] = 0;
out_922973610943044438[62] = 0;
out_922973610943044438[63] = 0;
out_922973610943044438[64] = 0;
out_922973610943044438[65] = 0;
out_922973610943044438[66] = 0;
out_922973610943044438[67] = 0;
out_922973610943044438[68] = 0;
out_922973610943044438[69] = 0;
out_922973610943044438[70] = 0;
out_922973610943044438[71] = 0;
out_922973610943044438[72] = 1.0;
out_922973610943044438[73] = 0;
out_922973610943044438[74] = 0;
out_922973610943044438[75] = 0;
out_922973610943044438[76] = 0;
out_922973610943044438[77] = 0;
out_922973610943044438[78] = 0;
out_922973610943044438[79] = 0;
out_922973610943044438[80] = 0;
out_922973610943044438[81] = 0;
out_922973610943044438[82] = 0;
out_922973610943044438[83] = 0;
out_922973610943044438[84] = 1.0;
out_922973610943044438[85] = 0;
out_922973610943044438[86] = 0;
out_922973610943044438[87] = 0;
out_922973610943044438[88] = 0;
out_922973610943044438[89] = 0;
out_922973610943044438[90] = 0;
out_922973610943044438[91] = 0;
out_922973610943044438[92] = 0;
out_922973610943044438[93] = 0;
out_922973610943044438[94] = 0;
out_922973610943044438[95] = 0;
out_922973610943044438[96] = 1.0;
out_922973610943044438[97] = 0;
out_922973610943044438[98] = 0;
out_922973610943044438[99] = 0;
out_922973610943044438[100] = 0;
out_922973610943044438[101] = 0;
out_922973610943044438[102] = 0;
out_922973610943044438[103] = 0;
out_922973610943044438[104] = 0;
out_922973610943044438[105] = 0;
out_922973610943044438[106] = 0;
out_922973610943044438[107] = 0;
out_922973610943044438[108] = 1.0;
out_922973610943044438[109] = 0;
out_922973610943044438[110] = 0;
out_922973610943044438[111] = 0;
out_922973610943044438[112] = 0;
out_922973610943044438[113] = 0;
out_922973610943044438[114] = 0;
out_922973610943044438[115] = 0;
out_922973610943044438[116] = 0;
out_922973610943044438[117] = 0;
out_922973610943044438[118] = 0;
out_922973610943044438[119] = 0;
out_922973610943044438[120] = 1.0;
void H_mod_fun(double *state, double *out_3038444346073204801) {
out_3038444346073204801[0] = 1.0;
out_3038444346073204801[1] = 0;
out_3038444346073204801[2] = 0;
out_3038444346073204801[3] = 0;
out_3038444346073204801[4] = 0;
out_3038444346073204801[5] = 0;
out_3038444346073204801[6] = 0;
out_3038444346073204801[7] = 0;
out_3038444346073204801[8] = 0;
out_3038444346073204801[9] = 0;
out_3038444346073204801[10] = 0;
out_3038444346073204801[11] = 0;
out_3038444346073204801[12] = 1.0;
out_3038444346073204801[13] = 0;
out_3038444346073204801[14] = 0;
out_3038444346073204801[15] = 0;
out_3038444346073204801[16] = 0;
out_3038444346073204801[17] = 0;
out_3038444346073204801[18] = 0;
out_3038444346073204801[19] = 0;
out_3038444346073204801[20] = 0;
out_3038444346073204801[21] = 0;
out_3038444346073204801[22] = 0;
out_3038444346073204801[23] = 0;
out_3038444346073204801[24] = 1.0;
out_3038444346073204801[25] = 0;
out_3038444346073204801[26] = 0;
out_3038444346073204801[27] = 0;
out_3038444346073204801[28] = 0;
out_3038444346073204801[29] = 0;
out_3038444346073204801[30] = 0;
out_3038444346073204801[31] = 0;
out_3038444346073204801[32] = 0;
out_3038444346073204801[33] = 0;
out_3038444346073204801[34] = 0;
out_3038444346073204801[35] = 0;
out_3038444346073204801[36] = 1.0;
out_3038444346073204801[37] = 0;
out_3038444346073204801[38] = 0;
out_3038444346073204801[39] = 0;
out_3038444346073204801[40] = 0;
out_3038444346073204801[41] = 0;
out_3038444346073204801[42] = 0;
out_3038444346073204801[43] = 0;
out_3038444346073204801[44] = 0;
out_3038444346073204801[45] = 0;
out_3038444346073204801[46] = 0;
out_3038444346073204801[47] = 0;
out_3038444346073204801[48] = 1.0;
out_3038444346073204801[49] = 0;
out_3038444346073204801[50] = 0;
out_3038444346073204801[51] = 0;
out_3038444346073204801[52] = 0;
out_3038444346073204801[53] = 0;
out_3038444346073204801[54] = 0;
out_3038444346073204801[55] = 0;
out_3038444346073204801[56] = 0;
out_3038444346073204801[57] = 0;
out_3038444346073204801[58] = 0;
out_3038444346073204801[59] = 0;
out_3038444346073204801[60] = 1.0;
out_3038444346073204801[61] = 0;
out_3038444346073204801[62] = 0;
out_3038444346073204801[63] = 0;
out_3038444346073204801[64] = 0;
out_3038444346073204801[65] = 0;
out_3038444346073204801[66] = 0;
out_3038444346073204801[67] = 0;
out_3038444346073204801[68] = 0;
out_3038444346073204801[69] = 0;
out_3038444346073204801[70] = 0;
out_3038444346073204801[71] = 0;
out_3038444346073204801[72] = 1.0;
out_3038444346073204801[73] = 0;
out_3038444346073204801[74] = 0;
out_3038444346073204801[75] = 0;
out_3038444346073204801[76] = 0;
out_3038444346073204801[77] = 0;
out_3038444346073204801[78] = 0;
out_3038444346073204801[79] = 0;
out_3038444346073204801[80] = 0;
out_3038444346073204801[81] = 0;
out_3038444346073204801[82] = 0;
out_3038444346073204801[83] = 0;
out_3038444346073204801[84] = 1.0;
out_3038444346073204801[85] = 0;
out_3038444346073204801[86] = 0;
out_3038444346073204801[87] = 0;
out_3038444346073204801[88] = 0;
out_3038444346073204801[89] = 0;
out_3038444346073204801[90] = 0;
out_3038444346073204801[91] = 0;
out_3038444346073204801[92] = 0;
out_3038444346073204801[93] = 0;
out_3038444346073204801[94] = 0;
out_3038444346073204801[95] = 0;
out_3038444346073204801[96] = 1.0;
out_3038444346073204801[97] = 0;
out_3038444346073204801[98] = 0;
out_3038444346073204801[99] = 0;
out_3038444346073204801[100] = 0;
out_3038444346073204801[101] = 0;
out_3038444346073204801[102] = 0;
out_3038444346073204801[103] = 0;
out_3038444346073204801[104] = 0;
out_3038444346073204801[105] = 0;
out_3038444346073204801[106] = 0;
out_3038444346073204801[107] = 0;
out_3038444346073204801[108] = 1.0;
out_3038444346073204801[109] = 0;
out_3038444346073204801[110] = 0;
out_3038444346073204801[111] = 0;
out_3038444346073204801[112] = 0;
out_3038444346073204801[113] = 0;
out_3038444346073204801[114] = 0;
out_3038444346073204801[115] = 0;
out_3038444346073204801[116] = 0;
out_3038444346073204801[117] = 0;
out_3038444346073204801[118] = 0;
out_3038444346073204801[119] = 0;
out_3038444346073204801[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_1079402724574081004) {
out_1079402724574081004[0] = dt*state[3] + state[0];
out_1079402724574081004[1] = dt*state[4] + state[1];
out_1079402724574081004[2] = dt*state[5] + state[2];
out_1079402724574081004[3] = state[3];
out_1079402724574081004[4] = state[4];
out_1079402724574081004[5] = state[5];
out_1079402724574081004[6] = dt*state[7] + state[6];
out_1079402724574081004[7] = dt*state[8] + state[7];
out_1079402724574081004[8] = state[8];
out_1079402724574081004[9] = state[9];
out_1079402724574081004[10] = state[10];
void f_fun(double *state, double dt, double *out_5331942404729498203) {
out_5331942404729498203[0] = dt*state[3] + state[0];
out_5331942404729498203[1] = dt*state[4] + state[1];
out_5331942404729498203[2] = dt*state[5] + state[2];
out_5331942404729498203[3] = state[3];
out_5331942404729498203[4] = state[4];
out_5331942404729498203[5] = state[5];
out_5331942404729498203[6] = dt*state[7] + state[6];
out_5331942404729498203[7] = dt*state[8] + state[7];
out_5331942404729498203[8] = state[8];
out_5331942404729498203[9] = state[9];
out_5331942404729498203[10] = state[10];
}
void F_fun(double *state, double dt, double *out_3039166340276301210) {
out_3039166340276301210[0] = 1;
out_3039166340276301210[1] = 0;
out_3039166340276301210[2] = 0;
out_3039166340276301210[3] = dt;
out_3039166340276301210[4] = 0;
out_3039166340276301210[5] = 0;
out_3039166340276301210[6] = 0;
out_3039166340276301210[7] = 0;
out_3039166340276301210[8] = 0;
out_3039166340276301210[9] = 0;
out_3039166340276301210[10] = 0;
out_3039166340276301210[11] = 0;
out_3039166340276301210[12] = 1;
out_3039166340276301210[13] = 0;
out_3039166340276301210[14] = 0;
out_3039166340276301210[15] = dt;
out_3039166340276301210[16] = 0;
out_3039166340276301210[17] = 0;
out_3039166340276301210[18] = 0;
out_3039166340276301210[19] = 0;
out_3039166340276301210[20] = 0;
out_3039166340276301210[21] = 0;
out_3039166340276301210[22] = 0;
out_3039166340276301210[23] = 0;
out_3039166340276301210[24] = 1;
out_3039166340276301210[25] = 0;
out_3039166340276301210[26] = 0;
out_3039166340276301210[27] = dt;
out_3039166340276301210[28] = 0;
out_3039166340276301210[29] = 0;
out_3039166340276301210[30] = 0;
out_3039166340276301210[31] = 0;
out_3039166340276301210[32] = 0;
out_3039166340276301210[33] = 0;
out_3039166340276301210[34] = 0;
out_3039166340276301210[35] = 0;
out_3039166340276301210[36] = 1;
out_3039166340276301210[37] = 0;
out_3039166340276301210[38] = 0;
out_3039166340276301210[39] = 0;
out_3039166340276301210[40] = 0;
out_3039166340276301210[41] = 0;
out_3039166340276301210[42] = 0;
out_3039166340276301210[43] = 0;
out_3039166340276301210[44] = 0;
out_3039166340276301210[45] = 0;
out_3039166340276301210[46] = 0;
out_3039166340276301210[47] = 0;
out_3039166340276301210[48] = 1;
out_3039166340276301210[49] = 0;
out_3039166340276301210[50] = 0;
out_3039166340276301210[51] = 0;
out_3039166340276301210[52] = 0;
out_3039166340276301210[53] = 0;
out_3039166340276301210[54] = 0;
out_3039166340276301210[55] = 0;
out_3039166340276301210[56] = 0;
out_3039166340276301210[57] = 0;
out_3039166340276301210[58] = 0;
out_3039166340276301210[59] = 0;
out_3039166340276301210[60] = 1;
out_3039166340276301210[61] = 0;
out_3039166340276301210[62] = 0;
out_3039166340276301210[63] = 0;
out_3039166340276301210[64] = 0;
out_3039166340276301210[65] = 0;
out_3039166340276301210[66] = 0;
out_3039166340276301210[67] = 0;
out_3039166340276301210[68] = 0;
out_3039166340276301210[69] = 0;
out_3039166340276301210[70] = 0;
out_3039166340276301210[71] = 0;
out_3039166340276301210[72] = 1;
out_3039166340276301210[73] = dt;
out_3039166340276301210[74] = 0;
out_3039166340276301210[75] = 0;
out_3039166340276301210[76] = 0;
out_3039166340276301210[77] = 0;
out_3039166340276301210[78] = 0;
out_3039166340276301210[79] = 0;
out_3039166340276301210[80] = 0;
out_3039166340276301210[81] = 0;
out_3039166340276301210[82] = 0;
out_3039166340276301210[83] = 0;
out_3039166340276301210[84] = 1;
out_3039166340276301210[85] = dt;
out_3039166340276301210[86] = 0;
out_3039166340276301210[87] = 0;
out_3039166340276301210[88] = 0;
out_3039166340276301210[89] = 0;
out_3039166340276301210[90] = 0;
out_3039166340276301210[91] = 0;
out_3039166340276301210[92] = 0;
out_3039166340276301210[93] = 0;
out_3039166340276301210[94] = 0;
out_3039166340276301210[95] = 0;
out_3039166340276301210[96] = 1;
out_3039166340276301210[97] = 0;
out_3039166340276301210[98] = 0;
out_3039166340276301210[99] = 0;
out_3039166340276301210[100] = 0;
out_3039166340276301210[101] = 0;
out_3039166340276301210[102] = 0;
out_3039166340276301210[103] = 0;
out_3039166340276301210[104] = 0;
out_3039166340276301210[105] = 0;
out_3039166340276301210[106] = 0;
out_3039166340276301210[107] = 0;
out_3039166340276301210[108] = 1;
out_3039166340276301210[109] = 0;
out_3039166340276301210[110] = 0;
out_3039166340276301210[111] = 0;
out_3039166340276301210[112] = 0;
out_3039166340276301210[113] = 0;
out_3039166340276301210[114] = 0;
out_3039166340276301210[115] = 0;
out_3039166340276301210[116] = 0;
out_3039166340276301210[117] = 0;
out_3039166340276301210[118] = 0;
out_3039166340276301210[119] = 0;
out_3039166340276301210[120] = 1;
void F_fun(double *state, double dt, double *out_3160440324773884395) {
out_3160440324773884395[0] = 1;
out_3160440324773884395[1] = 0;
out_3160440324773884395[2] = 0;
out_3160440324773884395[3] = dt;
out_3160440324773884395[4] = 0;
out_3160440324773884395[5] = 0;
out_3160440324773884395[6] = 0;
out_3160440324773884395[7] = 0;
out_3160440324773884395[8] = 0;
out_3160440324773884395[9] = 0;
out_3160440324773884395[10] = 0;
out_3160440324773884395[11] = 0;
out_3160440324773884395[12] = 1;
out_3160440324773884395[13] = 0;
out_3160440324773884395[14] = 0;
out_3160440324773884395[15] = dt;
out_3160440324773884395[16] = 0;
out_3160440324773884395[17] = 0;
out_3160440324773884395[18] = 0;
out_3160440324773884395[19] = 0;
out_3160440324773884395[20] = 0;
out_3160440324773884395[21] = 0;
out_3160440324773884395[22] = 0;
out_3160440324773884395[23] = 0;
out_3160440324773884395[24] = 1;
out_3160440324773884395[25] = 0;
out_3160440324773884395[26] = 0;
out_3160440324773884395[27] = dt;
out_3160440324773884395[28] = 0;
out_3160440324773884395[29] = 0;
out_3160440324773884395[30] = 0;
out_3160440324773884395[31] = 0;
out_3160440324773884395[32] = 0;
out_3160440324773884395[33] = 0;
out_3160440324773884395[34] = 0;
out_3160440324773884395[35] = 0;
out_3160440324773884395[36] = 1;
out_3160440324773884395[37] = 0;
out_3160440324773884395[38] = 0;
out_3160440324773884395[39] = 0;
out_3160440324773884395[40] = 0;
out_3160440324773884395[41] = 0;
out_3160440324773884395[42] = 0;
out_3160440324773884395[43] = 0;
out_3160440324773884395[44] = 0;
out_3160440324773884395[45] = 0;
out_3160440324773884395[46] = 0;
out_3160440324773884395[47] = 0;
out_3160440324773884395[48] = 1;
out_3160440324773884395[49] = 0;
out_3160440324773884395[50] = 0;
out_3160440324773884395[51] = 0;
out_3160440324773884395[52] = 0;
out_3160440324773884395[53] = 0;
out_3160440324773884395[54] = 0;
out_3160440324773884395[55] = 0;
out_3160440324773884395[56] = 0;
out_3160440324773884395[57] = 0;
out_3160440324773884395[58] = 0;
out_3160440324773884395[59] = 0;
out_3160440324773884395[60] = 1;
out_3160440324773884395[61] = 0;
out_3160440324773884395[62] = 0;
out_3160440324773884395[63] = 0;
out_3160440324773884395[64] = 0;
out_3160440324773884395[65] = 0;
out_3160440324773884395[66] = 0;
out_3160440324773884395[67] = 0;
out_3160440324773884395[68] = 0;
out_3160440324773884395[69] = 0;
out_3160440324773884395[70] = 0;
out_3160440324773884395[71] = 0;
out_3160440324773884395[72] = 1;
out_3160440324773884395[73] = dt;
out_3160440324773884395[74] = 0;
out_3160440324773884395[75] = 0;
out_3160440324773884395[76] = 0;
out_3160440324773884395[77] = 0;
out_3160440324773884395[78] = 0;
out_3160440324773884395[79] = 0;
out_3160440324773884395[80] = 0;
out_3160440324773884395[81] = 0;
out_3160440324773884395[82] = 0;
out_3160440324773884395[83] = 0;
out_3160440324773884395[84] = 1;
out_3160440324773884395[85] = dt;
out_3160440324773884395[86] = 0;
out_3160440324773884395[87] = 0;
out_3160440324773884395[88] = 0;
out_3160440324773884395[89] = 0;
out_3160440324773884395[90] = 0;
out_3160440324773884395[91] = 0;
out_3160440324773884395[92] = 0;
out_3160440324773884395[93] = 0;
out_3160440324773884395[94] = 0;
out_3160440324773884395[95] = 0;
out_3160440324773884395[96] = 1;
out_3160440324773884395[97] = 0;
out_3160440324773884395[98] = 0;
out_3160440324773884395[99] = 0;
out_3160440324773884395[100] = 0;
out_3160440324773884395[101] = 0;
out_3160440324773884395[102] = 0;
out_3160440324773884395[103] = 0;
out_3160440324773884395[104] = 0;
out_3160440324773884395[105] = 0;
out_3160440324773884395[106] = 0;
out_3160440324773884395[107] = 0;
out_3160440324773884395[108] = 1;
out_3160440324773884395[109] = 0;
out_3160440324773884395[110] = 0;
out_3160440324773884395[111] = 0;
out_3160440324773884395[112] = 0;
out_3160440324773884395[113] = 0;
out_3160440324773884395[114] = 0;
out_3160440324773884395[115] = 0;
out_3160440324773884395[116] = 0;
out_3160440324773884395[117] = 0;
out_3160440324773884395[118] = 0;
out_3160440324773884395[119] = 0;
out_3160440324773884395[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_3874365000426709074) {
out_3874365000426709074[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
void h_6(double *state, double *sat_pos, double *out_4164777334676317660) {
out_4164777334676317660[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
}
void H_6(double *state, double *sat_pos, double *out_3986922819635287129) {
out_3986922819635287129[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3986922819635287129[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3986922819635287129[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3986922819635287129[3] = 0;
out_3986922819635287129[4] = 0;
out_3986922819635287129[5] = 0;
out_3986922819635287129[6] = 1;
out_3986922819635287129[7] = 0;
out_3986922819635287129[8] = 0;
out_3986922819635287129[9] = 0;
out_3986922819635287129[10] = 0;
void H_6(double *state, double *sat_pos, double *out_8721522254593615931) {
out_8721522254593615931[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8721522254593615931[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8721522254593615931[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8721522254593615931[3] = 0;
out_8721522254593615931[4] = 0;
out_8721522254593615931[5] = 0;
out_8721522254593615931[6] = 1;
out_8721522254593615931[7] = 0;
out_8721522254593615931[8] = 0;
out_8721522254593615931[9] = 0;
out_8721522254593615931[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_5069168270486129229) {
out_5069168270486129229[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
void h_20(double *state, double *sat_pos, double *out_1506605518158843733) {
out_1506605518158843733[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
}
void H_20(double *state, double *sat_pos, double *out_678918665408427203) {
out_678918665408427203[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_678918665408427203[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_678918665408427203[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_678918665408427203[3] = 0;
out_678918665408427203[4] = 0;
out_678918665408427203[5] = 0;
out_678918665408427203[6] = 1;
out_678918665408427203[7] = 0;
out_678918665408427203[8] = 0;
out_678918665408427203[9] = 1;
out_678918665408427203[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_1146945854611411680) {
out_1146945854611411680[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1146945854611411680[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1146945854611411680[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1146945854611411680[3] = 0;
out_1146945854611411680[4] = 0;
out_1146945854611411680[5] = 0;
out_1146945854611411680[6] = 1;
out_1146945854611411680[7] = 0;
out_1146945854611411680[8] = 0;
out_1146945854611411680[9] = 1;
out_1146945854611411680[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_9105285764076347784) {
out_9105285764076347784[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_7(double *state, double *sat_pos_vel, double *out_2838038345896688742) {
out_2838038345896688742[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_7(double *state, double *sat_pos_vel, double *out_7685540822919128148) {
out_7685540822919128148[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[6] = 0;
out_7685540822919128148[7] = 1;
out_7685540822919128148[8] = 0;
out_7685540822919128148[9] = 0;
out_7685540822919128148[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_7683900239883798223) {
out_7683900239883798223[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[6] = 0;
out_7683900239883798223[7] = 1;
out_7683900239883798223[8] = 0;
out_7683900239883798223[9] = 0;
out_7683900239883798223[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_9105285764076347784) {
out_9105285764076347784[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_21(double *state, double *sat_pos_vel, double *out_2838038345896688742) {
out_2838038345896688742[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_21(double *state, double *sat_pos_vel, double *out_7685540822919128148) {
out_7685540822919128148[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7685540822919128148[6] = 0;
out_7685540822919128148[7] = 1;
out_7685540822919128148[8] = 0;
out_7685540822919128148[9] = 0;
out_7685540822919128148[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_7683900239883798223) {
out_7683900239883798223[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_7683900239883798223[6] = 0;
out_7683900239883798223[7] = 1;
out_7683900239883798223[8] = 0;
out_7683900239883798223[9] = 0;
out_7683900239883798223[10] = 0;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
void gnss_err_fun(double *nom_x, double *delta_x, double *out_4620912923766298580) {
err_fun(nom_x, delta_x, out_4620912923766298580);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_80014774094776572) {
err_fun(nom_x, delta_x, out_80014774094776572);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9187715529705882522) {
inv_err_fun(nom_x, true_x, out_9187715529705882522);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9144507820461143592) {
inv_err_fun(nom_x, true_x, out_9144507820461143592);
}
void gnss_H_mod_fun(double *state, double *out_922973610943044438) {
H_mod_fun(state, out_922973610943044438);
void gnss_H_mod_fun(double *state, double *out_3038444346073204801) {
H_mod_fun(state, out_3038444346073204801);
}
void gnss_f_fun(double *state, double dt, double *out_1079402724574081004) {
f_fun(state, dt, out_1079402724574081004);
void gnss_f_fun(double *state, double dt, double *out_5331942404729498203) {
f_fun(state, dt, out_5331942404729498203);
}
void gnss_F_fun(double *state, double dt, double *out_3039166340276301210) {
F_fun(state, dt, out_3039166340276301210);
void gnss_F_fun(double *state, double dt, double *out_3160440324773884395) {
F_fun(state, dt, out_3160440324773884395);
}
void gnss_h_6(double *state, double *sat_pos, double *out_3874365000426709074) {
h_6(state, sat_pos, out_3874365000426709074);
void gnss_h_6(double *state, double *sat_pos, double *out_4164777334676317660) {
h_6(state, sat_pos, out_4164777334676317660);
}
void gnss_H_6(double *state, double *sat_pos, double *out_3986922819635287129) {
H_6(state, sat_pos, out_3986922819635287129);
void gnss_H_6(double *state, double *sat_pos, double *out_8721522254593615931) {
H_6(state, sat_pos, out_8721522254593615931);
}
void gnss_h_20(double *state, double *sat_pos, double *out_5069168270486129229) {
h_20(state, sat_pos, out_5069168270486129229);
void gnss_h_20(double *state, double *sat_pos, double *out_1506605518158843733) {
h_20(state, sat_pos, out_1506605518158843733);
}
void gnss_H_20(double *state, double *sat_pos, double *out_678918665408427203) {
H_20(state, sat_pos, out_678918665408427203);
void gnss_H_20(double *state, double *sat_pos, double *out_1146945854611411680) {
H_20(state, sat_pos, out_1146945854611411680);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_9105285764076347784) {
h_7(state, sat_pos_vel, out_9105285764076347784);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_2838038345896688742) {
h_7(state, sat_pos_vel, out_2838038345896688742);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7685540822919128148) {
H_7(state, sat_pos_vel, out_7685540822919128148);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7683900239883798223) {
H_7(state, sat_pos_vel, out_7683900239883798223);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_9105285764076347784) {
h_21(state, sat_pos_vel, out_9105285764076347784);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_2838038345896688742) {
h_21(state, sat_pos_vel, out_2838038345896688742);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7685540822919128148) {
H_21(state, sat_pos_vel, out_7685540822919128148);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7683900239883798223) {
H_21(state, sat_pos_vel, out_7683900239883798223);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);