/// NOTE: This is a generated file; do not edit. #pragma once #include // clang-format off static inline void LighthouseIMUPrediction(CnMat* out, const SurvivePose* _x1) { const FLT x0 = (*_x1).Rot[2] * (*_x1).Rot[2]; const FLT x1 = (*_x1).Rot[1] * (*_x1).Rot[1]; const FLT x2 = 1. / (x1 + ((*_x1).Rot[0] * (*_x1).Rot[0]) + ((*_x1).Rot[3] * (*_x1).Rot[3]) + x0); const FLT x3 = x2 * (*_x1).Rot[1]; const FLT x4 = x2 * (*_x1).Rot[2]; cnMatrixOptionalSet(out, 0, 0, 2 * ((-1 * x4 * (*_x1).Rot[0]) + (x3 * (*_x1).Rot[3]))); cnMatrixOptionalSet(out, 1, 0, 2 * ((x4 * (*_x1).Rot[3]) + (x3 * (*_x1).Rot[0]))); cnMatrixOptionalSet(out, 2, 0, 1 + (2 * ((-1 * x2 * x1) + (-1 * x0 * x2)))); } // Jacobian of LighthouseIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] static inline void LighthouseIMUPrediction_jac_x1(CnMat* Hx, const SurvivePose* _x1) { const FLT x0 = (*_x1).Rot[0] * (*_x1).Rot[0]; const FLT x1 = (*_x1).Rot[3] * (*_x1).Rot[3]; const FLT x2 = (*_x1).Rot[2] * (*_x1).Rot[2]; const FLT x3 = (*_x1).Rot[1] * (*_x1).Rot[1]; const FLT x4 = x3 + x0 + x1 + x2; const FLT x5 = 1. / (x4 * x4); const FLT x6 = 2 * x5; const FLT x7 = x6 * (*_x1).Rot[2]; const FLT x8 = x6 * (*_x1).Rot[1]; const FLT x9 = x8 * (*_x1).Rot[3]; const FLT x10 = -1 * x9 * (*_x1).Rot[0]; const FLT x11 = 1. / x4; const FLT x12 = x11 * (*_x1).Rot[2]; const FLT x13 = x8 * (*_x1).Rot[2] * (*_x1).Rot[0]; const FLT x14 = x11 * (*_x1).Rot[3]; const FLT x15 = x3 * x6; const FLT x16 = x2 * x6; const FLT x17 = -1 * x9 * (*_x1).Rot[2]; const FLT x18 = x11 * (*_x1).Rot[0]; const FLT x19 = x7 * (*_x1).Rot[3] * (*_x1).Rot[0]; const FLT x20 = x11 * (*_x1).Rot[1]; const FLT x21 = 4 * x5; const FLT x22 = x21 * (*_x1).Rot[0]; const FLT x23 = x2 * x21; const FLT x24 = x3 * x21; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x12) + (x0 * x7) + x10)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * ((-1 * x15 * (*_x1).Rot[3]) + x13 + x14)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x18) + (x16 * (*_x1).Rot[0]) + x17)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * ((-1 * x1 * x8) + x19 + x20)); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x0 * x8) + (-1 * x19) + x20)); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * (x17 + (-1 * x15 * (*_x1).Rot[0]) + x18)); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * (x14 + (-1 * x16 * (*_x1).Rot[3]) + (-1 * x13))); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * (x12 + (-1 * x1 * x7) + x10)); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[0])/sizeof(FLT), (x3 * x22) + (x2 * x22)); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (x23 * (*_x1).Rot[1]) + (-4 * x20) + (x21 * ((*_x1).Rot[1] * (*_x1).Rot[1] * (*_x1).Rot[1]))); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-4 * x12) + (x21 * ((*_x1).Rot[2] * (*_x1).Rot[2] * (*_x1).Rot[2])) + (x24 * (*_x1).Rot[2])); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x24 * (*_x1).Rot[3]) + (x23 * (*_x1).Rot[3])); } // Full version Jacobian of LighthouseIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] static inline void LighthouseIMUPrediction_jac_x1_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1) { if(hx != 0) { LighthouseIMUPrediction(hx, _x1); } if(Hx != 0) { LighthouseIMUPrediction_jac_x1(Hx, _x1); } } static inline void LighthouseErrorIMUPrediction(CnMat* out, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x1).Rot[3]; const FLT x1 = 0.5 * (*_x1).Rot[2]; const FLT x2 = 0.5 * (*error_state).AxisAngleRot[0]; const FLT x3 = (*_x1).Rot[1] + (x2 * (*_x1).Rot[0]) + (-1 * x0 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]); const FLT x4 = 0.5 * (*_x1).Rot[0]; const FLT x5 = 0.5 * (*_x1).Rot[1]; const FLT x6 = (x5 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x2 * (*_x1).Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = (-1 * x5 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[2] + (x4 * (*error_state).AxisAngleRot[1]) + (x2 * (*_x1).Rot[3]); const FLT x9 = x8 * x8; const FLT x10 = (-1 * x2 * (*_x1).Rot[1]) + (*_x1).Rot[0] + (-1 * x0 * (*error_state).AxisAngleRot[2]) + (-1 * x1 * (*error_state).AxisAngleRot[1]); const FLT x11 = x10 * x10; const FLT x12 = x3 * x3; const FLT x13 = 1. / (x11 + x12 + x7 + x9); const FLT x14 = x9 * x13; const FLT x15 = x13 * x12; const FLT x16 = 1. / (x15 + (x13 * x11) + (x7 * x13) + x14); const FLT x17 = x13 * x16; const FLT x18 = x6 * x17; const FLT x19 = x10 * x17; cnMatrixOptionalSet(out, 0, 0, 2 * ((-1 * x8 * x19) + (x3 * x18))); cnMatrixOptionalSet(out, 1, 0, 2 * ((x8 * x18) + (x3 * x19))); cnMatrixOptionalSet(out, 2, 0, 1 + (2 * ((-1 * x15 * x16) + (-1 * x14 * x16)))); } // Jacobian of LighthouseErrorIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] static inline void LighthouseErrorIMUPrediction_jac_x1(CnMat* Hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x1).Rot[2]; const FLT x1 = 0.5 * (*_x1).Rot[0]; const FLT x2 = 0.5 * (*error_state).AxisAngleRot[1]; const FLT x3 = (x2 * (*_x1).Rot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x0 * (*error_state).AxisAngleRot[0]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*_x1).Rot[3]; const FLT x6 = 0.5 * (*_x1).Rot[1]; const FLT x7 = (-1 * x6 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (*_x1).Rot[2] + (x5 * (*error_state).AxisAngleRot[0]); const FLT x8 = x7 * x7; const FLT x9 = (-1 * x6 * (*error_state).AxisAngleRot[0]) + (*_x1).Rot[0] + (-1 * x5 * (*error_state).AxisAngleRot[2]) + (-1 * x2 * (*_x1).Rot[2]); const FLT x10 = x9 * x9; const FLT x11 = (*_x1).Rot[1] + (x1 * (*error_state).AxisAngleRot[0]) + (-1 * x5 * (*error_state).AxisAngleRot[1]) + (x0 * (*error_state).AxisAngleRot[2]); const FLT x12 = x11 * x11; const FLT x13 = x10 + x12 + x4 + x8; const FLT x14 = 1. / x13; const FLT x15 = x8 * x14; const FLT x16 = x14 * x12; const FLT x17 = (x14 * x10) + (x4 * x14) + x16 + x15; const FLT x18 = 1. / (x17 * x17); const FLT x19 = 1. / (x13 * x13); const FLT x20 = 1.0 * x11; const FLT x21 = x20 * (*error_state).AxisAngleRot[0]; const FLT x22 = 2 * x9; const FLT x23 = 1.0 * x3; const FLT x24 = x23 * (*error_state).AxisAngleRot[2]; const FLT x25 = 1.0 * x7; const FLT x26 = x25 * (*error_state).AxisAngleRot[1]; const FLT x27 = x26 + x24 + x21 + x22; const FLT x28 = x27 * x19; const FLT x29 = x28 * x12; const FLT x30 = x4 * x19; const FLT x31 = x8 * x28; const FLT x32 = ((-1 * x31) + (-1 * x30 * x27) + (x21 * x14) + (x26 * x14) + (-1 * x28 * x10) + (-1 * x29) + (x24 * x14) + (x22 * x14)) * x18; const FLT x33 = x9 * x14; const FLT x34 = x7 * x33; const FLT x35 = 1. / x17; const FLT x36 = x35 * x14; const FLT x37 = x7 * x36; const FLT x38 = x36 * x11; const FLT x39 = 0.5 * (*error_state).AxisAngleRot[2]; const FLT x40 = x38 * x39; const FLT x41 = x9 * x35; const FLT x42 = x41 * x28; const FLT x43 = x3 * x11; const FLT x44 = x43 * x14; const FLT x45 = x35 * x28; const FLT x46 = x9 * x36; const FLT x47 = x3 * x36; const FLT x48 = 0.5 * (*error_state).AxisAngleRot[0]; const FLT x49 = (x47 * x48) + (-1 * x2 * x46); const FLT x50 = 2 * x11; const FLT x51 = x23 * (*error_state).AxisAngleRot[1]; const FLT x52 = 1.0 * x9; const FLT x53 = x52 * (*error_state).AxisAngleRot[0]; const FLT x54 = x25 * (*error_state).AxisAngleRot[2]; const FLT x55 = (-1 * x54) + (-1 * x53) + x50 + x51; const FLT x56 = x55 * x19; const FLT x57 = x8 * x56; const FLT x58 = x56 * x12; const FLT x59 = (-1 * x53 * x14) + (x50 * x14) + (-1 * x55 * x30) + (-1 * x56 * x10) + (x51 * x14) + (-1 * x54 * x14) + (-1 * x57) + (-1 * x58); const FLT x60 = x59 * x18; const FLT x61 = x48 * x37; const FLT x62 = x56 * x41; const FLT x63 = x2 * x38; const FLT x64 = x56 * x35; const FLT x65 = (x46 * x39) + x47; const FLT x66 = x20 * (*error_state).AxisAngleRot[2]; const FLT x67 = x23 * (*error_state).AxisAngleRot[0]; const FLT x68 = x52 * (*error_state).AxisAngleRot[1]; const FLT x69 = 2 * x7; const FLT x70 = x69 + (-1 * x68) + x66 + (-1 * x67); const FLT x71 = x70 * x19; const FLT x72 = x71 * x41; const FLT x73 = x47 * x39; const FLT x74 = x71 * x35; const FLT x75 = x71 * x12; const FLT x76 = x8 * x71; const FLT x77 = (-1 * x68 * x14) + (-1 * x76) + (x66 * x14) + (x69 * x14) + (-1 * x75) + (-1 * x67 * x14) + (-1 * x71 * x10) + (-1 * x70 * x30); const FLT x78 = x77 * x18; const FLT x79 = (x2 * x37) + (-1 * x48 * x38); const FLT x80 = x20 * (*error_state).AxisAngleRot[1]; const FLT x81 = x52 * (*error_state).AxisAngleRot[2]; const FLT x82 = 2 * x3; const FLT x83 = x25 * (*error_state).AxisAngleRot[0]; const FLT x84 = x83 + x82 + (-1 * x80) + (-1 * x81); const FLT x85 = x84 * x19; const FLT x86 = x85 * x35; const FLT x87 = x9 * x86; const FLT x88 = x46 * x48; const FLT x89 = x2 * x47; const FLT x90 = x85 * x12; const FLT x91 = x8 * x85; const FLT x92 = ((x83 * x14) + (-1 * x91) + (x82 * x14) + (-1 * x80 * x14) + (-1 * x90) + (-1 * x85 * x10) + (-1 * x81 * x14) + (-1 * x84 * x30)) * x18; const FLT x93 = (x37 * x39) + x38; const FLT x94 = x3 * x7; const FLT x95 = x94 * x14; const FLT x96 = x33 * x11; const FLT x97 = 2 * x35; const FLT x98 = 2.0 * x38; const FLT x99 = 2.0 * x37; const FLT x100 = 2 * x32; const FLT x101 = 2 * x18; const FLT x102 = x59 * x101; const FLT x103 = x77 * x101; const FLT x104 = 2 * x92; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x43 * x45) + x49 + (x32 * x34) + (x7 * x42) + (-1 * x37) + x40 + (-1 * x44 * x32))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * (x61 + (x7 * x62) + (-1 * x64 * x43) + (x60 * x34) + (-1 * x60 * x44) + x65 + x63)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x46) + (-1 * x74 * x43) + x79 + x73 + (x7 * x72) + (x78 * x34) + (-1 * x78 * x44))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * (x93 + (-1 * x86 * x43) + (x7 * x87) + (-1 * x89) + (-1 * x88) + (x92 * x34) + (-1 * x92 * x44))); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * (x93 + (-1 * x42 * x11) + x88 + (-1 * x94 * x45) + (-1 * x95 * x32) + x89 + (-1 * x96 * x32))); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * ((-1 * x62 * x11) + x46 + (-1 * x64 * x94) + x79 + (-1 * x73) + (-1 * x60 * x95) + (-1 * x60 * x96))); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x61) + x65 + (-1 * x63) + (-1 * x78 * x95) + (-1 * x74 * x94) + (-1 * x72 * x11) + (-1 * x78 * x96))); cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * ((-1 * x87 * x11) + x49 + (-1 * x92 * x95) + (-1 * x86 * x94) + (-1 * x92 * x96) + (-1 * x40) + x37)); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[0])/sizeof(FLT), (x97 * x31) + (-1 * x99 * (*error_state).AxisAngleRot[1]) + (x15 * x100) + (x97 * x29) + (-1 * x98 * (*error_state).AxisAngleRot[0]) + (x16 * x100)); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (x99 * (*error_state).AxisAngleRot[2]) + (-4 * x38) + (x57 * x97) + (x58 * x97) + (x15 * x102) + (x16 * x102)); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[2])/sizeof(FLT), (x15 * x103) + (x76 * x97) + (x16 * x103) + (x75 * x97) + (-4 * x37) + (-1 * x98 * (*error_state).AxisAngleRot[2])); cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x98 * (*error_state).AxisAngleRot[1]) + (x15 * x104) + (-1 * x99 * (*error_state).AxisAngleRot[0]) + (x91 * x97) + (x16 * x104) + (x90 * x97)); } // Full version Jacobian of LighthouseErrorIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] static inline void LighthouseErrorIMUPrediction_jac_x1_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) { if(hx != 0) { LighthouseErrorIMUPrediction(hx, _x1, error_state); } if(Hx != 0) { LighthouseErrorIMUPrediction_jac_x1(Hx, _x1, error_state); } } // Jacobian of LighthouseErrorIMUPrediction wrt [(*error_state).AxisAngleRot[0], (*error_state).AxisAngleRot[1], (*error_state).AxisAngleRot[2], (*error_state).Pos[0], (*error_state).Pos[1], (*error_state).Pos[2]] static inline void LighthouseErrorIMUPrediction_jac_error_state(CnMat* Hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*error_state).AxisAngleRot[1]; const FLT x1 = 0.5 * (*error_state).AxisAngleRot[2]; const FLT x2 = 0.5 * (*_x1).Rot[0]; const FLT x3 = (*_x1).Rot[1] + (x2 * (*error_state).AxisAngleRot[0]) + (-1 * x0 * (*_x1).Rot[3]) + (x1 * (*_x1).Rot[2]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*error_state).AxisAngleRot[0]; const FLT x6 = (-1 * x5 * (*_x1).Rot[1]) + (-1 * x1 * (*_x1).Rot[3]) + (*_x1).Rot[0] + (-1 * x0 * (*_x1).Rot[2]); const FLT x7 = 1.0 * (*_x1).Rot[1]; const FLT x8 = x6 * x7; const FLT x9 = (x0 * (*_x1).Rot[1]) + (x2 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x5 * (*_x1).Rot[2]); const FLT x10 = 1.0 * x9; const FLT x11 = x10 * (*_x1).Rot[2]; const FLT x12 = 1.0 * x3; const FLT x13 = x12 * (*_x1).Rot[0]; const FLT x14 = (*_x1).Rot[2] + (-1 * x1 * (*_x1).Rot[1]) + (x0 * (*_x1).Rot[0]) + (x5 * (*_x1).Rot[3]); const FLT x15 = 1.0 * x14; const FLT x16 = x15 * (*_x1).Rot[3]; const FLT x17 = x16 + x13 + (-1 * x8) + (-1 * x11); const FLT x18 = x9 * x9; const FLT x19 = x14 * x14; const FLT x20 = x6 * x6; const FLT x21 = x20 + x4 + x18 + x19; const FLT x22 = 1. / (x21 * x21); const FLT x23 = x22 * x17; const FLT x24 = x4 * x23; const FLT x25 = 1. / x21; const FLT x26 = x22 * x20; const FLT x27 = x23 * x19; const FLT x28 = (-1 * x27) + (-1 * x25 * x11) + (x25 * x13) + (-1 * x26 * x17) + (-1 * x24) + (x25 * x16) + (-1 * x8 * x25) + (-1 * x23 * x18); const FLT x29 = x25 * x19; const FLT x30 = x4 * x25; const FLT x31 = x30 + (x25 * x20) + (x25 * x18) + x29; const FLT x32 = 1. / (x31 * x31); const FLT x33 = x32 * x25; const FLT x34 = x33 * x28; const FLT x35 = x6 * x14; const FLT x36 = 1. / x31; const FLT x37 = x36 * x23; const FLT x38 = x3 * x9; const FLT x39 = x36 * x25; const FLT x40 = x2 * x39; const FLT x41 = 0.5 * x39; const FLT x42 = x6 * x41; const FLT x43 = x41 * (*_x1).Rot[2]; const FLT x44 = x41 * x14; const FLT x45 = (x44 * (*_x1).Rot[1]) + (-1 * x3 * x43) + (x9 * x40) + (-1 * x42 * (*_x1).Rot[3]); const FLT x46 = 1.0 * x6; const FLT x47 = x46 * (*_x1).Rot[2]; const FLT x48 = x7 * x9; const FLT x49 = x12 * (*_x1).Rot[3]; const FLT x50 = x15 * (*_x1).Rot[0]; const FLT x51 = x50 + (-1 * x49) + (-1 * x47) + x48; const FLT x52 = x51 * x22; const FLT x53 = x52 * x36; const FLT x54 = x3 * x41; const FLT x55 = x54 * (*_x1).Rot[1]; const FLT x56 = x4 * x52; const FLT x57 = x52 * x19; const FLT x58 = (-1 * x57) + (-1 * x47 * x25) + (x50 * x25) + (-1 * x56) + (-1 * x51 * x26) + (-1 * x49 * x25) + (-1 * x52 * x18) + (x48 * x25); const FLT x59 = x58 * x33; const FLT x60 = x43 * x14; const FLT x61 = x9 * x41; const FLT x62 = x61 * (*_x1).Rot[3]; const FLT x63 = x6 * x40; const FLT x64 = x12 * (*_x1).Rot[2]; const FLT x65 = x46 * (*_x1).Rot[3]; const FLT x66 = x10 * (*_x1).Rot[0]; const FLT x67 = x7 * x14; const FLT x68 = (-1 * x67) + x66 + x64 + (-1 * x65); const FLT x69 = x68 * x22; const FLT x70 = x69 * x36; const FLT x71 = x4 * x69; const FLT x72 = x69 * x19; const FLT x73 = (-1 * x72) + (-1 * x71) + (-1 * x67 * x25) + (-1 * x69 * x18) + (-1 * x68 * x26) + (x64 * x25) + (-1 * x65 * x25) + (x66 * x25); const FLT x74 = x73 * x33; const FLT x75 = x9 * x14; const FLT x76 = x3 * x6; const FLT x77 = 2.0 * x39; const FLT x78 = x3 * x77; const FLT x79 = 2 * x32; const FLT x80 = x79 * x28; const FLT x81 = 2 * x36; const FLT x82 = x77 * x14; const FLT x83 = x79 * x58; const FLT x84 = x73 * x79; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), 2 * (x45 + (-1 * x34 * x38) + (x34 * x35) + (-1 * x38 * x37) + (x35 * x37))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), 2 * ((-1 * x53 * x38) + x55 + x60 + (-1 * x63) + (x53 * x35) + (-1 * x59 * x38) + (x59 * x35) + (-1 * x62))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), 2 * ((-1 * x70 * x38) + (-1 * x74 * x38) + (x74 * x35) + (x9 * x43) + (x44 * (*_x1).Rot[3]) + (x42 * (*_x1).Rot[1]) + (x3 * x40) + (x70 * x35))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), 2 * ((-1 * x76 * x37) + x63 + (-1 * x55) + (-1 * x75 * x34) + (-1 * x76 * x34) + (-1 * x75 * x37) + x62 + (-1 * x60))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), 2 * (x45 + (-1 * x76 * x59) + (-1 * x75 * x53) + (-1 * x76 * x53) + (-1 * x75 * x59))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), 2 * ((-1 * x61 * (*_x1).Rot[1]) + (-1 * x70 * x76) + (-1 * x54 * (*_x1).Rot[3]) + (-1 * x75 * x74) + (-1 * x70 * x75) + (x40 * x14) + (x6 * x43) + (-1 * x74 * x76))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (x80 * x30) + (-1 * x78 * (*_x1).Rot[0]) + (x80 * x29) + (x81 * x24) + (x81 * x27) + (-1 * x82 * (*_x1).Rot[3])); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (x83 * x29) + (x78 * (*_x1).Rot[3]) + (x81 * x57) + (x83 * x30) + (-1 * x82 * (*_x1).Rot[0]) + (x81 * x56)); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (x84 * x29) + (x84 * x30) + (x82 * (*_x1).Rot[1]) + (x81 * x72) + (-1 * x78 * (*_x1).Rot[2]) + (x81 * x71)); } // Full version Jacobian of LighthouseErrorIMUPrediction wrt [(*error_state).AxisAngleRot[0], (*error_state).AxisAngleRot[1], (*error_state).AxisAngleRot[2], (*error_state).Pos[0], (*error_state).Pos[1], (*error_state).Pos[2]] static inline void LighthouseErrorIMUPrediction_jac_error_state_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) { if(hx != 0) { LighthouseErrorIMUPrediction(hx, _x1, error_state); } if(Hx != 0) { LighthouseErrorIMUPrediction_jac_error_state(Hx, _x1, error_state); } } static inline void SurviveLighthouseKalmanModelToErrorModel(SurviveLighthouseKalmanErrorModel* out, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) { const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x1 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x2 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x3 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x4 = x0 * x0; out->Lighthouse.Pos[0]=(*_x1).Lighthouse.Pos[0] + (-1 * (*_x0).Lighthouse.Pos[0]); out->Lighthouse.Pos[1]=(*_x1).Lighthouse.Pos[1] + (-1 * (*_x0).Lighthouse.Pos[1]); out->Lighthouse.Pos[2]=(*_x1).Lighthouse.Pos[2] + (-1 * (*_x0).Lighthouse.Pos[2]); out->Lighthouse.AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x3 * x3) + x4))); out->Lighthouse.AxisAngleRot[1]=asin(2 * ((x0 * x2) + (-1 * x1 * x3))); out->Lighthouse.AxisAngleRot[2]=atan2(2 * ((x2 * x1) + (x0 * x3)), 1 + (-2 * (x4 + (x1 * x1)))); out->BSD0.phase=(*_x1).BSD0.phase + (-1 * (*_x0).BSD0.phase); out->BSD0.tilt=(*_x1).BSD0.tilt + (-1 * (*_x0).BSD0.tilt); out->BSD0.curve=(*_x1).BSD0.curve + (-1 * (*_x0).BSD0.curve); out->BSD0.gibpha=(*_x1).BSD0.gibpha + (-1 * (*_x0).BSD0.gibpha); out->BSD0.gibmag=(*_x1).BSD0.gibmag + (-1 * (*_x0).BSD0.gibmag); out->BSD0.ogeephase=(*_x1).BSD0.ogeephase + (-1 * (*_x0).BSD0.ogeephase); out->BSD0.ogeemag=(*_x1).BSD0.ogeemag + (-1 * (*_x0).BSD0.ogeemag); out->BSD1.phase=(*_x1).BSD1.phase + (-1 * (*_x0).BSD1.phase); out->BSD1.tilt=(*_x1).BSD1.tilt + (-1 * (*_x0).BSD1.tilt); out->BSD1.curve=(*_x1).BSD1.curve + (-1 * (*_x0).BSD1.curve); out->BSD1.gibpha=(*_x1).BSD1.gibpha + (-1 * (*_x0).BSD1.gibpha); out->BSD1.gibmag=(*_x1).BSD1.gibmag + (-1 * (*_x0).BSD1.gibmag); out->BSD1.ogeephase=(*_x1).BSD1.ogeephase + (-1 * (*_x0).BSD1.ogeephase); out->BSD1.ogeemag=(*_x1).BSD1.ogeemag + (-1 * (*_x0).BSD1.ogeemag); } // Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveLighthouseKalmanModelToErrorModel_jac_x1(CnMat* Hx, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) { const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x1 = x0 * (*_x0).Lighthouse.Rot[3]; const FLT x2 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x3 = x2 * (*_x0).Lighthouse.Rot[2]; const FLT x4 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x5 = x4 * (*_x0).Lighthouse.Rot[0]; const FLT x6 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x7 = x6 * (*_x0).Lighthouse.Rot[1]; const FLT x8 = (-1 * x7) + x5; const FLT x9 = x0 * x0; const FLT x10 = 1 + (-2 * ((x4 * x4) + x9)); const FLT x11 = 2 * (1. / x10); const FLT x12 = x0 * (*_x0).Lighthouse.Rot[2]; const FLT x13 = 4 * x12; const FLT x14 = x4 * (*_x0).Lighthouse.Rot[1]; const FLT x15 = x10 * x10; const FLT x16 = (x4 * x6) + (x0 * x2); const FLT x17 = 2 * (1. / x15) * x16; const FLT x18 = x15 * (1. / (x15 + (4 * (x16 * x16)))); const FLT x19 = x2 * (*_x0).Lighthouse.Rot[3]; const FLT x20 = x14 + (x6 * (*_x0).Lighthouse.Rot[0]); const FLT x21 = x20 + x12 + (-1 * x19); const FLT x22 = 4 * x1; const FLT x23 = x4 * (*_x0).Lighthouse.Rot[2]; const FLT x24 = x6 * (*_x0).Lighthouse.Rot[3]; const FLT x25 = x0 * (*_x0).Lighthouse.Rot[1]; const FLT x26 = x2 * (*_x0).Lighthouse.Rot[0]; const FLT x27 = x26 + (-1 * x25); const FLT x28 = x0 * (*_x0).Lighthouse.Rot[0]; const FLT x29 = -4 * x28; const FLT x30 = x4 * (*_x0).Lighthouse.Rot[3]; const FLT x31 = x6 * (*_x0).Lighthouse.Rot[2]; const FLT x32 = x2 * (*_x0).Lighthouse.Rot[1]; const FLT x33 = x28 + x32; const FLT x34 = x33 + x30 + (-1 * x31); const FLT x35 = -4 * x25; const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x6) + (-1 * x2 * x4)) * ((x0 * x6) + (-1 * x2 * x4)))))); const FLT x37 = (-1 * x23) + (-1 * x24); const FLT x38 = x3 + x1; const FLT x39 = 1 + (-2 * (x9 + (x2 * x2))); const FLT x40 = 2 * (1. / x39); const FLT x41 = x39 * x39; const FLT x42 = (x2 * x6) + (x0 * x4); const FLT x43 = 2 * (1. / x41) * x42; const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x5) + x22)) + (x21 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24)))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x23) + x35) * x17) + (x34 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x34 * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x36 * (x37 + x25 + (-1 * x26))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x36 * x21); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x36 * (x38 + x7 + (-1 * x5))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30))))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8)))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x26)) * x43) + (x40 * (x20 + (-1 * x12) + x19)))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] // Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveLighthouseKalmanModelToErrorModel_jac_x0(CnMat* Hx, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) { const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x1 = x0 * (*_x1).Lighthouse.Rot[3]; const FLT x2 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x3 = x2 * (*_x1).Lighthouse.Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x6 = x5 * (*_x1).Lighthouse.Rot[1]; const FLT x7 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x8 = x7 * (*_x1).Lighthouse.Rot[2]; const FLT x9 = x8 + x6; const FLT x10 = x0 * x0; const FLT x11 = 1 + (-2 * ((x2 * x2) + x10)); const FLT x12 = 2 * (1. / x11); const FLT x13 = x0 * (*_x1).Lighthouse.Rot[2]; const FLT x14 = -4 * x13; const FLT x15 = x2 * (*_x1).Lighthouse.Rot[1]; const FLT x16 = x11 * x11; const FLT x17 = (x2 * x5) + (x0 * x7); const FLT x18 = 2 * x17 * (1. / x16); const FLT x19 = x16 * (1. / (x16 + (4 * (x17 * x17)))); const FLT x20 = x7 * (*_x1).Lighthouse.Rot[3]; const FLT x21 = x20 + (-1 * x5 * (*_x1).Lighthouse.Rot[0]); const FLT x22 = -4 * x1; const FLT x23 = x5 * (*_x1).Lighthouse.Rot[3]; const FLT x24 = x7 * (*_x1).Lighthouse.Rot[0]; const FLT x25 = x2 * (*_x1).Lighthouse.Rot[2]; const FLT x26 = x0 * (*_x1).Lighthouse.Rot[1]; const FLT x27 = x26 + x25; const FLT x28 = x0 * (*_x1).Lighthouse.Rot[0]; const FLT x29 = 4 * x28; const FLT x30 = x2 * (*_x1).Lighthouse.Rot[3]; const FLT x31 = x30 + (-1 * x28); const FLT x32 = x5 * (*_x1).Lighthouse.Rot[2]; const FLT x33 = x7 * (*_x1).Lighthouse.Rot[1]; const FLT x34 = (-1 * x33) + x32; const FLT x35 = 4 * x26; const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7)))))); const FLT x37 = x23 + x27 + x24; const FLT x38 = x21 + x13 + (-1 * x15); const FLT x39 = 1 + (-2 * (x10 + (x7 * x7))); const FLT x40 = 2 * (1. / x39); const FLT x41 = x39 * x39; const FLT x42 = (x5 * x7) + (x0 * x2); const FLT x43 = 2 * (1. / x41) * x42; const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + (-1 * x13) + x15)))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24))))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x25) + x35) * x18) + ((x34 + x31) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x36 * (x34 + (-1 * x30) + x28)); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x36 * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x36 * x38); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x44 * ((-1 * (x14 + (-4 * x20)) * x43) + (x40 * x37))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (4 * x8))) + (x40 * (x31 + x33 + (-1 * x32))))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (-4 * x33)) * x43) + ((x9 + (-1 * x1) + (-1 * x3)) * x40))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (4 * x24)) * x43) + (x40 * x38))); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), -1); } // Full version Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveLighthouseKalmanModelAddErrorModel(SurviveLighthouseKalmanModel* out, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x0 * (*_x0).Lighthouse.Rot[0]); const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = 1. / sqrt((x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4)); out->Lighthouse.Pos[0]=(*_x0).Lighthouse.Pos[0] + (*error_state).Lighthouse.Pos[0]; out->Lighthouse.Pos[1]=(*_x0).Lighthouse.Pos[1] + (*error_state).Lighthouse.Pos[1]; out->Lighthouse.Pos[2]=(*_x0).Lighthouse.Pos[2] + (*error_state).Lighthouse.Pos[2]; out->Lighthouse.Rot[0]=x3 * x7; out->Lighthouse.Rot[1]=x6 * x7; out->Lighthouse.Rot[2]=x5 * x7; out->Lighthouse.Rot[3]=x4 * x7; out->BSD0.phase=(*error_state).BSD0.phase + (*_x0).BSD0.phase; out->BSD0.tilt=(*error_state).BSD0.tilt + (*_x0).BSD0.tilt; out->BSD0.curve=(*error_state).BSD0.curve + (*_x0).BSD0.curve; out->BSD0.gibpha=(*error_state).BSD0.gibpha + (*_x0).BSD0.gibpha; out->BSD0.gibmag=(*error_state).BSD0.gibmag + (*_x0).BSD0.gibmag; out->BSD0.ogeephase=(*error_state).BSD0.ogeephase + (*_x0).BSD0.ogeephase; out->BSD0.ogeemag=(*error_state).BSD0.ogeemag + (*_x0).BSD0.ogeemag; out->BSD1.phase=(*error_state).BSD1.phase + (*_x0).BSD1.phase; out->BSD1.tilt=(*error_state).BSD1.tilt + (*_x0).BSD1.tilt; out->BSD1.curve=(*error_state).BSD1.curve + (*_x0).BSD1.curve; out->BSD1.gibpha=(*error_state).BSD1.gibpha + (*_x0).BSD1.gibpha; out->BSD1.gibmag=(*error_state).BSD1.gibmag + (*_x0).BSD1.gibmag; out->BSD1.ogeephase=(*error_state).BSD1.ogeephase + (*_x0).BSD1.ogeephase; out->BSD1.ogeemag=(*error_state).BSD1.ogeemag + (*_x0).BSD1.ogeemag; } // Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveLighthouseKalmanModelAddErrorModel_jac_x0(CnMat* Hx, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x0 * (*_x0).Lighthouse.Rot[0]); const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = (x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4); const FLT x8 = 1. / sqrt(x7); const FLT x9 = 1.0 * x5; const FLT x10 = 1.0 * x6; const FLT x11 = 1.0 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x12 = 1.0/2.0 * (1. / (x7 * sqrt(x7))); const FLT x13 = x12 * ((x4 * x11) + (x10 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x9 * (*error_state).Lighthouse.AxisAngleRot[1]) + (2 * x3)); const FLT x14 = 0.5 * x8; const FLT x15 = x14 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x16 = -1 * x15; const FLT x17 = 1.0 * x3; const FLT x18 = 1.0 * x4; const FLT x19 = (x18 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x17 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x6) + (-1 * x5 * x11); const FLT x20 = x12 * x19; const FLT x21 = x14 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x22 = -1 * x21; const FLT x23 = (-1 * x17 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x6 * x11) + (-1 * x18 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x5); const FLT x24 = x23 * x12; const FLT x25 = x14 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x26 = -1 * x25; const FLT x27 = x12 * ((2 * x4) + (-1 * x10 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x9 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x3 * x11)); const FLT x28 = x6 * x12; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x3 * x13) + x8); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x3 * x20) + x16); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x3 * x24) + x22); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x3 * x27) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x6 * x13) + x15); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x28 * x19) + x8); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x23 * x28) + x25); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x6 * x27) + x22); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x5 * x13) + x21); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x5 * x20) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x5 * x24) + x8); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x5 * x27) + x15); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x4 * x13) + x25); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x4 * x20) + x21); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x4 * x24) + x16); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x4 * x27) + x8); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] // Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveLighthouseKalmanModelAddErrorModel_jac_error_state(CnMat* Hx, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = 0.5 * (*_x0).Lighthouse.Rot[0]; const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x4 * (*error_state).Lighthouse.AxisAngleRot[2]); const FLT x6 = (x4 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x7 = (x4 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[2]); const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5); const FLT x9 = 1. / sqrt(x8); const FLT x10 = 0.5 * x9; const FLT x11 = x10 * (*_x0).Lighthouse.Rot[1]; const FLT x12 = -1 * x11; const FLT x13 = 1.0 * x7; const FLT x14 = 1.0 * x6; const FLT x15 = 1.0 * x3; const FLT x16 = 1.0 * (*_x0).Lighthouse.Rot[2]; const FLT x17 = (-1 * x5 * x16) + (-1 * x15 * (*_x0).Lighthouse.Rot[1]) + (x13 * (*_x0).Lighthouse.Rot[0]) + (x14 * (*_x0).Lighthouse.Rot[3]); const FLT x18 = 1.0/2.0 * (1. / (x8 * sqrt(x8))); const FLT x19 = x3 * x18; const FLT x20 = x10 * (*_x0).Lighthouse.Rot[2]; const FLT x21 = -1 * x20; const FLT x22 = 1.0 * x5; const FLT x23 = (x22 * (*_x0).Lighthouse.Rot[1]) + (x14 * (*_x0).Lighthouse.Rot[0]) + (-1 * x13 * (*_x0).Lighthouse.Rot[3]) + (-1 * x3 * x16); const FLT x24 = x10 * (*_x0).Lighthouse.Rot[3]; const FLT x25 = -1 * x24; const FLT x26 = (x22 * (*_x0).Lighthouse.Rot[0]) + (-1 * x15 * (*_x0).Lighthouse.Rot[3]) + (x7 * x16) + (-1 * x14 * (*_x0).Lighthouse.Rot[1]); const FLT x27 = x4 * x9; const FLT x28 = x7 * x18; const FLT x29 = x18 * x17; const FLT x30 = x23 * x18; const FLT x31 = x26 * x18; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x19 * x17) + x12); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x23 * x19) + x21); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x26 * x19) + x25); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x28 * x17) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x23 * x28) + x25); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x28 * x26) + x20); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x29) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x30) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x31) + x12); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x5 * x29) + x21); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x5 * x30) + x11); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x5 * x31) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelToErrorModel(SurviveJointKalmanErrorModel* out, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) { const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]); const FLT x1 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]); const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]); const FLT x3 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]); const FLT x4 = x0 * x0; const FLT x5 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x6 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x7 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x8 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]); const FLT x9 = x5 * x5; const FLT x10 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x11 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x12 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x13 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x14 = x10 * x10; out->Object.Pose.Pos[0]=(*_x1).Object.Pose.Pos[0] + (-1 * (*_x0).Object.Pose.Pos[0]); out->Object.Pose.Pos[1]=(*_x1).Object.Pose.Pos[1] + (-1 * (*_x0).Object.Pose.Pos[1]); out->Object.Pose.Pos[2]=(*_x1).Object.Pose.Pos[2] + (-1 * (*_x0).Object.Pose.Pos[2]); out->Object.Pose.AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x2 * x2) + x4))); out->Object.Pose.AxisAngleRot[1]=asin(2 * ((x0 * x3) + (-1 * x2 * x1))); out->Object.Pose.AxisAngleRot[2]=atan2(2 * ((x1 * x3) + (x0 * x2)), 1 + (-2 * (x4 + (x1 * x1)))); out->Object.Velocity.Pos[0]=(*_x1).Object.Velocity.Pos[0] + (-1 * (*_x0).Object.Velocity.Pos[0]); out->Object.Velocity.Pos[1]=(*_x1).Object.Velocity.Pos[1] + (-1 * (*_x0).Object.Velocity.Pos[1]); out->Object.Velocity.Pos[2]=(*_x1).Object.Velocity.Pos[2] + (-1 * (*_x0).Object.Velocity.Pos[2]); out->Object.Velocity.AxisAngleRot[0]=(*_x1).Object.Velocity.AxisAngleRot[0] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[0]); out->Object.Velocity.AxisAngleRot[1]=(*_x1).Object.Velocity.AxisAngleRot[1] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[1]); out->Object.Velocity.AxisAngleRot[2]=(*_x1).Object.Velocity.AxisAngleRot[2] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[2]); out->Object.Acc[0]=(*_x1).Object.Acc[0] + (-1 * (*_x0).Object.Acc[0]); out->Object.Acc[1]=(*_x1).Object.Acc[1] + (-1 * (*_x0).Object.Acc[1]); out->Object.Acc[2]=(*_x1).Object.Acc[2] + (-1 * (*_x0).Object.Acc[2]); out->Object.IMUBias.AccScale[0]=(*_x1).Object.IMUBias.AccScale[0] + (-1 * (*_x0).Object.IMUBias.AccScale[0]); out->Object.IMUBias.AccScale[1]=(*_x1).Object.IMUBias.AccScale[1] + (-1 * (*_x0).Object.IMUBias.AccScale[1]); out->Object.IMUBias.AccScale[2]=(*_x1).Object.IMUBias.AccScale[2] + (-1 * (*_x0).Object.IMUBias.AccScale[2]); out->Object.IMUBias.IMUCorrection[0]=atan2(2 * ((x8 * x7) + (x6 * x5)), 1 + (-2 * ((x7 * x7) + x9))); out->Object.IMUBias.IMUCorrection[1]=asin(2 * ((x5 * x8) + (-1 * x6 * x7))); out->Object.IMUBias.IMUCorrection[2]=atan2(2 * ((x6 * x8) + (x5 * x7)), 1 + (-2 * (x9 + (x6 * x6)))); out->Object.IMUBias.AccBias[0]=(*_x1).Object.IMUBias.AccBias[0] + (-1 * (*_x0).Object.IMUBias.AccBias[0]); out->Object.IMUBias.AccBias[1]=(*_x1).Object.IMUBias.AccBias[1] + (-1 * (*_x0).Object.IMUBias.AccBias[1]); out->Object.IMUBias.AccBias[2]=(*_x1).Object.IMUBias.AccBias[2] + (-1 * (*_x0).Object.IMUBias.AccBias[2]); out->Object.IMUBias.GyroBias[0]=(*_x1).Object.IMUBias.GyroBias[0] + (-1 * (*_x0).Object.IMUBias.GyroBias[0]); out->Object.IMUBias.GyroBias[1]=(*_x1).Object.IMUBias.GyroBias[1] + (-1 * (*_x0).Object.IMUBias.GyroBias[1]); out->Object.IMUBias.GyroBias[2]=(*_x1).Object.IMUBias.GyroBias[2] + (-1 * (*_x0).Object.IMUBias.GyroBias[2]); out->Lighthouse.Pos[0]=(*_x1).Lighthouse.Pos[0] + (-1 * (*_x0).Lighthouse.Pos[0]); out->Lighthouse.Pos[1]=(*_x1).Lighthouse.Pos[1] + (-1 * (*_x0).Lighthouse.Pos[1]); out->Lighthouse.Pos[2]=(*_x1).Lighthouse.Pos[2] + (-1 * (*_x0).Lighthouse.Pos[2]); out->Lighthouse.AxisAngleRot[0]=atan2(2 * ((x13 * x12) + (x11 * x10)), 1 + (-2 * ((x13 * x13) + x14))); out->Lighthouse.AxisAngleRot[1]=asin(2 * ((x12 * x10) + (-1 * x13 * x11))); out->Lighthouse.AxisAngleRot[2]=atan2(2 * ((x12 * x11) + (x13 * x10)), 1 + (-2 * (x14 + (x11 * x11)))); out->BSD0.phase=(*_x1).BSD0.phase + (-1 * (*_x0).BSD0.phase); out->BSD0.tilt=(*_x1).BSD0.tilt + (-1 * (*_x0).BSD0.tilt); out->BSD0.curve=(*_x1).BSD0.curve + (-1 * (*_x0).BSD0.curve); out->BSD0.gibpha=(*_x1).BSD0.gibpha + (-1 * (*_x0).BSD0.gibpha); out->BSD0.gibmag=(*_x1).BSD0.gibmag + (-1 * (*_x0).BSD0.gibmag); out->BSD0.ogeephase=(*_x1).BSD0.ogeephase + (-1 * (*_x0).BSD0.ogeephase); out->BSD0.ogeemag=(*_x1).BSD0.ogeemag + (-1 * (*_x0).BSD0.ogeemag); out->BSD1.phase=(*_x1).BSD1.phase + (-1 * (*_x0).BSD1.phase); out->BSD1.tilt=(*_x1).BSD1.tilt + (-1 * (*_x0).BSD1.tilt); out->BSD1.curve=(*_x1).BSD1.curve + (-1 * (*_x0).BSD1.curve); out->BSD1.gibpha=(*_x1).BSD1.gibpha + (-1 * (*_x0).BSD1.gibpha); out->BSD1.gibmag=(*_x1).BSD1.gibmag + (-1 * (*_x0).BSD1.gibmag); out->BSD1.ogeephase=(*_x1).BSD1.ogeephase + (-1 * (*_x0).BSD1.ogeephase); out->BSD1.ogeemag=(*_x1).BSD1.ogeemag + (-1 * (*_x0).BSD1.ogeemag); } // Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], (*_x1).Object.Acc[0], (*_x1).Object.Acc[1], (*_x1).Object.Acc[2], (*_x1).Object.IMUBias.AccBias[0], (*_x1).Object.IMUBias.AccBias[1], (*_x1).Object.IMUBias.AccBias[2], (*_x1).Object.IMUBias.AccScale[0], (*_x1).Object.IMUBias.AccScale[1], (*_x1).Object.IMUBias.AccScale[2], (*_x1).Object.IMUBias.GyroBias[0], (*_x1).Object.IMUBias.GyroBias[1], (*_x1).Object.IMUBias.GyroBias[2], (*_x1).Object.IMUBias.IMUCorrection[0], (*_x1).Object.IMUBias.IMUCorrection[1], (*_x1).Object.IMUBias.IMUCorrection[2], (*_x1).Object.IMUBias.IMUCorrection[3], (*_x1).Object.Pose.Pos[0], (*_x1).Object.Pose.Pos[1], (*_x1).Object.Pose.Pos[2], (*_x1).Object.Pose.Rot[0], (*_x1).Object.Pose.Rot[1], (*_x1).Object.Pose.Rot[2], (*_x1).Object.Pose.Rot[3], (*_x1).Object.Velocity.AxisAngleRot[0], (*_x1).Object.Velocity.AxisAngleRot[1], (*_x1).Object.Velocity.AxisAngleRot[2], (*_x1).Object.Velocity.Pos[0], (*_x1).Object.Velocity.Pos[1], (*_x1).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelToErrorModel_jac_x1(CnMat* Hx, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) { const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]); const FLT x1 = x0 * (*_x0).Object.Pose.Rot[3]; const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]); const FLT x3 = x2 * (*_x0).Object.Pose.Rot[2]; const FLT x4 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]); const FLT x5 = x4 * (*_x0).Object.Pose.Rot[1]; const FLT x6 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]); const FLT x7 = x6 * (*_x0).Object.Pose.Rot[0]; const FLT x8 = x7 + (-1 * x5); const FLT x9 = x0 * x0; const FLT x10 = 1 + (-2 * ((x6 * x6) + x9)); const FLT x11 = 2 * (1. / x10); const FLT x12 = x0 * (*_x0).Object.Pose.Rot[2]; const FLT x13 = 4 * x12; const FLT x14 = x6 * (*_x0).Object.Pose.Rot[1]; const FLT x15 = x10 * x10; const FLT x16 = (x4 * x6) + (x0 * x2); const FLT x17 = 2 * (1. / x15) * x16; const FLT x18 = x15 * (1. / (x15 + (4 * (x16 * x16)))); const FLT x19 = x2 * (*_x0).Object.Pose.Rot[3]; const FLT x20 = x14 + (x4 * (*_x0).Object.Pose.Rot[0]); const FLT x21 = x20 + x12 + (-1 * x19); const FLT x22 = 4 * x1; const FLT x23 = x4 * (*_x0).Object.Pose.Rot[3]; const FLT x24 = x6 * (*_x0).Object.Pose.Rot[2]; const FLT x25 = x0 * (*_x0).Object.Pose.Rot[1]; const FLT x26 = x2 * (*_x0).Object.Pose.Rot[0]; const FLT x27 = x26 + (-1 * x25); const FLT x28 = x0 * (*_x0).Object.Pose.Rot[0]; const FLT x29 = -4 * x28; const FLT x30 = x6 * (*_x0).Object.Pose.Rot[3]; const FLT x31 = x4 * (*_x0).Object.Pose.Rot[2]; const FLT x32 = x2 * (*_x0).Object.Pose.Rot[1]; const FLT x33 = x28 + x32; const FLT x34 = (-1 * x31) + x33 + x30; const FLT x35 = -4 * x25; const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x4) + (-1 * x2 * x6)) * ((x0 * x4) + (-1 * x2 * x6)))))); const FLT x37 = (-1 * x24) + (-1 * x23); const FLT x38 = x1 + x3; const FLT x39 = 1 + (-2 * (x9 + (x2 * x2))); const FLT x40 = 2 * (1. / x39); const FLT x41 = x39 * x39; const FLT x42 = (x2 * x4) + (x0 * x6); const FLT x43 = 2 * (1. / x41) * x42; const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42)))); const FLT x45 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x46 = x45 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x47 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x48 = x47 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x49 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]); const FLT x50 = x49 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x51 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x52 = x51 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x53 = x52 + (-1 * x50); const FLT x54 = x45 * x45; const FLT x55 = 1 + (-2 * ((x51 * x51) + x54)); const FLT x56 = 2 * (1. / x55); const FLT x57 = x45 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x58 = 4 * x57; const FLT x59 = x51 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x60 = x55 * x55; const FLT x61 = (x51 * x49) + (x45 * x47); const FLT x62 = 2 * (1. / x60) * x61; const FLT x63 = x60 * (1. / (x60 + (4 * (x61 * x61)))); const FLT x64 = x47 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x65 = x59 + (x49 * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x66 = x65 + x57 + (-1 * x64); const FLT x67 = 4 * x46; const FLT x68 = x51 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x69 = x49 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x70 = x45 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x71 = x47 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x72 = x71 + (-1 * x70); const FLT x73 = x45 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x74 = -4 * x73; const FLT x75 = x51 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x76 = x49 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x77 = x47 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x78 = x77 + x73; const FLT x79 = x78 + x75 + (-1 * x76); const FLT x80 = -4 * x70; const FLT x81 = 2 * (1. / sqrt(1 + (-4 * (((x45 * x49) + (-1 * x51 * x47)) * ((x45 * x49) + (-1 * x51 * x47)))))); const FLT x82 = (-1 * x69) + (-1 * x68); const FLT x83 = x46 + x48; const FLT x84 = 1 + (-2 * (x54 + (x47 * x47))); const FLT x85 = 2 * (1. / x84); const FLT x86 = x84 * x84; const FLT x87 = (x47 * x49) + (x51 * x45); const FLT x88 = 2 * (1. / x86) * x87; const FLT x89 = x86 * (1. / (x86 + (4 * (x87 * x87)))); const FLT x90 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x91 = x90 * (*_x0).Lighthouse.Rot[3]; const FLT x92 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x93 = x92 * (*_x0).Lighthouse.Rot[2]; const FLT x94 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x95 = x94 * (*_x0).Lighthouse.Rot[0]; const FLT x96 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x97 = x96 * (*_x0).Lighthouse.Rot[1]; const FLT x98 = (-1 * x97) + x95; const FLT x99 = x90 * x90; const FLT x100 = 1 + (-2 * ((x94 * x94) + x99)); const FLT x101 = 2 * (1. / x100); const FLT x102 = x90 * (*_x0).Lighthouse.Rot[2]; const FLT x103 = 4 * x102; const FLT x104 = x94 * (*_x0).Lighthouse.Rot[1]; const FLT x105 = x100 * x100; const FLT x106 = (x96 * x94) + (x92 * x90); const FLT x107 = 2 * (1. / x105) * x106; const FLT x108 = x105 * (1. / (x105 + (4 * (x106 * x106)))); const FLT x109 = x92 * (*_x0).Lighthouse.Rot[3]; const FLT x110 = x104 + (x96 * (*_x0).Lighthouse.Rot[0]); const FLT x111 = x102 + x110 + (-1 * x109); const FLT x112 = 4 * x91; const FLT x113 = x94 * (*_x0).Lighthouse.Rot[2]; const FLT x114 = x96 * (*_x0).Lighthouse.Rot[3]; const FLT x115 = x90 * (*_x0).Lighthouse.Rot[1]; const FLT x116 = x92 * (*_x0).Lighthouse.Rot[0]; const FLT x117 = x116 + (-1 * x115); const FLT x118 = x90 * (*_x0).Lighthouse.Rot[0]; const FLT x119 = -4 * x118; const FLT x120 = x94 * (*_x0).Lighthouse.Rot[3]; const FLT x121 = x96 * (*_x0).Lighthouse.Rot[2]; const FLT x122 = x92 * (*_x0).Lighthouse.Rot[1]; const FLT x123 = x118 + x122; const FLT x124 = x123 + x120 + (-1 * x121); const FLT x125 = -4 * x115; const FLT x126 = 2 * (1. / sqrt(1 + (-4 * (((x90 * x96) + (-1 * x92 * x94)) * ((x90 * x96) + (-1 * x92 * x94)))))); const FLT x127 = (-1 * x113) + (-1 * x114); const FLT x128 = x91 + x93; const FLT x129 = 1 + (-2 * (x99 + (x92 * x92))); const FLT x130 = 2 * (1. / x129); const FLT x131 = x129 * x129; const FLT x132 = (x92 * x96) + (x90 * x94); const FLT x133 = 2 * (1. / x131) * x132; const FLT x134 = x131 * (1. / (x131 + (4 * (x132 * x132)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x7) + x22)) + (x21 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x24) + x35) * x17) + (x34 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x34 * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x36 * (x37 + x25 + (-1 * x26))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x36 * x21); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x36 * (x38 + x5 + (-1 * x7))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x26)) * x43) + (x40 * (x20 + x19 + (-1 * x12))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x63 * ((-1 * ((4 * x59) + x58) * x62) + (x56 * (x53 + (-1 * x46) + (-1 * x48))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x63 * ((-1 * ((-4 * x52) + x67) * x62) + (x66 * x56))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x63 * ((-1 * ((-4 * x75) + x74) * x62) + (x56 * (x72 + x68 + x69)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x63 * ((-1 * ((4 * x68) + x80) * x62) + (x79 * x56))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * x79); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * (x82 + x70 + (-1 * x71))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x66); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x50 + x83 + (-1 * x52))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x58 + (4 * x64)) * x88) + ((x72 + x82) * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (-4 * x48)) * x88) + (x85 * (x78 + x76 + (-1 * x75))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (4 * x77)) * x88) + ((x83 + x53) * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (-4 * x71)) * x88) + (x85 * (x65 + x64 + (-1 * x57))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x108 * ((-1 * ((4 * x104) + x103) * x107) + ((x98 + (-1 * x91) + (-1 * x93)) * x101))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x108 * ((-1 * x107 * ((-4 * x95) + x112)) + (x101 * x111))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x108 * ((-1 * ((-4 * x120) + x119) * x107) + (x101 * (x117 + x113 + x114)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x108 * ((-1 * ((4 * x113) + x125) * x107) + (x101 * x124))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x124 * x126); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x126 * (x127 + x115 + (-1 * x116))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x111 * x126); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x126 * (x128 + x97 + (-1 * x95))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * (x103 + (4 * x109)) * x133) + ((x117 + x127) * x130)) * x134); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x134 * ((-1 * x133 * (x112 + (-4 * x93))) + (x130 * (x121 + x123 + (-1 * x120))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x134 * ((-1 * (x119 + (4 * x122)) * x133) + (x130 * (x128 + x98)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x134 * ((-1 * (x125 + (-4 * x116)) * x133) + (x130 * ((-1 * x102) + x110 + x109)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], (*_x1).Object.Acc[0], (*_x1).Object.Acc[1], (*_x1).Object.Acc[2], (*_x1).Object.IMUBias.AccBias[0], (*_x1).Object.IMUBias.AccBias[1], (*_x1).Object.IMUBias.AccBias[2], (*_x1).Object.IMUBias.AccScale[0], (*_x1).Object.IMUBias.AccScale[1], (*_x1).Object.IMUBias.AccScale[2], (*_x1).Object.IMUBias.GyroBias[0], (*_x1).Object.IMUBias.GyroBias[1], (*_x1).Object.IMUBias.GyroBias[2], (*_x1).Object.IMUBias.IMUCorrection[0], (*_x1).Object.IMUBias.IMUCorrection[1], (*_x1).Object.IMUBias.IMUCorrection[2], (*_x1).Object.IMUBias.IMUCorrection[3], (*_x1).Object.Pose.Pos[0], (*_x1).Object.Pose.Pos[1], (*_x1).Object.Pose.Pos[2], (*_x1).Object.Pose.Rot[0], (*_x1).Object.Pose.Rot[1], (*_x1).Object.Pose.Rot[2], (*_x1).Object.Pose.Rot[3], (*_x1).Object.Velocity.AxisAngleRot[0], (*_x1).Object.Velocity.AxisAngleRot[1], (*_x1).Object.Velocity.AxisAngleRot[2], (*_x1).Object.Velocity.Pos[0], (*_x1).Object.Velocity.Pos[1], (*_x1).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] // Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelToErrorModel_jac_x0(CnMat* Hx, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) { const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]); const FLT x1 = x0 * (*_x1).Object.Pose.Rot[3]; const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]); const FLT x3 = x2 * (*_x1).Object.Pose.Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]); const FLT x6 = x5 * (*_x1).Object.Pose.Rot[1]; const FLT x7 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]); const FLT x8 = x7 * (*_x1).Object.Pose.Rot[2]; const FLT x9 = x8 + x6; const FLT x10 = x0 * x0; const FLT x11 = 1 + (-2 * ((x2 * x2) + x10)); const FLT x12 = 2 * (1. / x11); const FLT x13 = x0 * (*_x1).Object.Pose.Rot[2]; const FLT x14 = -4 * x13; const FLT x15 = x2 * (*_x1).Object.Pose.Rot[1]; const FLT x16 = x11 * x11; const FLT x17 = (x2 * x5) + (x0 * x7); const FLT x18 = 2 * x17 * (1. / x16); const FLT x19 = x16 * (1. / (x16 + (4 * (x17 * x17)))); const FLT x20 = x7 * (*_x1).Object.Pose.Rot[3]; const FLT x21 = x20 + (-1 * x5 * (*_x1).Object.Pose.Rot[0]); const FLT x22 = -4 * x1; const FLT x23 = x5 * (*_x1).Object.Pose.Rot[3]; const FLT x24 = x7 * (*_x1).Object.Pose.Rot[0]; const FLT x25 = x2 * (*_x1).Object.Pose.Rot[2]; const FLT x26 = x0 * (*_x1).Object.Pose.Rot[1]; const FLT x27 = x26 + x25; const FLT x28 = x0 * (*_x1).Object.Pose.Rot[0]; const FLT x29 = 4 * x28; const FLT x30 = x2 * (*_x1).Object.Pose.Rot[3]; const FLT x31 = x5 * (*_x1).Object.Pose.Rot[2]; const FLT x32 = x7 * (*_x1).Object.Pose.Rot[1]; const FLT x33 = (-1 * x32) + x31; const FLT x34 = (-1 * x28) + x30; const FLT x35 = 4 * x26; const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7)))))); const FLT x37 = x23 + x27 + x24; const FLT x38 = x21 + x13 + (-1 * x15); const FLT x39 = 1 + (-2 * (x10 + (x7 * x7))); const FLT x40 = 2 * (1. / x39); const FLT x41 = x39 * x39; const FLT x42 = (x5 * x7) + (x0 * x2); const FLT x43 = 2 * (1. / x41) * x42; const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42)))); const FLT x45 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x46 = x45 * (*_x1).Object.IMUBias.IMUCorrection[2]; const FLT x47 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]); const FLT x48 = x47 * (*_x1).Object.IMUBias.IMUCorrection[1]; const FLT x49 = x48 + x46; const FLT x50 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x51 = x50 * (*_x1).Object.IMUBias.IMUCorrection[3]; const FLT x52 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x53 = x52 * (*_x1).Object.IMUBias.IMUCorrection[0]; const FLT x54 = x53 + x51; const FLT x55 = x50 * x50; const FLT x56 = 1 + (-2 * ((x52 * x52) + x55)); const FLT x57 = 2 * (1. / x56); const FLT x58 = x50 * (*_x1).Object.IMUBias.IMUCorrection[2]; const FLT x59 = -4 * x58; const FLT x60 = x52 * (*_x1).Object.IMUBias.IMUCorrection[1]; const FLT x61 = x56 * x56; const FLT x62 = (x52 * x47) + (x50 * x45); const FLT x63 = 2 * (1. / x61) * x62; const FLT x64 = x61 * (1. / (x61 + (4 * (x62 * x62)))); const FLT x65 = x45 * (*_x1).Object.IMUBias.IMUCorrection[3]; const FLT x66 = x65 + (-1 * x47 * (*_x1).Object.IMUBias.IMUCorrection[0]); const FLT x67 = -4 * x51; const FLT x68 = x47 * (*_x1).Object.IMUBias.IMUCorrection[3]; const FLT x69 = x45 * (*_x1).Object.IMUBias.IMUCorrection[0]; const FLT x70 = x52 * (*_x1).Object.IMUBias.IMUCorrection[2]; const FLT x71 = x50 * (*_x1).Object.IMUBias.IMUCorrection[1]; const FLT x72 = x71 + x70; const FLT x73 = x50 * (*_x1).Object.IMUBias.IMUCorrection[0]; const FLT x74 = 4 * x73; const FLT x75 = x52 * (*_x1).Object.IMUBias.IMUCorrection[3]; const FLT x76 = x75 + (-1 * x73); const FLT x77 = x47 * (*_x1).Object.IMUBias.IMUCorrection[2]; const FLT x78 = x45 * (*_x1).Object.IMUBias.IMUCorrection[1]; const FLT x79 = (-1 * x78) + x77; const FLT x80 = 4 * x71; const FLT x81 = 2 * (1. / sqrt(1 + (-4 * (((x50 * x47) + (-1 * x52 * x45)) * ((x50 * x47) + (-1 * x52 * x45)))))); const FLT x82 = x72 + x68 + x69; const FLT x83 = x66 + x58 + (-1 * x60); const FLT x84 = 1 + (-2 * (x55 + (x45 * x45))); const FLT x85 = 2 * (1. / x84); const FLT x86 = x84 * x84; const FLT x87 = (x45 * x47) + (x50 * x52); const FLT x88 = 2 * (1. / x86) * x87; const FLT x89 = x86 * (1. / (x86 + (4 * (x87 * x87)))); const FLT x90 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]); const FLT x91 = x90 * (*_x1).Lighthouse.Rot[3]; const FLT x92 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]); const FLT x93 = x92 * (*_x1).Lighthouse.Rot[0]; const FLT x94 = x93 + x91; const FLT x95 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]); const FLT x96 = x95 * (*_x1).Lighthouse.Rot[2]; const FLT x97 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]); const FLT x98 = x97 * (*_x1).Lighthouse.Rot[1]; const FLT x99 = x98 + x96; const FLT x100 = x90 * x90; const FLT x101 = 1 + (-2 * ((x92 * x92) + x100)); const FLT x102 = 2 * (1. / x101); const FLT x103 = x90 * (*_x1).Lighthouse.Rot[2]; const FLT x104 = -4 * x103; const FLT x105 = x92 * (*_x1).Lighthouse.Rot[1]; const FLT x106 = x101 * x101; const FLT x107 = (x92 * x97) + (x90 * x95); const FLT x108 = 2 * x107 * (1. / x106); const FLT x109 = x106 * (1. / (x106 + (4 * (x107 * x107)))); const FLT x110 = x95 * (*_x1).Lighthouse.Rot[3]; const FLT x111 = x110 + (-1 * x97 * (*_x1).Lighthouse.Rot[0]); const FLT x112 = -4 * x91; const FLT x113 = x97 * (*_x1).Lighthouse.Rot[3]; const FLT x114 = x95 * (*_x1).Lighthouse.Rot[0]; const FLT x115 = x92 * (*_x1).Lighthouse.Rot[2]; const FLT x116 = x90 * (*_x1).Lighthouse.Rot[1]; const FLT x117 = x116 + x115; const FLT x118 = x90 * (*_x1).Lighthouse.Rot[0]; const FLT x119 = 4 * x118; const FLT x120 = x92 * (*_x1).Lighthouse.Rot[3]; const FLT x121 = x97 * (*_x1).Lighthouse.Rot[2]; const FLT x122 = x95 * (*_x1).Lighthouse.Rot[1]; const FLT x123 = (-1 * x122) + x121; const FLT x124 = (-1 * x118) + x120; const FLT x125 = 4 * x116; const FLT x126 = 2 * (1. / sqrt(1 + (-4 * (((x90 * x97) + (-1 * x92 * x95)) * ((x90 * x97) + (-1 * x92 * x95)))))); const FLT x127 = x117 + x113 + x114; const FLT x128 = x111 + x103 + (-1 * x105); const FLT x129 = 1 + (-2 * (x100 + (x95 * x95))); const FLT x130 = 2 * (1. / x129); const FLT x131 = x129 * x129; const FLT x132 = (x97 * x95) + (x92 * x90); const FLT x133 = 2 * (1. / x131) * x132; const FLT x134 = x131 * (1. / (x131 + (4 * (x132 * x132)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + x15 + (-1 * x13))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x25) + x35) * x18) + ((x34 + x33) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x36 * (x28 + x33 + (-1 * x30))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x36 * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x36 * x38); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x44 * ((-1 * (x14 + (-4 * x20)) * x43) + (x40 * x37))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (4 * x8))) + (x40 * (x34 + x32 + (-1 * x31))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (-4 * x32)) * x43) + ((x9 + (-1 * x3) + (-1 * x1)) * x40))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (4 * x24)) * x43) + (x40 * x38))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x64 * ((-1 * ((-4 * x60) + x59) * x63) + ((x54 + x49) * x57))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x64 * ((-1 * ((4 * x53) + x67) * x63) + (x57 * (x66 + x60 + (-1 * x58))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x64 * ((-1 * ((4 * x75) + x74) * x63) + (x57 * (x72 + (-1 * x68) + (-1 * x69))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x64 * ((-1 * ((-4 * x70) + x80) * x63) + ((x79 + x76) * x57))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * (x79 + x73 + (-1 * x75))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * x82); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x83); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x54 + (-1 * x48) + (-1 * x46))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x59 + (-4 * x65)) * x88) + (x82 * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (4 * x46)) * x88) + (x85 * (x76 + x78 + (-1 * x77))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (-4 * x78)) * x88) + (x85 * (x49 + (-1 * x51) + (-1 * x53))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (4 * x69)) * x88) + (x83 * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x105) + x104) * x108) + ((x99 + x94) * x102)) * x109); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x109 * ((-1 * x108 * ((4 * x93) + x112)) + (x102 * (x111 + (-1 * x103) + x105)))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x109 * ((-1 * ((4 * x120) + x119) * x108) + (x102 * (x117 + (-1 * x113) + (-1 * x114))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x115) + x125) * x108) + ((x124 + x123) * x102)) * x109); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x126 * (x123 + (-1 * x120) + x118)); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x127 * x126); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x128 * x126); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (x94 + (-1 * x98) + (-1 * x96)) * x126); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x134 * ((-1 * (x104 + (-4 * x110)) * x133) + (x127 * x130))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x134 * ((-1 * x133 * (x112 + (4 * x96))) + (x130 * (x124 + x122 + (-1 * x121))))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x134 * ((-1 * (x119 + (-4 * x122)) * x133) + ((x99 + (-1 * x91) + (-1 * x93)) * x130))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x134 * ((-1 * (x125 + (4 * x114)) * x133) + (x128 * x130))); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), -1); } // Full version Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelAddErrorModel(SurviveJointKalmanModel* out, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x2 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*error_state).Object.Pose.AxisAngleRot[0]); const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[0]; const FLT x7 = (x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[3] + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[0]); const FLT x8 = (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[1] + (x4 * (*error_state).Object.Pose.AxisAngleRot[2]); const FLT x9 = 1. / sqrt((x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6)); const FLT x10 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x11 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[0]; const FLT x12 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x13 = (x12 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x11 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x10 * (*error_state).Object.IMUBias.IMUCorrection[2]); const FLT x14 = (-1 * x12 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (x11 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x10 * (*error_state).Object.IMUBias.IMUCorrection[1]); const FLT x15 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x16 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x17 = (-1 * x11 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x15 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x16 * (*error_state).Object.IMUBias.IMUCorrection[1]); const FLT x18 = (*_x0).Object.IMUBias.IMUCorrection[1] + (x11 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x15 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (x16 * (*error_state).Object.IMUBias.IMUCorrection[2]); const FLT x19 = 1. / sqrt((x18 * x18) + (x17 * x17) + (x13 * x13) + (x14 * x14)); const FLT x20 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x21 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x22 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x23 = (-1 * x20 * (*error_state).Lighthouse.AxisAngleRot[2]) + (-1 * x22 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x21 * (*_x0).Lighthouse.Rot[2]); const FLT x24 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x25 = (*_x0).Lighthouse.Rot[3] + (-1 * x22 * (*_x0).Lighthouse.Rot[2]) + (x21 * (*_x0).Lighthouse.Rot[1]) + (x24 * (*_x0).Lighthouse.Rot[0]); const FLT x26 = (x21 * (*_x0).Lighthouse.Rot[0]) + (-1 * x24 * (*_x0).Lighthouse.Rot[1]) + (x20 * (*error_state).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x27 = (*_x0).Lighthouse.Rot[1] + (x22 * (*_x0).Lighthouse.Rot[0]) + (-1 * x20 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x24 * (*_x0).Lighthouse.Rot[2]); const FLT x28 = 1. / sqrt((x27 * x27) + (x26 * x26) + (x23 * x23) + (x25 * x25)); out->Object.Pose.Pos[0]=(*_x0).Object.Pose.Pos[0] + (*error_state).Object.Pose.Pos[0]; out->Object.Pose.Pos[1]=(*_x0).Object.Pose.Pos[1] + (*error_state).Object.Pose.Pos[1]; out->Object.Pose.Pos[2]=(*_x0).Object.Pose.Pos[2] + (*error_state).Object.Pose.Pos[2]; out->Object.Pose.Rot[0]=x6 * x9; out->Object.Pose.Rot[1]=x8 * x9; out->Object.Pose.Rot[2]=x3 * x9; out->Object.Pose.Rot[3]=x7 * x9; out->Object.Velocity.Pos[0]=(*_x0).Object.Velocity.Pos[0] + (*error_state).Object.Velocity.Pos[0]; out->Object.Velocity.Pos[1]=(*_x0).Object.Velocity.Pos[1] + (*error_state).Object.Velocity.Pos[1]; out->Object.Velocity.Pos[2]=(*_x0).Object.Velocity.Pos[2] + (*error_state).Object.Velocity.Pos[2]; out->Object.Velocity.AxisAngleRot[0]=(*_x0).Object.Velocity.AxisAngleRot[0] + (*error_state).Object.Velocity.AxisAngleRot[0]; out->Object.Velocity.AxisAngleRot[1]=(*_x0).Object.Velocity.AxisAngleRot[1] + (*error_state).Object.Velocity.AxisAngleRot[1]; out->Object.Velocity.AxisAngleRot[2]=(*_x0).Object.Velocity.AxisAngleRot[2] + (*error_state).Object.Velocity.AxisAngleRot[2]; out->Object.Acc[0]=(*_x0).Object.Acc[0] + (*error_state).Object.Acc[0]; out->Object.Acc[1]=(*_x0).Object.Acc[1] + (*error_state).Object.Acc[1]; out->Object.Acc[2]=(*_x0).Object.Acc[2] + (*error_state).Object.Acc[2]; out->Object.IMUBias.AccScale[0]=(*_x0).Object.IMUBias.AccScale[0] + (*error_state).Object.IMUBias.AccScale[0]; out->Object.IMUBias.AccScale[1]=(*_x0).Object.IMUBias.AccScale[1] + (*error_state).Object.IMUBias.AccScale[1]; out->Object.IMUBias.AccScale[2]=(*_x0).Object.IMUBias.AccScale[2] + (*error_state).Object.IMUBias.AccScale[2]; out->Object.IMUBias.IMUCorrection[0]=x19 * x17; out->Object.IMUBias.IMUCorrection[1]=x19 * x18; out->Object.IMUBias.IMUCorrection[2]=x14 * x19; out->Object.IMUBias.IMUCorrection[3]=x13 * x19; out->Object.IMUBias.AccBias[0]=(*_x0).Object.IMUBias.AccBias[0] + (*error_state).Object.IMUBias.AccBias[0]; out->Object.IMUBias.AccBias[1]=(*_x0).Object.IMUBias.AccBias[1] + (*error_state).Object.IMUBias.AccBias[1]; out->Object.IMUBias.AccBias[2]=(*_x0).Object.IMUBias.AccBias[2] + (*error_state).Object.IMUBias.AccBias[2]; out->Object.IMUBias.GyroBias[0]=(*_x0).Object.IMUBias.GyroBias[0] + (*error_state).Object.IMUBias.GyroBias[0]; out->Object.IMUBias.GyroBias[1]=(*_x0).Object.IMUBias.GyroBias[1] + (*error_state).Object.IMUBias.GyroBias[1]; out->Object.IMUBias.GyroBias[2]=(*_x0).Object.IMUBias.GyroBias[2] + (*error_state).Object.IMUBias.GyroBias[2]; out->Lighthouse.Pos[0]=(*_x0).Lighthouse.Pos[0] + (*error_state).Lighthouse.Pos[0]; out->Lighthouse.Pos[1]=(*_x0).Lighthouse.Pos[1] + (*error_state).Lighthouse.Pos[1]; out->Lighthouse.Pos[2]=(*_x0).Lighthouse.Pos[2] + (*error_state).Lighthouse.Pos[2]; out->Lighthouse.Rot[0]=x23 * x28; out->Lighthouse.Rot[1]=x28 * x27; out->Lighthouse.Rot[2]=x28 * x26; out->Lighthouse.Rot[3]=x25 * x28; out->BSD0.phase=(*error_state).BSD0.phase + (*_x0).BSD0.phase; out->BSD0.tilt=(*error_state).BSD0.tilt + (*_x0).BSD0.tilt; out->BSD0.curve=(*error_state).BSD0.curve + (*_x0).BSD0.curve; out->BSD0.gibpha=(*error_state).BSD0.gibpha + (*_x0).BSD0.gibpha; out->BSD0.gibmag=(*error_state).BSD0.gibmag + (*_x0).BSD0.gibmag; out->BSD0.ogeephase=(*error_state).BSD0.ogeephase + (*_x0).BSD0.ogeephase; out->BSD0.ogeemag=(*error_state).BSD0.ogeemag + (*_x0).BSD0.ogeemag; out->BSD1.phase=(*error_state).BSD1.phase + (*_x0).BSD1.phase; out->BSD1.tilt=(*error_state).BSD1.tilt + (*_x0).BSD1.tilt; out->BSD1.curve=(*error_state).BSD1.curve + (*_x0).BSD1.curve; out->BSD1.gibpha=(*error_state).BSD1.gibpha + (*_x0).BSD1.gibpha; out->BSD1.gibmag=(*error_state).BSD1.gibmag + (*_x0).BSD1.gibmag; out->BSD1.ogeephase=(*error_state).BSD1.ogeephase + (*_x0).BSD1.ogeephase; out->BSD1.ogeemag=(*error_state).BSD1.ogeemag + (*_x0).BSD1.ogeemag; } // Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelAddErrorModel_jac_x0(CnMat* Hx, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x2 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*error_state).Object.Pose.AxisAngleRot[0]); const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x2 * (*_x0).Object.Pose.Rot[3]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[0]; const FLT x7 = (x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[3] + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[0]); const FLT x8 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[2]); const FLT x9 = (x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6); const FLT x10 = 1. / sqrt(x9); const FLT x11 = 1.0 * x8; const FLT x12 = 1.0 * x3; const FLT x13 = 1.0 * x7; const FLT x14 = (x13 * (*error_state).Object.Pose.AxisAngleRot[2]) + (2 * x6) + (x11 * (*error_state).Object.Pose.AxisAngleRot[0]) + (x12 * (*error_state).Object.Pose.AxisAngleRot[1]); const FLT x15 = 1.0/2.0 * (1. / (x9 * sqrt(x9))); const FLT x16 = x15 * x14; const FLT x17 = 0.5 * x10; const FLT x18 = x17 * (*error_state).Object.Pose.AxisAngleRot[0]; const FLT x19 = -1 * x18; const FLT x20 = 1.0 * x6; const FLT x21 = (2 * x8) + (x13 * (*error_state).Object.Pose.AxisAngleRot[1]) + (-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x12 * (*error_state).Object.Pose.AxisAngleRot[2]); const FLT x22 = x21 * x15; const FLT x23 = x17 * (*error_state).Object.Pose.AxisAngleRot[1]; const FLT x24 = -1 * x23; const FLT x25 = x15 * ((-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[1]) + (2 * x3) + (x11 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x13 * (*error_state).Object.Pose.AxisAngleRot[0])); const FLT x26 = x17 * (*error_state).Object.Pose.AxisAngleRot[2]; const FLT x27 = -1 * x26; const FLT x28 = (-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x11 * (*error_state).Object.Pose.AxisAngleRot[1]) + (2 * x7) + (x12 * (*error_state).Object.Pose.AxisAngleRot[0]); const FLT x29 = x28 * x15; const FLT x30 = x3 * x15; const FLT x31 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x32 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x33 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x34 = (x33 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x32 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x31 * (*error_state).Object.IMUBias.IMUCorrection[2]); const FLT x35 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x36 = (-1 * x33 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x35 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (x31 * (*error_state).Object.IMUBias.IMUCorrection[1]); const FLT x37 = (-1 * x33 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x35 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x32 * (*error_state).Object.IMUBias.IMUCorrection[1]); const FLT x38 = (x31 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (-1 * x35 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (*_x0).Object.IMUBias.IMUCorrection[1] + (x32 * (*error_state).Object.IMUBias.IMUCorrection[2]); const FLT x39 = (x38 * x38) + (x37 * x37) + (x34 * x34) + (x36 * x36); const FLT x40 = 1. / sqrt(x39); const FLT x41 = 1.0 * x38; const FLT x42 = 1.0 * x34; const FLT x43 = 1.0 * x36; const FLT x44 = 1.0/2.0 * (1. / (x39 * sqrt(x39))); const FLT x45 = x44 * ((x43 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (x42 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (2 * x37) + (x41 * (*error_state).Object.IMUBias.IMUCorrection[0])); const FLT x46 = 0.5 * x40; const FLT x47 = x46 * (*error_state).Object.IMUBias.IMUCorrection[0]; const FLT x48 = -1 * x47; const FLT x49 = 1.0 * x37; const FLT x50 = x44 * ((-1 * x43 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (2 * x38) + (x42 * (*error_state).Object.IMUBias.IMUCorrection[1])); const FLT x51 = x46 * (*error_state).Object.IMUBias.IMUCorrection[1]; const FLT x52 = -1 * x51; const FLT x53 = x44 * ((x41 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (2 * x36) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x42 * (*error_state).Object.IMUBias.IMUCorrection[0])); const FLT x54 = x46 * (*error_state).Object.IMUBias.IMUCorrection[2]; const FLT x55 = -1 * x54; const FLT x56 = x44 * ((x43 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x41 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (2 * x34)); const FLT x57 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x58 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x59 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x60 = (-1 * x59 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x57 * (*_x0).Lighthouse.Rot[3]) + (-1 * x58 * (*_x0).Lighthouse.Rot[2]); const FLT x61 = (*_x0).Lighthouse.Rot[3] + (-1 * x59 * (*_x0).Lighthouse.Rot[2]) + (x58 * (*_x0).Lighthouse.Rot[1]) + (x57 * (*_x0).Lighthouse.Rot[0]); const FLT x62 = (-1 * x57 * (*_x0).Lighthouse.Rot[1]) + (x58 * (*_x0).Lighthouse.Rot[0]) + (x59 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x63 = (x59 * (*_x0).Lighthouse.Rot[0]) + (-1 * x58 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x57 * (*_x0).Lighthouse.Rot[2]); const FLT x64 = (x63 * x63) + (x62 * x62) + (x60 * x60) + (x61 * x61); const FLT x65 = 1. / sqrt(x64); const FLT x66 = 1.0 * x62; const FLT x67 = 1.0 * x63; const FLT x68 = 1.0 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x69 = (x61 * x68) + (x66 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x67 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x60); const FLT x70 = 1.0/2.0 * (1. / (x64 * sqrt(x64))); const FLT x71 = x70 * x60; const FLT x72 = 0.5 * x65; const FLT x73 = x72 * (*error_state).Lighthouse.AxisAngleRot[0]; const FLT x74 = -1 * x73; const FLT x75 = 1.0 * x60; const FLT x76 = 1.0 * x61; const FLT x77 = (x76 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x75 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x63) + (-1 * x62 * x68); const FLT x78 = x72 * (*error_state).Lighthouse.AxisAngleRot[1]; const FLT x79 = -1 * x78; const FLT x80 = (-1 * x76 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x75 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x63 * x68) + (2 * x62); const FLT x81 = x72 * (*error_state).Lighthouse.AxisAngleRot[2]; const FLT x82 = -1 * x81; const FLT x83 = (-1 * x67 * (*error_state).Lighthouse.AxisAngleRot[1]) + (2 * x61) + (x66 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x60 * x68); const FLT x84 = x70 * x69; const FLT x85 = x70 * x77; const FLT x86 = x80 * x70; const FLT x87 = x83 * x70; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x6 * x16) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x6 * x22) + x19); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x6 * x25) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x6 * x29) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x8 * x16) + x18); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x8 * x22) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x8 * x25) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x8 * x29) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x30 * x14) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x30 * x21) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x3 * x25) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x30 * x28) + x18); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x7 * x16) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x7 * x22) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x7 * x25) + x19); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x7 * x29) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x37) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x37) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x37) + x52); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x37) + x55); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x38) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x38) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x38) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x38) + x52); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x36) + x51); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x36) + x55); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x36) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x36) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x34) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x34) + x51); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x34) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x34) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x71 * x69) + x65); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x71 * x77) + x74); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x80 * x71) + x79); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x83 * x71) + x82); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x63) + x73); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x63) + x65); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x63) + x81); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x63) + x79); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x62) + x78); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x62) + x82); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x62) + x65); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x62) + x73); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x61) + x81); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x61) + x78); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x61) + x74); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x61) + x65); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] // Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], (*error_state).Object.Acc[0], (*error_state).Object.Acc[1], (*error_state).Object.Acc[2], (*error_state).Object.IMUBias.AccBias[0], (*error_state).Object.IMUBias.AccBias[1], (*error_state).Object.IMUBias.AccBias[2], (*error_state).Object.IMUBias.AccScale[0], (*error_state).Object.IMUBias.AccScale[1], (*error_state).Object.IMUBias.AccScale[2], (*error_state).Object.IMUBias.GyroBias[0], (*error_state).Object.IMUBias.GyroBias[1], (*error_state).Object.IMUBias.GyroBias[2], (*error_state).Object.IMUBias.IMUCorrection[0], (*error_state).Object.IMUBias.IMUCorrection[1], (*error_state).Object.IMUBias.IMUCorrection[2], (*error_state).Object.Pose.AxisAngleRot[0], (*error_state).Object.Pose.AxisAngleRot[1], (*error_state).Object.Pose.AxisAngleRot[2], (*error_state).Object.Pose.Pos[0], (*error_state).Object.Pose.Pos[1], (*error_state).Object.Pose.Pos[2], (*error_state).Object.Velocity.AxisAngleRot[0], (*error_state).Object.Velocity.AxisAngleRot[1], (*error_state).Object.Velocity.AxisAngleRot[2], (*error_state).Object.Velocity.Pos[0], (*error_state).Object.Velocity.Pos[1], (*error_state).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanModelAddErrorModel_jac_error_state(CnMat* Hx, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[0]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x3 = (-1 * x2 * (*error_state).Object.Pose.AxisAngleRot[2]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*_x0).Object.Pose.Rot[3]); const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (-1 * x2 * (*error_state).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0]; const FLT x7 = (x2 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[2]) + (*_x0).Object.Pose.Rot[3] + (-1 * x0 * (*_x0).Object.Pose.Rot[2]); const FLT x8 = (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[1] + (x4 * (*error_state).Object.Pose.AxisAngleRot[2]); const FLT x9 = (x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6); const FLT x10 = 1. / sqrt(x9); const FLT x11 = x2 * x10; const FLT x12 = -1 * x11; const FLT x13 = 1.0 * x8; const FLT x14 = 1.0 * x7; const FLT x15 = 1.0 * x3; const FLT x16 = 1.0 * (*_x0).Object.Pose.Rot[1]; const FLT x17 = 1.0/2.0 * (1. / (x9 * sqrt(x9))); const FLT x18 = x17 * ((x15 * (*_x0).Object.Pose.Rot[3]) + (-1 * x6 * x16) + (x13 * (*_x0).Object.Pose.Rot[0]) + (-1 * x14 * (*_x0).Object.Pose.Rot[2])); const FLT x19 = x4 * x10; const FLT x20 = -1 * x19; const FLT x21 = 1.0 * x6; const FLT x22 = x17 * ((-1 * x21 * (*_x0).Object.Pose.Rot[2]) + (x15 * (*_x0).Object.Pose.Rot[0]) + (-1 * x13 * (*_x0).Object.Pose.Rot[3]) + (x7 * x16)); const FLT x23 = x5 * x10; const FLT x24 = -1 * x23; const FLT x25 = x17 * ((-1 * x21 * (*_x0).Object.Pose.Rot[3]) + (x13 * (*_x0).Object.Pose.Rot[2]) + (x14 * (*_x0).Object.Pose.Rot[0]) + (-1 * x3 * x16)); const FLT x26 = x1 * x10; const FLT x27 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[2]; const FLT x28 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[0]; const FLT x29 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1]; const FLT x30 = (x29 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x28 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x27 * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x31 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[1]; const FLT x32 = (-1 * x27 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (x28 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x31 * (*_x0).Object.IMUBias.IMUCorrection[0]); const FLT x33 = (-1 * x29 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x27 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * x31 * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x34 = (*_x0).Object.IMUBias.IMUCorrection[1] + (-1 * x31 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x28 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (x27 * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x35 = (x34 * x34) + (x33 * x33) + (x30 * x30) + (x32 * x32); const FLT x36 = 1. / sqrt(x35); const FLT x37 = x36 * x29; const FLT x38 = -1 * x37; const FLT x39 = 1.0 * x33; const FLT x40 = 1.0 * x30; const FLT x41 = 1.0 * x34; const FLT x42 = 1.0 * x32; const FLT x43 = (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (x42 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x41 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x40 * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x44 = 1.0/2.0 * (1. / (x35 * sqrt(x35))); const FLT x45 = x44 * x33; const FLT x46 = 0.5 * x36; const FLT x47 = x46 * (*_x0).Object.IMUBias.IMUCorrection[2]; const FLT x48 = -1 * x47; const FLT x49 = (x42 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x41 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x40 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[2]); const FLT x50 = x46 * (*_x0).Object.IMUBias.IMUCorrection[3]; const FLT x51 = -1 * x50; const FLT x52 = (x40 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (x41 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (-1 * x42 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[3]); const FLT x53 = x46 * (*_x0).Object.IMUBias.IMUCorrection[0]; const FLT x54 = x43 * x44; const FLT x55 = x44 * x49; const FLT x56 = x52 * x44; const FLT x57 = x44 * x30; const FLT x58 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x59 = 0.5 * (*_x0).Lighthouse.Rot[2]; const FLT x60 = 0.5 * (*_x0).Lighthouse.Rot[1]; const FLT x61 = (*_x0).Lighthouse.Rot[0] + (-1 * x58 * (*error_state).Lighthouse.AxisAngleRot[2]) + (-1 * x60 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x59 * (*error_state).Lighthouse.AxisAngleRot[1]); const FLT x62 = 0.5 * (*_x0).Lighthouse.Rot[0]; const FLT x63 = (x60 * (*error_state).Lighthouse.AxisAngleRot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x59 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x62 * (*error_state).Lighthouse.AxisAngleRot[2]); const FLT x64 = (-1 * x60 * (*error_state).Lighthouse.AxisAngleRot[2]) + (x58 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x62 * (*error_state).Lighthouse.AxisAngleRot[1]) + (*_x0).Lighthouse.Rot[2]; const FLT x65 = (*_x0).Lighthouse.Rot[1] + (x62 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x58 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x59 * (*error_state).Lighthouse.AxisAngleRot[2]); const FLT x66 = (x64 * x64) + (x65 * x65) + (x61 * x61) + (x63 * x63); const FLT x67 = 0.5 * (1. / sqrt(x66)); const FLT x68 = x67 * (*_x0).Lighthouse.Rot[1]; const FLT x69 = -1 * x68; const FLT x70 = 1.0 * (*_x0).Lighthouse.Rot[0]; const FLT x71 = 1.0 * x64; const FLT x72 = 1.0 * x61; const FLT x73 = 1.0 * x63; const FLT x74 = (x70 * x65) + (-1 * x73 * (*_x0).Lighthouse.Rot[2]) + (-1 * x72 * (*_x0).Lighthouse.Rot[1]) + (x71 * (*_x0).Lighthouse.Rot[3]); const FLT x75 = 1.0/2.0 * (1. / (x66 * sqrt(x66))); const FLT x76 = x75 * x61; const FLT x77 = x67 * (*_x0).Lighthouse.Rot[2]; const FLT x78 = -1 * x77; const FLT x79 = 1.0 * x65; const FLT x80 = (x73 * (*_x0).Lighthouse.Rot[1]) + (-1 * x79 * (*_x0).Lighthouse.Rot[3]) + (x70 * x64) + (-1 * x72 * (*_x0).Lighthouse.Rot[2]); const FLT x81 = x67 * (*_x0).Lighthouse.Rot[3]; const FLT x82 = -1 * x81; const FLT x83 = (-1 * x72 * (*_x0).Lighthouse.Rot[3]) + (x79 * (*_x0).Lighthouse.Rot[2]) + (x70 * x63) + (-1 * x71 * (*_x0).Lighthouse.Rot[1]); const FLT x84 = x67 * (*_x0).Lighthouse.Rot[0]; const FLT x85 = x75 * x74; const FLT x86 = x80 * x75; const FLT x87 = x83 * x75; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x18) + x12); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x22) + x20); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x25) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x8 * x18) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x8 * x22) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x8 * x25) + x19); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x3 * x18) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x3 * x22) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x3 * x25) + x12); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x7 * x18) + x20); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x7 * x22) + x11); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x7 * x25) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x43 * x45) + x38); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x45 * x49) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x52 * x45) + x51); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x54 * x34) + x53); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x55 * x34) + x51); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x34) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x54 * x32) + x50); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x55 * x32) + x53); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x32) + x38); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x57 * x43) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x57 * x49) + x37); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x52 * x57) + x53); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x74 * x76) + x69); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x80 * x76) + x78); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x83 * x76) + x82); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x65) + x84); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x65) + x82); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x65) + x77); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x64) + x81); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x64) + x84); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x64) + x69); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x63) + x78); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x63) + x68); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x63) + x84); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), 1); } // Full version Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], (*error_state).Object.Acc[0], (*error_state).Object.Acc[1], (*error_state).Object.Acc[2], (*error_state).Object.IMUBias.AccBias[0], (*error_state).Object.IMUBias.AccBias[1], (*error_state).Object.IMUBias.AccBias[2], (*error_state).Object.IMUBias.AccScale[0], (*error_state).Object.IMUBias.AccScale[1], (*error_state).Object.IMUBias.AccScale[2], (*error_state).Object.IMUBias.GyroBias[0], (*error_state).Object.IMUBias.GyroBias[1], (*error_state).Object.IMUBias.GyroBias[2], (*error_state).Object.IMUBias.IMUCorrection[0], (*error_state).Object.IMUBias.IMUCorrection[1], (*error_state).Object.IMUBias.IMUCorrection[2], (*error_state).Object.Pose.AxisAngleRot[0], (*error_state).Object.Pose.AxisAngleRot[1], (*error_state).Object.Pose.AxisAngleRot[2], (*error_state).Object.Pose.Pos[0], (*error_state).Object.Pose.Pos[1], (*error_state).Object.Pose.Pos[2], (*error_state).Object.Velocity.AxisAngleRot[0], (*error_state).Object.Velocity.AxisAngleRot[1], (*error_state).Object.Velocity.AxisAngleRot[2], (*error_state).Object.Velocity.Pos[0], (*error_state).Object.Velocity.Pos[1], (*error_state).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline FLT SurviveJointKalmanErrorModel_LightMeas_x_gen1(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = 0.5 * (*_x0).Lighthouse.Rot[1]; const FLT x4 = (-1 * x3 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x6 = (x3 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x5 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = x4 * x4; const FLT x9 = (-1 * x3 * (*error_model).Lighthouse.AxisAngleRot[2]) + (x5 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x10 = x9 * x9; const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]); const FLT x12 = x11 * x11; const FLT x13 = x10 + x12 + x7 + x8; const FLT x14 = 1. / x13; const FLT x15 = (1. / sqrt((x14 * x12) + (x8 * x14) + (x7 * x14) + (x14 * x10))) * (1. / sqrt(x13)); const FLT x16 = x4 * x15; const FLT x17 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x18 = x6 * x15; const FLT x19 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x20 = x15 * x11; const FLT x21 = (x20 * x19) + (x0 * x16) + (-1 * x18 * x17); const FLT x22 = x9 * x15; const FLT x23 = (x22 * x17) + (x19 * x16) + (-1 * x0 * x20); const FLT x24 = 1.0/2.0 * dt * fabs(dt); const FLT x25 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x27 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x28 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x26 * (*_x0).Object.Pose.Rot[0]) + (x25 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x29 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x30 = dt * dt; const FLT x31 = x30 * (x29 * x29); const FLT x32 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x33 = x30 * (x32 * x32); const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x35 = x30 * (x34 * x34); const FLT x36 = 1e-10 + x35 + x31 + x33; const FLT x37 = sqrt(x36); const FLT x38 = 0.5 * x37; const FLT x39 = sin(x38); const FLT x40 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x41 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x42 = (x27 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x41 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x40 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x43 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x25 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x26 * (*_x0).Object.Pose.Rot[2]); const FLT x44 = (x40 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[1] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (x41 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x45 = (1. / x36) * (x39 * x39); const FLT x46 = cos(x38); const FLT x47 = (1. / sqrt((x44 * x44) + (x43 * x43) + (x42 * x42) + (x28 * x28))) * (1. / sqrt((x46 * x46) + (x45 * x33) + (x45 * x31) + (x45 * x35))); const FLT x48 = dt * x47 * (1. / x37) * x39; const FLT x49 = x48 * x29; const FLT x50 = x48 * x34; const FLT x51 = x46 * x47; const FLT x52 = x48 * x32; const FLT x53 = (x52 * x43) + (x51 * x44) + (-1 * x49 * x28) + (x50 * x42); const FLT x54 = (x51 * x43) + (-1 * x52 * x44) + (-1 * x42 * x49) + (-1 * x50 * x28); const FLT x55 = (-1 * x52 * x42) + (x44 * x49) + (x50 * x43) + (x51 * x28); const FLT x56 = (-1 * x55 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x53 * sensor_pt[1]); const FLT x57 = (x51 * x42) + (x43 * x49) + (x52 * x28) + (-1 * x50 * x44); const FLT x58 = (-1 * x57 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x55 * sensor_pt[2]); const FLT x59 = (*_x0).Object.Pose.Pos[1] + (x24 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x58 * x57) + (-1 * x53 * x56))); const FLT x60 = x59 * x15; const FLT x61 = (-1 * x53 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x57 * sensor_pt[0]); const FLT x62 = (x24 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (*_x0).Object.Pose.Pos[0] + (2 * ((x56 * x55) + (-1 * x61 * x57))); const FLT x63 = (*_x0).Object.Pose.Pos[2] + (2 * ((x61 * x53) + (-1 * x58 * x55))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x24 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x64 = (x63 * x20) + (x4 * x60) + (-1 * x62 * x18); const FLT x65 = (x62 * x22) + (x63 * x16) + (-1 * x60 * x11); const FLT x66 = x62 + (-1 * (x17 + (2 * ((-1 * x22 * x23) + (x21 * x18))))) + (2 * ((-1 * x65 * x22) + (x64 * x18))); const FLT x67 = (x0 * x18) + (-1 * x22 * x19) + (x17 * x16); const FLT x68 = (x59 * x18) + (x62 * x16) + (-1 * x63 * x22); const FLT x69 = x63 + (-1 * (x19 + (2 * ((-1 * x20 * x21) + (x67 * x22))))) + (2 * ((-1 * x64 * x20) + (x68 * x22))); const FLT x70 = -1 * x69; const FLT x71 = x59 + (2 * ((-1 * x68 * x18) + (x65 * x20))) + (-1 * (x0 + (2 * ((-1 * x67 * x18) + (x23 * x20))))); const FLT x72 = (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * atan2(x66, x70)) + (-1 * asin((1. / sqrt((x66 * x66) + (x69 * x69))) * x71 * ((*error_model).BSD0.tilt + (*_x0).BSD0.tilt))); return x72 + (-1 * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag) * cos(1.5707963267949 + x72 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha)) + ((atan2(x71, x70) * atan2(x71, x70)) * ((*error_model).BSD0.curve + (*_x0).BSD0.curve)); } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Lighthouse.Rot[2]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[0]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x6 = (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*_x0).Lighthouse.Rot[1]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[1]); const FLT x7 = x6 * x6; const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x5 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x9 = x8 * x8; const FLT x10 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x0 * (*error_model).Lighthouse.AxisAngleRot[2]); const FLT x11 = x10 * x10; const FLT x12 = x11 + x9 + x4 + x7; const FLT x13 = 1. / x12; const FLT x14 = x4 * x13; const FLT x15 = x9 * x13; const FLT x16 = x13 * x11; const FLT x17 = (x7 * x13) + x14 + x16 + x15; const FLT x18 = 1. / x17; const FLT x19 = 2 * x10; const FLT x20 = x13 * x19; const FLT x21 = x20 * x18; const FLT x22 = x3 * x21; const FLT x23 = -1 * x22; const FLT x24 = 2 * x6; const FLT x25 = x24 * x13; const FLT x26 = x8 * x25 * x18; const FLT x27 = -1 * x26; const FLT x28 = x27 + x23; const FLT x29 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x30 = 1. / sqrt(x17); const FLT x31 = 1. / sqrt(x12); const FLT x32 = x30 * x31; const FLT x33 = x32 * x29; const FLT x34 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x35 = x32 * x34; const FLT x36 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x37 = x32 * x36; const FLT x38 = (x37 * x10) + (x6 * x33) + (-1 * x3 * x35); const FLT x39 = x3 * x32; const FLT x40 = (x8 * x35) + (x6 * x37) + (-1 * x33 * x10); const FLT x41 = x8 * x32; const FLT x42 = dt * fabs(dt); const FLT x43 = 1.0/2.0 * x42; const FLT x44 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x45 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x46 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x47 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x48 = (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x46 * (*_x0).Object.Pose.Rot[0]) + (x45 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x49 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x50 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x51 = (x46 * (*_x0).Object.Pose.Rot[1]) + (-1 * x50 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x49 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x52 = (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x45 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x46 * (*_x0).Object.Pose.Rot[2]); const FLT x53 = (*_x0).Object.Pose.Rot[1] + (x49 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x46 * (*_x0).Object.Pose.Rot[3]) + (x50 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x54 = (x53 * x53) + (x52 * x52) + (x51 * x51) + (x48 * x48); const FLT x55 = 1. / sqrt(x54); const FLT x56 = dt * dt; const FLT x57 = x44 * x44; const FLT x58 = x57 * x56; const FLT x59 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x60 = x59 * x59; const FLT x61 = x60 * x56; const FLT x62 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x63 = x62 * x62; const FLT x64 = x63 * x56; const FLT x65 = 1e-10 + x64 + x58 + x61; const FLT x66 = sqrt(x65); const FLT x67 = 0.5 * x66; const FLT x68 = sin(x67); const FLT x69 = x68 * x68; const FLT x70 = 1. / x65; const FLT x71 = x70 * x69; const FLT x72 = cos(x67); const FLT x73 = (x71 * x61) + (x72 * x72) + (x71 * x58) + (x71 * x64); const FLT x74 = 1. / sqrt(x73); const FLT x75 = x74 * x55; const FLT x76 = x68 * (1. / x66); const FLT x77 = dt * x76; const FLT x78 = x75 * x77; const FLT x79 = x78 * x48; const FLT x80 = x78 * x51; const FLT x81 = x72 * x55; const FLT x82 = x81 * x74; const FLT x83 = x82 * x53; const FLT x84 = x78 * x52; const FLT x85 = (x84 * x59) + x83 + (-1 * x79 * x44) + (x80 * x62); const FLT x86 = x82 * x52; const FLT x87 = x75 * x53; const FLT x88 = x87 * x77; const FLT x89 = (-1 * x88 * x59) + x86 + (-1 * x80 * x44) + (-1 * x79 * x62); const FLT x90 = x82 * x48; const FLT x91 = (-1 * x80 * x59) + (x88 * x44) + (x84 * x62) + x90; const FLT x92 = (-1 * x91 * sensor_pt[0]) + (x89 * sensor_pt[2]) + (x85 * sensor_pt[1]); const FLT x93 = x82 * x51; const FLT x94 = (x79 * x59) + x93 + (x84 * x44) + (-1 * x88 * x62); const FLT x95 = (-1 * x94 * sensor_pt[1]) + (x89 * sensor_pt[0]) + (x91 * sensor_pt[2]); const FLT x96 = (*_x0).Object.Pose.Pos[1] + (x43 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + sensor_pt[1] + (2 * ((x95 * x94) + (-1 * x85 * x92))); const FLT x97 = x96 * x32; const FLT x98 = (-1 * x85 * sensor_pt[2]) + (x89 * sensor_pt[1]) + (x94 * sensor_pt[0]); const FLT x99 = (*_x0).Object.Pose.Pos[0] + (x43 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x92 * x91) + (-1 * x98 * x94))); const FLT x100 = x99 * x32; const FLT x101 = (*_x0).Object.Pose.Pos[2] + (2 * ((x85 * x98) + (-1 * x91 * x95))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x43 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x102 = x32 * x101; const FLT x103 = (x10 * x102) + (x6 * x97) + (-1 * x3 * x100); const FLT x104 = (x6 * x102) + (x8 * x100) + (-1 * x97 * x10); const FLT x105 = x32 * x104; const FLT x106 = x99 + (-1 * (x34 + (2 * ((-1 * x40 * x41) + (x38 * x39))))) + (2 * ((-1 * x8 * x105) + (x39 * x103))); const FLT x107 = (x3 * x33) + (-1 * x8 * x37) + (x6 * x35); const FLT x108 = x32 * x10; const FLT x109 = (x6 * x100) + (x3 * x97) + (-1 * x8 * x102); const FLT x110 = (-1 * (x36 + (2 * ((-1 * x38 * x108) + (x41 * x107))))) + x101 + (2 * ((-1 * x103 * x108) + (x41 * x109))); const FLT x111 = x110 * x110; const FLT x112 = 1. / x111; const FLT x113 = x106 * x112; const FLT x114 = 2 * x18; const FLT x115 = x15 * x114; const FLT x116 = x14 * x114; const FLT x117 = -1 + x116 + x115; const FLT x118 = 1. / x110; const FLT x119 = (x106 * x106) + x111; const FLT x120 = 1. / x119; const FLT x121 = x111 * x120; const FLT x122 = 2 * x106; const FLT x123 = 2 * x110; const FLT x124 = (2 * ((-1 * x39 * x109) + (x10 * x105))) + x96 + (-1 * (x29 + (2 * ((-1 * x39 * x107) + (x40 * x108))))); const FLT x125 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x126 = 1.0/2.0 * (1. / (x119 * sqrt(x119))) * x124 * x125; const FLT x127 = 2 * x3; const FLT x128 = x13 * x127; const FLT x129 = x18 * x128; const FLT x130 = x6 * x129; const FLT x131 = x8 * x21; const FLT x132 = -1 * x131; const FLT x133 = x132 + x130; const FLT x134 = 1. / sqrt(x119); const FLT x135 = x125 * x134; const FLT x136 = x124 * x124; const FLT x137 = 1. / sqrt(1 + (-1 * x120 * (x125 * x125) * x136)); const FLT x138 = (-1 * x137 * ((x133 * x135) + (-1 * x126 * ((x28 * x123) + (x117 * x122))))) + (-1 * x121 * ((-1 * x118 * x117) + (x28 * x113))); const FLT x139 = -1 * x110; const FLT x140 = 1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * atan2(x106, x139)) + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * asin(x124 * x135)); const FLT x141 = sin(x140) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag); const FLT x142 = x112 * x124; const FLT x143 = atan2(x124, x139); const FLT x144 = 2 * (1. / (x111 + x136)) * x111 * x143 * ((*error_model).BSD0.curve + (*_x0).BSD0.curve); const FLT x145 = x6 * x21; const FLT x146 = x8 * x129; const FLT x147 = -1 * x146; const FLT x148 = x147 + x145; const FLT x149 = -1 * x130; const FLT x150 = x149 + x132; const FLT x151 = x16 * x114; const FLT x152 = -1 + x151; const FLT x153 = x152 + x116; const FLT x154 = (-1 * x137 * ((x135 * x153) + (-1 * ((x123 * x148) + (x122 * x150)) * x126))) + (-1 * ((-1 * x118 * x150) + (x113 * x148)) * x121); const FLT x155 = x152 + x115; const FLT x156 = x23 + x26; const FLT x157 = -1 * x145; const FLT x158 = x157 + x147; const FLT x159 = (-1 * x137 * ((x135 * x158) + (-1 * ((x123 * x155) + (x122 * x156)) * x126))) + (-1 * ((-1 * x118 * x156) + (x113 * x155)) * x121); const FLT x160 = 1.0 * x8; const FLT x161 = x160 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x162 = 1.0 * x10; const FLT x163 = x162 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x164 = 1.0 * x3; const FLT x165 = x164 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x166 = x24 + x165 + x161 + x163; const FLT x167 = x30 * (1. / (x12 * sqrt(x12))); const FLT x168 = x167 * x166; const FLT x169 = x10 * x103; const FLT x170 = x8 * x109; const FLT x171 = 1. / (x12 * x12); const FLT x172 = x166 * x171; const FLT x173 = x31 * (1. / (x17 * sqrt(x17))); const FLT x174 = x173 * (x25 + (x13 * x161) + (-1 * x9 * x172) + (-1 * x4 * x172) + (x13 * x163) + (-1 * x11 * x172) + (-1 * x7 * x172) + (x13 * x165)); const FLT x175 = x38 * x10; const FLT x176 = x8 * x107; const FLT x177 = 1.0 * x32; const FLT x178 = x177 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x179 = x109 * x178; const FLT x180 = x107 * x178; const FLT x181 = 1.0/2.0 * x10; const FLT x182 = x101 * x174; const FLT x183 = x5 * x102; const FLT x184 = 1.0/2.0 * x3; const FLT x185 = x99 * x184; const FLT x186 = 1.0/2.0 * x6; const FLT x187 = x96 * x174; const FLT x188 = x101 * x181; const FLT x189 = x96 * x168; const FLT x190 = -1 * x1 * x100; const FLT x191 = x97 + x190; const FLT x192 = (x174 * x185) + (-1 * x186 * x189) + x183 + x191 + (-1 * x181 * x182) + (x168 * x185) + (-1 * x187 * x186) + (-1 * x168 * x188); const FLT x193 = x32 * x19; const FLT x194 = x34 * x184; const FLT x195 = x36 * x181; const FLT x196 = x168 * x186; const FLT x197 = x5 * x37; const FLT x198 = x174 * x186; const FLT x199 = -1 * x1 * x35; const FLT x200 = x197 + x199 + (x174 * x194) + (-1 * x29 * x198) + (-1 * x174 * x195) + x33 + (x168 * x194) + (-1 * x29 * x196) + (-1 * x168 * x195); const FLT x201 = x29 * x184; const FLT x202 = x1 * x33; const FLT x203 = 1.0/2.0 * x8; const FLT x204 = x36 * x203; const FLT x205 = -1 * x2 * x37; const FLT x206 = x205 + x35; const FLT x207 = x206 + (x204 * x168) + (-1 * x34 * x198) + x202 + (-1 * x201 * x168) + (-1 * x201 * x174) + (-1 * x34 * x196) + (x204 * x174); const FLT x208 = 2 * x8; const FLT x209 = x32 * x208; const FLT x210 = x1 * x97; const FLT x211 = x99 * x186; const FLT x212 = -1 * x2 * x102; const FLT x213 = x203 * x101; const FLT x214 = (x213 * x174) + x100 + x212 + (-1 * x189 * x184) + (-1 * x187 * x184) + (-1 * x211 * x168) + x210 + (x213 * x168) + (-1 * x211 * x174); const FLT x215 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x216 = x32 * x215; const FLT x217 = (x38 * x216) + (-1 * x216 * x103); const FLT x218 = x217 + (x214 * x209) + (-1 * x168 * x175) + (x176 * x174) + (x200 * x193) + (-1 * x180) + (-1 * x170 * x174) + x179 + (x168 * x176) + (x169 * x168) + (-1 * x168 * x170) + (-1 * x174 * x175) + (-1 * x192 * x193) + (-1 * x209 * x207) + (x169 * x174); const FLT x219 = x32 * x127; const FLT x220 = x177 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x221 = x220 * x103; const FLT x222 = x3 * x103; const FLT x223 = x3 * x38; const FLT x224 = x38 * x220; const FLT x225 = x8 * x104; const FLT x226 = x34 * x203; const FLT x227 = x29 * x181; const FLT x228 = x2 * x35; const FLT x229 = -1 * x5 * x33; const FLT x230 = x229 + (x227 * x174) + (-1 * x36 * x196) + (-1 * x226 * x174) + x37 + x228 + (x227 * x168) + (-1 * x226 * x168) + (-1 * x36 * x198); const FLT x231 = x8 * x40; const FLT x232 = x99 * x203; const FLT x233 = x101 * x186; const FLT x234 = x2 * x100; const FLT x235 = -1 * x5 * x97; const FLT x236 = x235 + (-1 * x233 * x168) + x102 + (-1 * x232 * x168) + (x181 * x189) + (-1 * x186 * x182) + x234 + (-1 * x232 * x174) + (x181 * x187); const FLT x237 = 1.0 * x105; const FLT x238 = (x40 * x178) + (-1 * x237 * (*error_model).Lighthouse.AxisAngleRot[1]); const FLT x239 = x238 + (x225 * x174) + (-1 * x222 * x168) + (-1 * x222 * x174) + (-1 * x224) + (x219 * x192) + (x223 * x168) + (-1 * x236 * x209) + (-1 * x219 * x200) + (x225 * x168) + (-1 * x231 * x168) + (-1 * x231 * x174) + x221 + (x230 * x209) + (x223 * x174); const FLT x240 = x40 * x10; const FLT x241 = x40 * x216; const FLT x242 = x3 * x107; const FLT x243 = x10 * x104; const FLT x244 = x3 * x109; const FLT x245 = x215 * x105; const FLT x246 = (x220 * x107) + (-1 * x220 * x109); const FLT x247 = x246 + (-1 * x230 * x193) + (x240 * x168) + (-1 * x219 * x214) + (x236 * x193) + (-1 * x242 * x168) + (-1 * x243 * x168) + (-1 * x241) + (-1 * x243 * x174) + (x240 * x174) + (-1 * x242 * x174) + (x244 * x174) + (x219 * x207) + x245 + (x244 * x168); const FLT x248 = (-1 * x137 * ((x247 * x135) + (-1 * ((x218 * x123) + (x239 * x122)) * x126))) + (-1 * ((-1 * x239 * x118) + (x218 * x113)) * x121); const FLT x249 = x164 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x250 = x160 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x251 = 1.0 * x6; const FLT x252 = x251 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x253 = (-1 * x250) + x249 + (-1 * x252) + x19; const FLT x254 = x253 * x167; const FLT x255 = x253 * x171; const FLT x256 = x173 * ((-1 * x9 * x255) + (-1 * x13 * x250) + x20 + (-1 * x7 * x255) + (-1 * x13 * x252) + (-1 * x4 * x255) + (-1 * x11 * x255) + (x13 * x249)); const FLT x257 = x96 * x256; const FLT x258 = x254 * x186; const FLT x259 = (-1 * x234) + x235; const FLT x260 = x259 + (-1 * x96 * x258) + (-1 * x254 * x188) + x102 + (x254 * x185) + (x256 * x185) + (-1 * x256 * x188) + (-1 * x257 * x186); const FLT x261 = 2 * x32; const FLT x262 = x261 * x103; const FLT x263 = x38 * x261; const FLT x264 = x29 * x254; const FLT x265 = x256 * x186; const FLT x266 = (-1 * x228) + x229; const FLT x267 = x266 + (-1 * x29 * x265) + (x256 * x194) + x37 + (-1 * x254 * x195) + (x254 * x194) + (-1 * x264 * x186) + (-1 * x256 * x195); const FLT x268 = x2 * x33; const FLT x269 = x8 * x256; const FLT x270 = 1.0/2.0 * x269; const FLT x271 = x1 * x37; const FLT x272 = x203 * x254; const FLT x273 = x5 * x35; const FLT x274 = (-1 * x201 * x256) + (-1 * x34 * x265) + (-1 * x264 * x184) + (-1 * x273) + (-1 * x34 * x258) + (x36 * x272) + x268 + (x36 * x270) + x271; const FLT x275 = x2 * x97; const FLT x276 = x5 * x100; const FLT x277 = x96 * x254; const FLT x278 = x1 * x102; const FLT x279 = x278 + (x270 * x101) + (x213 * x254) + (-1 * x257 * x184) + (-1 * x211 * x256) + (-1 * x276) + (-1 * x99 * x258) + x275 + (-1 * x277 * x184); const FLT x280 = x246 + (x254 * x169) + (x269 * x107) + (-1 * x262) + (-1 * x254 * x170) + (x209 * x279) + (-1 * x256 * x175) + (x254 * x176) + (-1 * x254 * x175) + (-1 * x256 * x170) + x263 + (x267 * x193) + (-1 * x209 * x274) + (-1 * x260 * x193) + (x256 * x169); const FLT x281 = x199 + (-1 * x197); const FLT x282 = x281 + (-1 * x33) + (x256 * x227) + (x264 * x181) + (-1 * x36 * x265) + (-1 * x34 * x272) + (-1 * x36 * x258) + (-1 * x34 * x270); const FLT x283 = -1 * x183; const FLT x284 = x190 + (-1 * x99 * x270) + (x257 * x181) + (-1 * x99 * x272) + (x277 * x181) + (-1 * x97) + (-1 * x258 * x101) + x283 + (-1 * x233 * x256); const FLT x285 = (x103 * x178) + (-1 * x38 * x178); const FLT x286 = (x237 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x40 * x220); const FLT x287 = x286 + (-1 * x209 * x284) + (x256 * x223) + (x254 * x225) + (x269 * x104) + x285 + (-1 * x219 * x267) + (-1 * x40 * x269) + (-1 * x254 * x222) + (-1 * x231 * x254) + (-1 * x256 * x222) + (x209 * x282) + (x254 * x223) + (x219 * x260); const FLT x288 = 2 * x105; const FLT x289 = x40 * x261; const FLT x290 = (-1 * x256 * x242) + (-1 * x179) + (x284 * x193) + (x256 * x244) + (x219 * x274) + (-1 * x254 * x242) + (x254 * x244) + (x256 * x240) + (-1 * x256 * x243) + x180 + x288 + (-1 * x219 * x279) + (x254 * x240) + (-1 * x289) + (-1 * x282 * x193) + (-1 * x254 * x243); const FLT x291 = (-1 * x137 * ((x290 * x135) + (-1 * ((x280 * x123) + (x287 * x122)) * x126))) + (-1 * ((-1 * x287 * x118) + (x280 * x113)) * x121); const FLT x292 = x162 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x293 = x3 * x215; const FLT x294 = x251 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x295 = (-1 * x294) + (-1 * x293) + x208 + x292; const FLT x296 = x295 * x167; const FLT x297 = x11 * x171; const FLT x298 = x4 * x171; const FLT x299 = x7 * x171; const FLT x300 = x9 * x171; const FLT x301 = ((-1 * x13 * x294) + (x13 * x208) + (-1 * x295 * x300) + (-1 * x298 * x295) + (-1 * x297 * x295) + (x13 * x292) + (-1 * x299 * x295) + (-1 * x13 * x293)) * x173; const FLT x302 = x301 * x186; const FLT x303 = x296 * x186; const FLT x304 = x276 + (-1 * x96 * x303) + (-1 * x275) + x278 + (x301 * x185) + (-1 * x301 * x188) + (-1 * x296 * x188) + (-1 * x96 * x302) + (x296 * x185); const FLT x305 = x261 * x109; const FLT x306 = x266 + (-1 * x201 * x296) + (-1 * x34 * x302) + (x204 * x301) + (x204 * x296) + (-1 * x34 * x303) + (-1 * x201 * x301) + (-1 * x37); const FLT x307 = (x301 * x194) + x273 + (-1 * x268) + x271 + (-1 * x29 * x302) + (-1 * x296 * x195) + (-1 * x29 * x303) + (-1 * x301 * x195) + (x296 * x194); const FLT x308 = x261 * x107; const FLT x309 = x96 * x301; const FLT x310 = x96 * x296; const FLT x311 = (x213 * x301) + (-1 * x102) + (x213 * x296) + (-1 * x99 * x302) + (-1 * x211 * x296) + x259 + (-1 * x309 * x184) + (-1 * x310 * x184); const FLT x312 = (x209 * x311) + (x296 * x176) + (-1 * x296 * x175) + (-1 * x209 * x306) + (x296 * x169) + (-1 * x296 * x170) + (-1 * x301 * x175) + (x307 * x193) + x305 + (-1 * x308) + (-1 * x304 * x193) + (-1 * x221) + (x301 * x169) + (x301 * x176) + (-1 * x301 * x170) + x224; const FLT x313 = -1 * x202; const FLT x314 = (x227 * x301) + x313 + (-1 * x36 * x302) + (-1 * x226 * x301) + x206 + (-1 * x36 * x303) + (-1 * x296 * x226) + (x296 * x227); const FLT x315 = x212 + (-1 * x210); const FLT x316 = x315 + (-1 * x232 * x301) + (x310 * x181) + (-1 * x232 * x296) + (-1 * x302 * x101) + x100 + (x309 * x181) + (-1 * x233 * x296); const FLT x317 = x217 + (x296 * x225) + (x219 * x304) + (-1 * x288) + (-1 * x209 * x316) + (x209 * x314) + (-1 * x296 * x222) + (x223 * x301) + (x225 * x301) + (-1 * x222 * x301) + (-1 * x231 * x301) + (-1 * x231 * x296) + x289 + (x296 * x223) + (-1 * x219 * x307); const FLT x318 = (-1 * x216 * x107) + (x216 * x109); const FLT x319 = x286 + (x296 * x244) + (x219 * x306) + (x240 * x301) + (-1 * x243 * x301) + (x316 * x193) + (x244 * x301) + (x296 * x240) + (-1 * x314 * x193) + (-1 * x296 * x242) + (-1 * x296 * x243) + x318 + (-1 * x219 * x311) + (-1 * x242 * x301); const FLT x320 = (-1 * x137 * ((x319 * x135) + (-1 * ((x312 * x123) + (x317 * x122)) * x126))) + (-1 * ((-1 * x317 * x118) + (x312 * x113)) * x121); const FLT x321 = x162 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x322 = x8 * x215; const FLT x323 = x251 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x324 = (-1 * x323) + x127 + (-1 * x321) + x322; const FLT x325 = x324 * x167; const FLT x326 = x173 * ((-1 * x298 * x324) + (-1 * x13 * x323) + (x13 * x322) + (-1 * x297 * x324) + (-1 * x13 * x321) + (-1 * x299 * x324) + (-1 * x300 * x324) + x128); const FLT x327 = x325 * x186; const FLT x328 = x96 * x326; const FLT x329 = x315 + (-1 * x100) + (-1 * x326 * x188) + (x326 * x185) + (-1 * x325 * x188) + (-1 * x96 * x327) + (x325 * x185) + (-1 * x328 * x186); const FLT x330 = x96 * x325; const FLT x331 = x203 * x326; const FLT x332 = x191 + (x213 * x325) + (x331 * x101) + (-1 * x330 * x184) + (-1 * x99 * x327) + x283 + (-1 * x328 * x184) + (-1 * x211 * x326); const FLT x333 = x29 * x326; const FLT x334 = x36 * x326; const FLT x335 = x34 * x326; const FLT x336 = (x335 * x184) + (-1 * x35) + x313 + (-1 * x333 * x186) + x205 + (-1 * x29 * x327) + (-1 * x325 * x195) + (-1 * x334 * x181) + (x325 * x194); const FLT x337 = x36 * x325; const FLT x338 = (x203 * x337) + (-1 * x335 * x186) + (-1 * x333 * x184) + (x36 * x331) + (-1 * x34 * x327) + x281 + x33 + (-1 * x201 * x325); const FLT x339 = x326 * x103; const FLT x340 = (x10 * x339) + x318 + (x325 * x176) + x285 + (-1 * x325 * x175) + (x336 * x193) + (x325 * x169) + (x326 * x176) + (-1 * x329 * x193) + (-1 * x325 * x170) + (-1 * x209 * x338) + (x209 * x332) + (-1 * x326 * x170) + (-1 * x326 * x175); const FLT x341 = (-1 * x271) + (x333 * x181) + x273 + (-1 * x334 * x186) + (x227 * x325) + (-1 * x34 * x331) + x268 + (-1 * x337 * x186) + (-1 * x226 * x325); const FLT x342 = (-1 * x278) + x275 + (-1 * x99 * x331) + (x328 * x181) + (x330 * x181) + x276 + (-1 * x233 * x326) + (-1 * x232 * x325) + (-1 * x327 * x101); const FLT x343 = (x225 * x325) + (x225 * x326) + (x223 * x326) + (-1 * x263) + x262 + (x223 * x325) + (-1 * x222 * x325) + (-1 * x3 * x339) + (x219 * x329) + (-1 * x245) + (-1 * x231 * x326) + (-1 * x209 * x342) + x241 + (-1 * x219 * x336) + (-1 * x231 * x325) + (x209 * x341); const FLT x344 = x308 + (-1 * x305) + x238 + (x244 * x325) + (-1 * x242 * x326) + (x342 * x193) + (-1 * x243 * x326) + (x219 * x338) + (-1 * x219 * x332) + (x244 * x326) + (x240 * x325) + (x240 * x326) + (-1 * x242 * x325) + (-1 * x243 * x325) + (-1 * x341 * x193); const FLT x345 = (-1 * x137 * ((x344 * x135) + (-1 * ((x340 * x123) + (x343 * x122)) * x126))) + (-1 * ((-1 * x343 * x118) + (x340 * x113)) * x121); const FLT x346 = x42 * x18; const FLT x347 = x13 * x346; const FLT x348 = x8 * x347; const FLT x349 = x6 * x348; const FLT x350 = x3 * x10 * x347; const FLT x351 = x350 + x349; const FLT x352 = -1 * x14 * x346; const FLT x353 = (-1 * x15 * x346) + x43; const FLT x354 = x353 + x352; const FLT x355 = x10 * x348; const FLT x356 = x6 * x347; const FLT x357 = x3 * x356; const FLT x358 = (-1 * x357) + x355; const FLT x359 = (-1 * x137 * ((x358 * x135) + (-1 * ((x351 * x123) + (x354 * x122)) * x126))) + (-1 * ((-1 * x354 * x118) + (x351 * x113)) * x121); const FLT x360 = x3 * x348; const FLT x361 = x10 * x356; const FLT x362 = (-1 * x361) + x360; const FLT x363 = x355 + x357; const FLT x364 = -1 * x16 * x346; const FLT x365 = x364 + x43 + x352; const FLT x366 = (-1 * x137 * ((x365 * x135) + (-1 * ((x362 * x123) + (x363 * x122)) * x126))) + (-1 * ((-1 * x363 * x118) + (x362 * x113)) * x121); const FLT x367 = x353 + x364; const FLT x368 = (-1 * x349) + x350; const FLT x369 = x360 + x361; const FLT x370 = (-1 * x137 * ((x369 * x135) + (-1 * ((x367 * x123) + (x368 * x122)) * x126))) + (-1 * ((-1 * x368 * x118) + (x367 * x113)) * x121); const FLT x371 = x22 + x26; const FLT x372 = -1 * x116; const FLT x373 = -1 * x115; const FLT x374 = 1 + x373 + x372; const FLT x375 = x149 + x131; const FLT x376 = (-1 * x137 * ((x375 * x135) + (-1 * ((x371 * x123) + (x374 * x122)) * x126))) + (-1 * ((-1 * x374 * x118) + (x371 * x113)) * x121); const FLT x377 = x157 + x146; const FLT x378 = x131 + x130; const FLT x379 = 1 + (-1 * x151); const FLT x380 = x379 + x372; const FLT x381 = (-1 * x137 * ((x380 * x135) + (-1 * ((x377 * x123) + (x378 * x122)) * x126))) + (-1 * ((-1 * x378 * x118) + (x377 * x113)) * x121); const FLT x382 = x379 + x373; const FLT x383 = x27 + x22; const FLT x384 = x146 + x145; const FLT x385 = (-1 * x137 * ((x384 * x135) + (-1 * ((x382 * x123) + (x383 * x122)) * x126))) + (-1 * ((-1 * x383 * x118) + (x382 * x113)) * x121); const FLT x386 = 1.0/2.0 * x48; const FLT x387 = x77 * x386; const FLT x388 = 1.0 * x53; const FLT x389 = 1.0 * x51; const FLT x390 = 1.0 * x48; const FLT x391 = (x390 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x389 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x388 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x52); const FLT x392 = x74 * (1. / (x54 * sqrt(x54))); const FLT x393 = x391 * x392; const FLT x394 = x59 * x393; const FLT x395 = 1.0/2.0 * x51; const FLT x396 = x72 * x392; const FLT x397 = x396 * x391; const FLT x398 = 1.0/2.0 * x52; const FLT x399 = x77 * x398; const FLT x400 = x44 * x393; const FLT x401 = x78 * x59; const FLT x402 = x46 * x401; const FLT x403 = 1.0/2.0 * x53; const FLT x404 = x77 * x403; const FLT x405 = x62 * x393; const FLT x406 = 0.5 * x82; const FLT x407 = x406 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x408 = x78 * x44; const FLT x409 = x78 * x62; const FLT x410 = 0.5 * x409; const FLT x411 = -1 * x410 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x412 = x411 + x408; const FLT x413 = (-1 * x395 * x397) + x412 + (-1 * x400 * x399) + x407 + x402 + (-1 * x394 * x387) + (x405 * x404); const FLT x414 = 2 * x98; const FLT x415 = 0.5 * x408; const FLT x416 = x415 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x417 = x82 * x46; const FLT x418 = x77 * x395; const FLT x419 = x59 * x418; const FLT x420 = x62 * x399; const FLT x421 = 0.5 * x401; const FLT x422 = -1 * x421 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x423 = x409 + x422; const FLT x424 = x423 + (-1 * x397 * x386) + x417 + (x419 * x393) + (-1 * x400 * x404) + (-1 * x420 * x393) + x416; const FLT x425 = x421 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x426 = -1 * x425; const FLT x427 = x46 * x409; const FLT x428 = -1 * x427; const FLT x429 = x415 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x430 = x82 + (-1 * x429); const FLT x431 = (x405 * x387) + x430 + x426 + (-1 * x398 * x397) + (x404 * x394) + x428 + (x400 * x418); const FLT x432 = x406 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x433 = x59 * x399; const FLT x434 = -1 * x46 * x408; const FLT x435 = x410 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x436 = x435 + x434; const FLT x437 = (-1 * x405 * x418) + x401 + x436 + (-1 * x433 * x393) + (-1 * x403 * x397) + x432 + (x400 * x387); const FLT x438 = (x437 * sensor_pt[1]) + (-1 * x424 * sensor_pt[0]) + (x431 * sensor_pt[2]); const FLT x439 = 2 * x91; const FLT x440 = (x413 * sensor_pt[0]) + (-1 * x437 * sensor_pt[2]) + (x431 * sensor_pt[1]); const FLT x441 = 2 * x94; const FLT x442 = 2 * x92; const FLT x443 = (x424 * x442) + (-1 * x441 * x440) + (-1 * x413 * x414) + (x438 * x439); const FLT x444 = x6 * x32; const FLT x445 = 2 * x85; const FLT x446 = 2 * x95; const FLT x447 = (x431 * sensor_pt[0]) + (x424 * sensor_pt[2]) + (-1 * x413 * sensor_pt[1]); const FLT x448 = (x441 * x447) + (x413 * x446) + (-1 * x437 * x442) + (-1 * x438 * x445); const FLT x449 = x32 * x448; const FLT x450 = (x445 * x440) + (x414 * x437) + (-1 * x424 * x446) + (-1 * x439 * x447); const FLT x451 = (-1 * x41 * x450) + (x444 * x443) + (x3 * x449); const FLT x452 = (-1 * x39 * x443) + (x450 * x108) + (x6 * x449); const FLT x453 = (x451 * x209) + x450 + (-1 * x452 * x193); const FLT x454 = (-1 * x10 * x449) + (x41 * x443) + (x450 * x444); const FLT x455 = x443 + (-1 * x454 * x209) + (x452 * x219); const FLT x456 = x448 + (x454 * x193) + (-1 * x451 * x219); const FLT x457 = (-1 * x137 * ((x456 * x135) + (-1 * ((x453 * x123) + (x455 * x122)) * x126))) + (-1 * ((-1 * x455 * x118) + (x453 * x113)) * x121); const FLT x458 = 1.0 * x52; const FLT x459 = (-1 * x390 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x53) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x389 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x460 = x59 * x459; const FLT x461 = x460 * x392; const FLT x462 = x459 * x392; const FLT x463 = x62 * x462; const FLT x464 = x459 * x396; const FLT x465 = x44 * x462; const FLT x466 = x429 + x82; const FLT x467 = x466 + (x465 * x387) + x427 + (-1 * x463 * x418) + (-1 * x464 * x403) + x426 + (-1 * x461 * x399); const FLT x468 = -1 * x402; const FLT x469 = -1 * x407; const FLT x470 = x404 * x392; const FLT x471 = x459 * x470; const FLT x472 = (-1 * x463 * x399) + x469 + (-1 * x464 * x386) + (x462 * x419) + x412 + (-1 * x44 * x471) + x468; const FLT x473 = -1 * x401; const FLT x474 = -1 * x432; const FLT x475 = x436 + x474 + (x463 * x387) + (x460 * x470) + x473 + (-1 * x464 * x398) + (x465 * x418); const FLT x476 = (x467 * sensor_pt[1]) + (-1 * x472 * sensor_pt[0]) + (x475 * sensor_pt[2]); const FLT x477 = -1 * x416; const FLT x478 = (-1 * x409) + x422; const FLT x479 = x417 + x478 + (-1 * x461 * x387) + (-1 * x465 * x399) + (x62 * x471) + x477 + (-1 * x464 * x395); const FLT x480 = (x472 * sensor_pt[2]) + (-1 * x479 * sensor_pt[1]) + (x475 * sensor_pt[0]); const FLT x481 = (x480 * x441) + (x479 * x446) + (-1 * x467 * x442) + (-1 * x476 * x445); const FLT x482 = (x479 * sensor_pt[0]) + (x475 * sensor_pt[1]) + (-1 * x467 * sensor_pt[2]); const FLT x483 = (-1 * x482 * x441) + (-1 * x479 * x414) + (x472 * x442) + (x476 * x439); const FLT x484 = (x467 * x414) + (-1 * x472 * x446) + (x482 * x445) + (-1 * x480 * x439); const FLT x485 = (x39 * x481) + (-1 * x41 * x484) + (x483 * x444); const FLT x486 = (-1 * x39 * x483) + (x484 * x108) + (x481 * x444); const FLT x487 = x484 + (x485 * x209) + (-1 * x486 * x193); const FLT x488 = (-1 * x481 * x108) + (x41 * x483) + (x484 * x444); const FLT x489 = x483 + (x486 * x219) + (-1 * x488 * x209); const FLT x490 = x481 + (-1 * x485 * x219) + (x488 * x193); const FLT x491 = (-1 * x137 * ((x490 * x135) + (-1 * ((x487 * x123) + (x489 * x122)) * x126))) + (-1 * ((-1 * x489 * x118) + (x487 * x113)) * x121); const FLT x492 = (2 * x48) + (x388 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x389 * (*error_model).Object.Pose.AxisAngleRot[0]); const FLT x493 = x492 * x392; const FLT x494 = x492 * x396; const FLT x495 = x493 * x387; const FLT x496 = x62 * x493; const FLT x497 = (-1 * x408) + x411; const FLT x498 = x497 + (-1 * x496 * x418) + x468 + x407 + (-1 * x493 * x433) + (x44 * x495) + (-1 * x494 * x403); const FLT x499 = x492 * x470; const FLT x500 = x44 * x399; const FLT x501 = x434 + (-1 * x435); const FLT x502 = x501 + (-1 * x494 * x395) + (-1 * x59 * x495) + x401 + (x62 * x499) + x474 + (-1 * x493 * x500); const FLT x503 = -1 * x417; const FLT x504 = x503 + (x44 * x493 * x418) + x416 + (x59 * x499) + x478 + (-1 * x494 * x398) + (x496 * x387); const FLT x505 = x466 + x425 + (-1 * x44 * x499) + (-1 * x494 * x386) + x428 + (-1 * x493 * x420) + (x493 * x419); const FLT x506 = (x505 * sensor_pt[2]) + (-1 * x502 * sensor_pt[1]) + (x504 * sensor_pt[0]); const FLT x507 = (x498 * sensor_pt[1]) + (-1 * x505 * sensor_pt[0]) + (x504 * sensor_pt[2]); const FLT x508 = (x446 * x502) + (-1 * x445 * x507) + (-1 * x498 * x442) + (x441 * x506); const FLT x509 = (x502 * sensor_pt[0]) + (-1 * x498 * sensor_pt[2]) + (x504 * sensor_pt[1]); const FLT x510 = (-1 * x441 * x509) + (x442 * x505) + (-1 * x414 * x502) + (x439 * x507); const FLT x511 = (x445 * x509) + (-1 * x439 * x506) + (-1 * x446 * x505) + (x498 * x414); const FLT x512 = (-1 * x41 * x511) + (x39 * x508) + (x444 * x510); const FLT x513 = (-1 * x39 * x510) + (x511 * x108) + (x444 * x508); const FLT x514 = x511 + (x512 * x209) + (-1 * x513 * x193); const FLT x515 = (-1 * x508 * x108) + (x41 * x510) + (x444 * x511); const FLT x516 = x510 + (x513 * x219) + (-1 * x515 * x209); const FLT x517 = x508 + (x515 * x193) + (-1 * x512 * x219); const FLT x518 = (-1 * x137 * ((x517 * x135) + (-1 * ((x514 * x123) + (x516 * x122)) * x126))) + (-1 * ((-1 * x516 * x118) + (x514 * x113)) * x121); const FLT x519 = (x390 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x388 * (*error_model).Object.Pose.AxisAngleRot[1]) + (2 * x51) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x520 = x519 * x392; const FLT x521 = x59 * x520; const FLT x522 = x44 * x520; const FLT x523 = x519 * x396; const FLT x524 = x418 * x520; const FLT x525 = (-1 * x62 * x524) + x477 + x503 + x423 + (-1 * x521 * x399) + (x522 * x387) + (-1 * x403 * x523); const FLT x526 = x62 * x520; const FLT x527 = x497 + x469 + (x526 * x387) + x402 + (x404 * x521) + (-1 * x523 * x398) + (x44 * x524); const FLT x528 = (-1 * x523 * x386) + (-1 * x404 * x522) + x473 + x501 + (x419 * x520) + (-1 * x420 * x520) + x432; const FLT x529 = (x525 * sensor_pt[1]) + (x527 * sensor_pt[2]) + (-1 * x528 * sensor_pt[0]); const FLT x530 = x427 + (x404 * x526) + (-1 * x523 * x395) + (-1 * x521 * x387) + x430 + x425 + (-1 * x520 * x500); const FLT x531 = (x528 * sensor_pt[2]) + (-1 * x530 * sensor_pt[1]) + (x527 * sensor_pt[0]); const FLT x532 = (x441 * x531) + (x446 * x530) + (-1 * x442 * x525) + (-1 * x445 * x529); const FLT x533 = (x530 * sensor_pt[0]) + (-1 * x525 * sensor_pt[2]) + (x527 * sensor_pt[1]); const FLT x534 = (x442 * x528) + (-1 * x441 * x533) + (-1 * x414 * x530) + (x439 * x529); const FLT x535 = (x445 * x533) + (-1 * x446 * x528) + (x414 * x525) + (-1 * x439 * x531); const FLT x536 = (-1 * x41 * x535) + (x39 * x532) + (x444 * x534); const FLT x537 = x32 * x535; const FLT x538 = (-1 * x39 * x534) + (x10 * x537) + (x444 * x532); const FLT x539 = x535 + (x536 * x209) + (-1 * x538 * x193); const FLT x540 = (-1 * x532 * x108) + (x41 * x534) + (x6 * x537); const FLT x541 = x534 + (x538 * x219) + (-1 * x540 * x209); const FLT x542 = x532 + (-1 * x536 * x219) + (x540 * x193); const FLT x543 = (-1 * x137 * ((x542 * x135) + (-1 * ((x539 * x123) + (x541 * x122)) * x126))) + (-1 * ((-1 * x541 * x118) + (x539 * x113)) * x121); const FLT x544 = dt * dt * dt; const FLT x545 = 0.5 * x70 * x544; const FLT x546 = x90 * x545; const FLT x547 = x44 * x546; const FLT x548 = x59 * x547; const FLT x549 = x62 * x59; const FLT x550 = x545 * x549; const FLT x551 = x93 * x550; const FLT x552 = x60 * x545; const FLT x553 = x59 * x59 * x59; const FLT x554 = dt * dt * dt * dt; const FLT x555 = 2 * (1. / (x65 * x65)) * x69; const FLT x556 = x554 * x555; const FLT x557 = x68 * (1. / (x65 * sqrt(x65))); const FLT x558 = 1.0 * x72; const FLT x559 = x558 * x557; const FLT x560 = x559 * x554; const FLT x561 = x57 * x59; const FLT x562 = x63 * x556; const FLT x563 = 2 * x71; const FLT x564 = x56 * x563; const FLT x565 = x63 * x560; const FLT x566 = x76 * x558; const FLT x567 = x56 * x566; const FLT x568 = (x560 * x561) + (x553 * x560) + (x59 * x564) + (-1 * x553 * x556) + (-1 * x59 * x567) + (x59 * x565) + (-1 * x556 * x561) + (-1 * x59 * x562); const FLT x569 = 1. / (x73 * sqrt(x73)); const FLT x570 = x55 * x569; const FLT x571 = x62 * x570; const FLT x572 = x571 * x568; const FLT x573 = x44 * x570; const FLT x574 = x573 * x568; const FLT x575 = 0.5 * x76; const FLT x576 = x56 * x575; const FLT x577 = x59 * x576; const FLT x578 = x81 * x569; const FLT x579 = x578 * x568; const FLT x580 = x75 * x51; const FLT x581 = x544 * x557; const FLT x582 = x62 * x581; const FLT x583 = x59 * x582; const FLT x584 = x580 * x583; const FLT x585 = x570 * x568; const FLT x586 = x75 * x44; const FLT x587 = x48 * x586; const FLT x588 = x59 * x581; const FLT x589 = x587 * x588; const FLT x590 = x60 * x581; const FLT x591 = x75 * x590; const FLT x592 = (-1 * x52 * x591) + (-1 * x418 * x572) + x551 + (-1 * x548) + (-1 * x433 * x585) + (x574 * x387) + (-1 * x87 * x577) + (-1 * x403 * x579) + (-1 * x584) + (x86 * x552) + x84 + x589; const FLT x593 = x59 * x585; const FLT x594 = (x87 * x583) + (-1 * x83 * x550); const FLT x595 = x44 * x545; const FLT x596 = x59 * x595; const FLT x597 = x52 * x586; const FLT x598 = (-1 * x597 * x588) + (x86 * x596); const FLT x599 = x594 + (-1 * x48 * x591) + x598 + (-1 * x579 * x395) + (-1 * x577 * x580) + (x60 * x546) + (x404 * x572) + (-1 * x593 * x387) + x79 + (-1 * x574 * x399); const FLT x600 = x75 * x48; const FLT x601 = x600 * x583; const FLT x602 = x418 * x573; const FLT x603 = x546 * x549; const FLT x604 = x75 * x52; const FLT x605 = -1 * x88; const FLT x606 = x93 * x595; const FLT x607 = x44 * x580; const FLT x608 = (x607 * x588) + (-1 * x59 * x606); const FLT x609 = (-1 * x579 * x398) + x605 + (-1 * x83 * x552) + x608 + (-1 * x604 * x577) + (x404 * x593) + (x87 * x590) + (x572 * x387) + (x602 * x568) + (-1 * x603) + x601; const FLT x610 = -1 * x80; const FLT x611 = x87 * x44; const FLT x612 = x611 * x588; const FLT x613 = x83 * x596; const FLT x614 = (-1 * x604 * x583) + (x86 * x550); const FLT x615 = x613 + (x590 * x580) + (x419 * x585) + (-1 * x612) + (-1 * x579 * x386) + (-1 * x600 * x577) + x614 + (-1 * x572 * x399) + x610 + (-1 * x404 * x574) + (-1 * x93 * x552); const FLT x616 = (x615 * sensor_pt[2]) + (-1 * x599 * sensor_pt[1]) + (x609 * sensor_pt[0]); const FLT x617 = (x592 * sensor_pt[1]) + (-1 * x615 * sensor_pt[0]) + (x609 * sensor_pt[2]); const FLT x618 = (-1 * x445 * x617) + (x441 * x616) + (-1 * x442 * x592) + (x446 * x599); const FLT x619 = x32 * x618; const FLT x620 = (x599 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x609 * sensor_pt[1]); const FLT x621 = (x442 * x615) + (-1 * x441 * x620) + (-1 * x414 * x599) + (x439 * x617); const FLT x622 = x32 * x621; const FLT x623 = (x414 * x592) + (x445 * x620) + (-1 * x446 * x615) + (-1 * x439 * x616); const FLT x624 = (-1 * x41 * x623) + (x3 * x619) + (x6 * x622); const FLT x625 = (x623 * x108) + (-1 * x3 * x622) + (x444 * x618); const FLT x626 = x623 + (x624 * x209) + (-1 * x625 * x193); const FLT x627 = x32 * ((-1 * x10 * x619) + (x8 * x622) + (x444 * x623)); const FLT x628 = x621 + (x625 * x219) + (-1 * x627 * x208); const FLT x629 = x618 + (x19 * x627) + (-1 * x624 * x219); const FLT x630 = (-1 * x137 * ((x629 * x135) + (-1 * ((x626 * x123) + (x628 * x122)) * x126))) + (-1 * ((-1 * x628 * x118) + (x626 * x113)) * x121); const FLT x631 = x63 * x545; const FLT x632 = x63 * x581; const FLT x633 = x62 * x556; const FLT x634 = x62 * x560; const FLT x635 = x62 * x56; const FLT x636 = (x62 * x62 * x62) * x554; const FLT x637 = (x635 * x563) + (x636 * x559) + (-1 * x636 * x555) + (-1 * x60 * x633) + (x60 * x634) + (-1 * x635 * x566) + (-1 * x57 * x633) + (x57 * x634); const FLT x638 = x637 * x570; const FLT x639 = x59 * x638; const FLT x640 = x637 * x573; const FLT x641 = x637 * x571; const FLT x642 = x77 * x641; const FLT x643 = x637 * x578; const FLT x644 = x635 * x575; const FLT x645 = x62 * x595; const FLT x646 = (-1 * x597 * x582) + (x86 * x645); const FLT x647 = (-1 * x643 * x395) + (-1 * x601) + (x403 * x642) + (-1 * x83 * x631) + (-1 * x639 * x387) + (-1 * x644 * x580) + (-1 * x640 * x399) + x605 + x646 + (x87 * x632) + x603; const FLT x648 = x75 * x632; const FLT x649 = x611 * x582; const FLT x650 = x83 * x645; const FLT x651 = x84 + (-1 * x643 * x386) + (-1 * x551) + (-1 * x600 * x644) + (-1 * x642 * x398) + (-1 * x404 * x640) + (x419 * x638) + (-1 * x649) + (x86 * x631) + (-1 * x52 * x648) + x584 + x650; const FLT x652 = x607 * x582; const FLT x653 = x62 * x606; const FLT x654 = -1 * x79; const FLT x655 = (-1 * x604 * x644) + x594 + (x641 * x387) + x654 + (-1 * x63 * x546) + (x48 * x648) + (x602 * x637) + x652 + (x404 * x639) + (-1 * x643 * x398) + (-1 * x653); const FLT x656 = (x582 * x587) + (-1 * x62 * x547); const FLT x657 = x656 + x614 + (-1 * x403 * x643) + (-1 * x418 * x641) + (-1 * x87 * x644) + (x93 * x631) + x80 + (-1 * x632 * x580) + (-1 * x433 * x638) + (x640 * x387); const FLT x658 = (x657 * sensor_pt[1]) + (-1 * x651 * sensor_pt[0]) + (x655 * sensor_pt[2]); const FLT x659 = (x647 * sensor_pt[0]) + (-1 * x657 * sensor_pt[2]) + (x655 * sensor_pt[1]); const FLT x660 = (x442 * x651) + (-1 * x414 * x647) + (-1 * x441 * x659) + (x439 * x658); const FLT x661 = (x651 * sensor_pt[2]) + (-1 * x647 * sensor_pt[1]) + (x655 * sensor_pt[0]); const FLT x662 = (x441 * x661) + (-1 * x445 * x658) + (-1 * x442 * x657) + (x446 * x647); const FLT x663 = (-1 * x439 * x661) + (x445 * x659) + (-1 * x446 * x651) + (x414 * x657); const FLT x664 = (-1 * x41 * x663) + (x444 * x660) + (x39 * x662); const FLT x665 = x32 * x663; const FLT x666 = (-1 * x39 * x660) + (x10 * x665) + (x444 * x662); const FLT x667 = x663 + (x664 * x209) + (-1 * x666 * x193); const FLT x668 = (-1 * x662 * x108) + (x41 * x660) + (x6 * x665); const FLT x669 = x660 + (x666 * x219) + (-1 * x668 * x209); const FLT x670 = x662 + (-1 * x664 * x219) + (x668 * x193); const FLT x671 = (-1 * x137 * ((x670 * x135) + (-1 * ((x667 * x123) + (x669 * x122)) * x126))) + (-1 * ((-1 * x669 * x118) + (x667 * x113)) * x121); const FLT x672 = x57 * x581; const FLT x673 = x75 * x672; const FLT x674 = x44 * x576; const FLT x675 = x60 * x44; const FLT x676 = x44 * x44 * x44; const FLT x677 = (x44 * x565) + (-1 * x676 * x556) + (-1 * x675 * x556) + (x675 * x560) + (-1 * x44 * x567) + (x44 * x564) + (x676 * x560) + (-1 * x44 * x562); const FLT x678 = x677 * x570; const FLT x679 = x677 * x578; const FLT x680 = 1.0/2.0 * x679; const FLT x681 = x677 * x573; const FLT x682 = x418 * x677; const FLT x683 = x598 + (-1 * x682 * x571) + (-1 * x53 * x680) + (-1 * x433 * x678) + (-1 * x652) + x654 + (x681 * x387) + (-1 * x57 * x546) + (x48 * x673) + x653 + (-1 * x87 * x674); const FLT x684 = x677 * x571; const FLT x685 = x57 * x545; const FLT x686 = x59 * x678; const FLT x687 = (x404 * x684) + x548 + (-1 * x674 * x580) + (-1 * x52 * x673) + (-1 * x681 * x399) + (x86 * x685) + x649 + (-1 * x650) + x84 + (-1 * x589) + (-1 * x686 * x387) + (-1 * x679 * x395); const FLT x688 = x656 + (-1 * x597 * x576) + (x404 * x686) + (x672 * x580) + (-1 * x613) + (x684 * x387) + x610 + (-1 * x93 * x685) + (x682 * x573) + (-1 * x52 * x680) + x612; const FLT x689 = x646 + x608 + (-1 * x87 * x672) + (-1 * x48 * x680) + (-1 * x576 * x587) + (x419 * x678) + (-1 * x404 * x681) + (-1 * x684 * x399) + x88 + (x83 * x685); const FLT x690 = (x683 * sensor_pt[1]) + (x688 * sensor_pt[2]) + (-1 * x689 * sensor_pt[0]); const FLT x691 = (x689 * sensor_pt[2]) + (-1 * x687 * sensor_pt[1]) + (x688 * sensor_pt[0]); const FLT x692 = (x441 * x691) + (-1 * x445 * x690) + (-1 * x442 * x683) + (x446 * x687); const FLT x693 = (x687 * sensor_pt[0]) + (-1 * x683 * sensor_pt[2]) + (x688 * sensor_pt[1]); const FLT x694 = (x439 * x690) + (x442 * x689) + (-1 * x414 * x687) + (-1 * x441 * x693); const FLT x695 = (-1 * x439 * x691) + (x445 * x693) + (-1 * x446 * x689) + (x414 * x683); const FLT x696 = (-1 * x41 * x695) + (x39 * x692) + (x444 * x694); const FLT x697 = x32 * ((-1 * x39 * x694) + (x695 * x108) + (x444 * x692)); const FLT x698 = (x696 * x209) + x695 + (-1 * x19 * x697); const FLT x699 = (-1 * x692 * x108) + (x41 * x694) + (x444 * x695); const FLT x700 = x694 + (x697 * x127) + (-1 * x699 * x209); const FLT x701 = x692 + (x699 * x193) + (-1 * x696 * x219); const FLT x702 = (-1 * x137 * ((x701 * x135) + (-1 * ((x698 * x123) + (x700 * x122)) * x126))) + (-1 * ((-1 * x700 * x118) + (x698 * x113)) * x121); const FLT x703 = dt * x26; const FLT x704 = dt * x22; const FLT x705 = x704 + x703; const FLT x706 = -1 * dt * x115; const FLT x707 = -1 * dt * x116; const FLT x708 = x707 + x706 + dt; const FLT x709 = dt * x131; const FLT x710 = dt * x130; const FLT x711 = (-1 * x710) + x709; const FLT x712 = (-1 * x137 * ((x711 * x135) + (-1 * ((x705 * x123) + (x708 * x122)) * x126))) + (-1 * ((-1 * x708 * x118) + (x705 * x113)) * x121); const FLT x713 = dt * x146; const FLT x714 = dt * x145; const FLT x715 = (-1 * x714) + x713; const FLT x716 = x709 + x710; const FLT x717 = (-1 * dt * x151) + dt; const FLT x718 = x717 + x707; const FLT x719 = (-1 * x137 * ((x718 * x135) + (-1 * ((x715 * x123) + (x716 * x122)) * x126))) + (-1 * ((-1 * x716 * x118) + (x715 * x113)) * x121); const FLT x720 = x717 + x706; const FLT x721 = (-1 * x703) + x704; const FLT x722 = x713 + x714; const FLT x723 = (-1 * x137 * ((x722 * x135) + (-1 * ((x720 * x123) + (x721 * x122)) * x126))) + (-1 * ((-1 * x721 * x118) + (x720 * x113)) * x121); const FLT x724 = x124 * x134 * x137; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x138 + (x138 * x141) + (x144 * ((-1 * x118 * x133) + (x28 * x142)))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x154 + (x141 * x154) + (((-1 * x118 * x153) + (x142 * x148)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x159 + (x141 * x159) + (((-1 * x118 * x158) + (x142 * x155)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x248 + (x248 * x141) + (((-1 * x247 * x118) + (x218 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x291 + (x291 * x141) + (((-1 * x290 * x118) + (x280 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x320 + (x320 * x141) + (((-1 * x319 * x118) + (x312 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x345 + (x345 * x141) + (((-1 * x344 * x118) + (x340 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x359 + (x359 * x141) + (((-1 * x358 * x118) + (x351 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x366 + (x366 * x141) + (((-1 * x365 * x118) + (x362 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x370 + (x370 * x141) + (((-1 * x369 * x118) + (x367 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), (x376 * x141) + x376 + (((-1 * x375 * x118) + (x371 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x381 + (x381 * x141) + (((-1 * x380 * x118) + (x377 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x385 + (x385 * x141) + (((-1 * x384 * x118) + (x382 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x457 + (x457 * x141) + (((-1 * x456 * x118) + (x453 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x491 + (x491 * x141) + (((-1 * x490 * x118) + (x487 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x518 + (x518 * x141) + (((-1 * x517 * x118) + (x514 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x543 + (x543 * x141) + (((-1 * x542 * x118) + (x539 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x630 + (x630 * x141) + (((-1 * x629 * x118) + (x626 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x671 + (x671 * x141) + (((-1 * x670 * x118) + (x667 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), (x702 * x141) + x702 + (((-1 * x701 * x118) + (x698 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x712 + (x712 * x141) + (((-1 * x711 * x118) + (x705 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), (x719 * x141) + x719 + (((-1 * x718 * x118) + (x715 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), (x723 * x141) + x723 + (((-1 * x722 * x118) + (x720 * x142)) * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), x143 * x143); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), -1 * cos(x140)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), x141); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1 + (-1 * x141)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), (-1 * x724 * x141) + (-1 * x724)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[0]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x3 = (x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x4 = dt * fabs(dt); const FLT x5 = 1.0/2.0 * x4; const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x7 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x8 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x9 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x10 = (*_x0).Object.Pose.Rot[3] + (x9 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x8 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x7 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x11 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x12 = (-1 * x9 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x7 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x11 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x13 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x14 = (-1 * x9 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x11 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x13 * (*_x0).Object.Pose.Rot[2]); const FLT x15 = (*_x0).Object.Pose.Rot[1] + (x7 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x13 * (*_x0).Object.Pose.Rot[3]) + (x8 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x16 = (x15 * x15) + (x14 * x14) + (x10 * x10) + (x12 * x12); const FLT x17 = 1. / sqrt(x16); const FLT x18 = dt * dt; const FLT x19 = x6 * x6; const FLT x20 = x19 * x18; const FLT x21 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x22 = x21 * x21; const FLT x23 = x22 * x18; const FLT x24 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x25 = x24 * x24; const FLT x26 = x25 * x18; const FLT x27 = 1e-10 + x26 + x20 + x23; const FLT x28 = sqrt(x27); const FLT x29 = 0.5 * x28; const FLT x30 = sin(x29); const FLT x31 = x30 * (1. / x28); const FLT x32 = dt * x31; const FLT x33 = x32 * x17; const FLT x34 = x30 * x30; const FLT x35 = 1. / x27; const FLT x36 = x34 * x35; const FLT x37 = cos(x29); const FLT x38 = (x36 * x23) + (x37 * x37) + (x36 * x20) + (x36 * x26); const FLT x39 = 1. / sqrt(x38); const FLT x40 = x39 * x14; const FLT x41 = x40 * x33; const FLT x42 = x39 * x15; const FLT x43 = x42 * x33; const FLT x44 = x37 * x17; const FLT x45 = x39 * x10; const FLT x46 = x44 * x45; const FLT x47 = x39 * x12; const FLT x48 = x47 * x33; const FLT x49 = x46 + (x48 * x21) + (x6 * x41) + (-1 * x43 * x24); const FLT x50 = x45 * x17; const FLT x51 = x50 * x32; const FLT x52 = x44 * x39; const FLT x53 = x52 * x14; const FLT x54 = (-1 * x43 * x21) + (-1 * x6 * x51) + x53 + (-1 * x48 * x24); const FLT x55 = x52 * x15; const FLT x56 = (x41 * x21) + x55 + (-1 * x6 * x48) + (x51 * x24); const FLT x57 = (-1 * x56 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x49 * sensor_pt[0]); const FLT x58 = x52 * x12; const FLT x59 = (x6 * x43) + (x41 * x24) + (-1 * x51 * x21) + x58; const FLT x60 = (-1 * x59 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x56 * sensor_pt[1]); const FLT x61 = (*_x0).Object.Pose.Pos[0] + (x5 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x60 * x59) + (-1 * x57 * x49))); const FLT x62 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x63 = (*_x0).Lighthouse.Rot[0] + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (-1 * x62 * (*_x0).Lighthouse.Rot[2]); const FLT x64 = (x62 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]); const FLT x65 = x64 * x64; const FLT x66 = x63 * x63; const FLT x67 = x3 * x3; const FLT x68 = (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x62 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[2]); const FLT x69 = x68 * x68; const FLT x70 = x69 + x67 + x65 + x66; const FLT x71 = 1. / x70; const FLT x72 = x71 * x65; const FLT x73 = x71 * x67; const FLT x74 = x71 * x69; const FLT x75 = x74 + (x71 * x66) + x72 + x73; const FLT x76 = 1. / sqrt(x75); const FLT x77 = 1. / sqrt(x70); const FLT x78 = x77 * x76; const FLT x79 = x78 * x63; const FLT x80 = (-1 * x49 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x59 * sensor_pt[2]); const FLT x81 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x57 * x56) + (-1 * x80 * x59))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x5 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x82 = x3 * x78; const FLT x83 = (*_x0).Object.Pose.Pos[1] + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (x5 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (2 * ((x80 * x49) + (-1 * x60 * x56))); const FLT x84 = x78 * x64; const FLT x85 = (x83 * x84) + (x79 * x61) + (-1 * x81 * x82); const FLT x86 = x3 * x85; const FLT x87 = 1.0 * x68; const FLT x88 = x87 * (*_x0).Lighthouse.Rot[0]; const FLT x89 = 1.0 * x3; const FLT x90 = x89 * (*_x0).Lighthouse.Rot[3]; const FLT x91 = 1.0 * x64; const FLT x92 = x91 * (*_x0).Lighthouse.Rot[2]; const FLT x93 = 1.0 * x63; const FLT x94 = x93 * (*_x0).Lighthouse.Rot[1]; const FLT x95 = (-1 * x94) + (-1 * x92) + x88 + x90; const FLT x96 = (1. / (x70 * sqrt(x70))) * x76; const FLT x97 = x96 * x95; const FLT x98 = 1.0/2.0 * x83; const FLT x99 = x64 * x97; const FLT x100 = 1.0/2.0 * x61; const FLT x101 = x63 * x97; const FLT x102 = 1.0/2.0 * x81; const FLT x103 = x97 * x102; const FLT x104 = 1. / (x70 * x70); const FLT x105 = x95 * x104; const FLT x106 = (1. / (x75 * sqrt(x75))) * x77; const FLT x107 = ((-1 * x71 * x94) + (-1 * x67 * x105) + (-1 * x71 * x92) + (x88 * x71) + (x71 * x90) + (-1 * x66 * x105) + (-1 * x65 * x105) + (-1 * x69 * x105)) * x106; const FLT x108 = x63 * x107; const FLT x109 = x98 * x107; const FLT x110 = x3 * x107; const FLT x111 = 0.5 * x78; const FLT x112 = x61 * x111; const FLT x113 = x81 * x78; const FLT x114 = 0.5 * x113; const FLT x115 = x83 * x111; const FLT x116 = (-1 * x115 * (*_x0).Lighthouse.Rot[2]) + (-1 * x112 * (*_x0).Lighthouse.Rot[1]) + (-1 * x114 * (*_x0).Lighthouse.Rot[3]); const FLT x117 = (x102 * x110) + (-1 * x101 * x100) + (-1 * x99 * x98) + x116 + (x3 * x103) + (-1 * x64 * x109) + (-1 * x100 * x108); const FLT x118 = 2 * x82; const FLT x119 = x78 * x68; const FLT x120 = (x81 * x119) + (x83 * x79) + (-1 * x84 * x61); const FLT x121 = x68 * x120; const FLT x122 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x123 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x124 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x125 = (x119 * x124) + (x79 * x122) + (-1 * x84 * x123); const FLT x126 = x107 * x125; const FLT x127 = x97 * x125; const FLT x128 = 1.0/2.0 * x122; const FLT x129 = x64 * x107; const FLT x130 = 1.0/2.0 * x123; const FLT x131 = 1.0/2.0 * x124; const FLT x132 = x97 * x128; const FLT x133 = x97 * x131; const FLT x134 = x111 * x123; const FLT x135 = x111 * (*_x0).Lighthouse.Rot[2]; const FLT x136 = x111 * (*_x0).Lighthouse.Rot[3]; const FLT x137 = (-1 * x124 * x136) + (-1 * x134 * (*_x0).Lighthouse.Rot[1]) + (-1 * x122 * x135); const FLT x138 = x137 + (-1 * x101 * x130) + (-1 * x108 * x130) + (-1 * x128 * x129) + (x110 * x131) + (-1 * x64 * x132) + (x3 * x133); const FLT x139 = (x84 * x122) + (-1 * x82 * x124) + (x79 * x123); const FLT x140 = x107 * x139; const FLT x141 = x97 * x139; const FLT x142 = 1.0 * x78; const FLT x143 = x85 * x142; const FLT x144 = x120 * x142; const FLT x145 = x144 * (*_x0).Lighthouse.Rot[0]; const FLT x146 = x125 * x142; const FLT x147 = x146 * (*_x0).Lighthouse.Rot[0]; const FLT x148 = x1 * x113; const FLT x149 = x115 * (*_x0).Lighthouse.Rot[1]; const FLT x150 = x112 * (*_x0).Lighthouse.Rot[2]; const FLT x151 = x68 * x107; const FLT x152 = (-1 * x102 * x151) + (x100 * x129) + x148 + (-1 * x68 * x103) + (-1 * x63 * x109) + (-1 * x149) + x150 + (x99 * x100) + (-1 * x98 * x101); const FLT x153 = 2 * x119; const FLT x154 = x1 * x78; const FLT x155 = x124 * x154; const FLT x156 = x111 * (*_x0).Lighthouse.Rot[1]; const FLT x157 = x122 * x156; const FLT x158 = x134 * (*_x0).Lighthouse.Rot[2]; const FLT x159 = x158 + (x129 * x130) + (-1 * x157) + (x99 * x130) + (-1 * x68 * x133) + x155 + (-1 * x108 * x128) + (-1 * x101 * x128) + (-1 * x131 * x151); const FLT x160 = x139 * x142; const FLT x161 = (x153 * x159) + (-1 * x152 * x153) + (-1 * x86 * x107) + (-1 * x68 * x127) + (x3 * x141) + (-1 * x160 * (*_x0).Lighthouse.Rot[3]) + (x97 * x121) + (x107 * x121) + x147 + (-1 * x68 * x126) + (-1 * x86 * x97) + (x3 * x140) + (x118 * x117) + (-1 * x118 * x138) + (x143 * (*_x0).Lighthouse.Rot[3]) + (-1 * x145); const FLT x162 = (x82 * x123) + (x79 * x124) + (-1 * x119 * x122); const FLT x163 = (x82 * x61) + (x81 * x79) + (-1 * x83 * x119); const FLT x164 = x61 + (-1 * (x123 + (2 * ((-1 * x82 * x162) + (x84 * x125))))) + (2 * ((-1 * x82 * x163) + (x84 * x120))); const FLT x165 = (-1 * (x124 + (2 * ((-1 * x119 * x125) + (x82 * x139))))) + x81 + (2 * ((-1 * x119 * x120) + (x82 * x85))); const FLT x166 = x165 * x165; const FLT x167 = 1. / x166; const FLT x168 = x167 * x164; const FLT x169 = x3 * x163; const FLT x170 = x112 * (*_x0).Lighthouse.Rot[3]; const FLT x171 = x114 * (*_x0).Lighthouse.Rot[1]; const FLT x172 = x83 * x154; const FLT x173 = (-1 * x172) + (x68 * x109) + (-1 * x102 * x108) + x170 + (-1 * x100 * x110) + (-1 * x3 * x97 * x100) + (-1 * x101 * x102) + (x68 * x98 * x97) + (-1 * x171); const FLT x174 = x64 * x120; const FLT x175 = x3 * x162; const FLT x176 = 2 * x84; const FLT x177 = x134 * (*_x0).Lighthouse.Rot[3]; const FLT x178 = x124 * x156; const FLT x179 = x122 * x154; const FLT x180 = x3 * x130; const FLT x181 = (-1 * x179) + (-1 * x97 * x180) + (-1 * x178) + (-1 * x108 * x131) + x177 + (-1 * x63 * x133) + (x128 * x151) + (-1 * x110 * x130) + (x68 * x132); const FLT x182 = (x146 * (*_x0).Lighthouse.Rot[2]) + (-1 * x144 * (*_x0).Lighthouse.Rot[2]); const FLT x183 = x163 * x142; const FLT x184 = x162 * x142; const FLT x185 = (x184 * (*_x0).Lighthouse.Rot[3]) + (-1 * x183 * (*_x0).Lighthouse.Rot[3]); const FLT x186 = x182 + (-1 * x97 * x175) + x185 + (-1 * x107 * x175) + (x64 * x127) + (-1 * x97 * x174) + (x107 * x169) + (-1 * x118 * x173) + (x176 * x152) + (-1 * x176 * x159) + (x97 * x169) + (x118 * x181) + (-1 * x107 * x174) + (x64 * x126); const FLT x187 = 1. / x165; const FLT x188 = (x164 * x164) + x166; const FLT x189 = 1. / x188; const FLT x190 = x166 * x189; const FLT x191 = x83 + (2 * ((-1 * x84 * x85) + (x119 * x163))) + (-1 * (x122 + (2 * ((-1 * x84 * x139) + (x119 * x162))))); const FLT x192 = x191 * x191; const FLT x193 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x194 = 1. / sqrt(1 + (-1 * x189 * x192 * (x193 * x193))); const FLT x195 = 2 * x164; const FLT x196 = 2 * x165; const FLT x197 = 1.0/2.0 * (1. / (x188 * sqrt(x188))) * x191 * x193; const FLT x198 = x183 * (*_x0).Lighthouse.Rot[0]; const FLT x199 = x85 * x64; const FLT x200 = x68 * x163; const FLT x201 = x68 * x162; const FLT x202 = x184 * (*_x0).Lighthouse.Rot[0]; const FLT x203 = (x97 * x201) + (-1 * x117 * x176) + (-1 * x200 * x107) + (-1 * x202) + x198 + (-1 * x181 * x153) + (-1 * x97 * x200) + (x173 * x153) + (-1 * x64 * x141) + (-1 * x64 * x140) + (x176 * x138) + (-1 * x160 * (*_x0).Lighthouse.Rot[2]) + (x107 * x199) + (x97 * x199) + (x143 * (*_x0).Lighthouse.Rot[2]) + (x201 * x107); const FLT x204 = 1. / sqrt(x188); const FLT x205 = x204 * x193; const FLT x206 = (-1 * x194 * ((x203 * x205) + (-1 * ((x161 * x196) + (x186 * x195)) * x197))) + (-1 * ((-1 * x187 * x186) + (x161 * x168)) * x190); const FLT x207 = -1 * x165; const FLT x208 = 1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * atan2(x164, x207)) + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * asin(x205 * x191)); const FLT x209 = sin(x208) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag); const FLT x210 = x167 * x191; const FLT x211 = atan2(x191, x207); const FLT x212 = 2 * (1. / (x166 + x192)) * x211 * x166 * ((*error_model).BSD0.curve + (*_x0).BSD0.curve); const FLT x213 = x87 * (*_x0).Lighthouse.Rot[3]; const FLT x214 = x89 * (*_x0).Lighthouse.Rot[0]; const FLT x215 = x91 * (*_x0).Lighthouse.Rot[1]; const FLT x216 = x93 * (*_x0).Lighthouse.Rot[2]; const FLT x217 = (-1 * x216) + x215 + (-1 * x213) + x214; const FLT x218 = x217 * x104; const FLT x219 = ((-1 * x67 * x218) + (x71 * x214) + (x71 * x215) + (-1 * x65 * x218) + (-1 * x69 * x218) + (-1 * x71 * x213) + (-1 * x71 * x216) + (-1 * x66 * x218)) * x106; const FLT x220 = x143 * (*_x0).Lighthouse.Rot[0]; const FLT x221 = x96 * x217; const FLT x222 = x64 * x221; const FLT x223 = x98 * x219; const FLT x224 = x3 * x221; const FLT x225 = x63 * x221; const FLT x226 = x219 * x102; const FLT x227 = x219 * x100; const FLT x228 = (-1 * x63 * x227) + (x3 * x226) + (-1 * x98 * x222) + (-1 * x148) + (-1 * x64 * x223) + x149 + (x224 * x102) + (-1 * x150) + (-1 * x225 * x100); const FLT x229 = x221 * x125; const FLT x230 = x68 * x221; const FLT x231 = x64 * x219; const FLT x232 = (x231 * x100) + (-1 * x63 * x223) + x116 + (x222 * x100) + (-1 * x230 * x102) + (-1 * x98 * x225) + (-1 * x68 * x226); const FLT x233 = x221 * x139; const FLT x234 = x160 * (*_x0).Lighthouse.Rot[0]; const FLT x235 = x219 * x131; const FLT x236 = x219 * x128; const FLT x237 = (x222 * x130) + (-1 * x230 * x131) + (x231 * x130) + (-1 * x68 * x235) + (-1 * x63 * x236) + x137 + (-1 * x225 * x128); const FLT x238 = x219 * x125; const FLT x239 = x219 * x139; const FLT x240 = (-1 * x63 * x219 * x130) + (x224 * x131) + x157 + (-1 * x158) + (-1 * x222 * x128) + (-1 * x225 * x130) + (x3 * x235) + (-1 * x155) + (-1 * x231 * x128); const FLT x241 = x221 * x120; const FLT x242 = (-1 * x86 * x221) + (x3 * x239) + (-1 * x68 * x229) + (-1 * x146 * (*_x0).Lighthouse.Rot[3]) + (-1 * x234) + (x144 * (*_x0).Lighthouse.Rot[3]) + (x228 * x118) + (x237 * x153) + (-1 * x86 * x219) + (x219 * x121) + (x3 * x233) + (x68 * x241) + (-1 * x240 * x118) + x220 + (-1 * x68 * x238) + (-1 * x232 * x153); const FLT x243 = x124 * x135; const FLT x244 = x122 * x136; const FLT x245 = x123 * x154; const FLT x246 = x245 + (-1 * x225 * x131) + x244 + (-1 * x219 * x180) + (-1 * x224 * x130) + (x230 * x128) + (x68 * x236) + (-1 * x63 * x235) + (-1 * x243); const FLT x247 = x61 * x154; const FLT x248 = x114 * (*_x0).Lighthouse.Rot[2]; const FLT x249 = x115 * (*_x0).Lighthouse.Rot[3]; const FLT x250 = (-1 * x225 * x102) + (x68 * x223) + x249 + (-1 * x224 * x100) + (-1 * x3 * x227) + x247 + (x98 * x230) + (-1 * x248) + (-1 * x63 * x226); const FLT x251 = x219 * x163; const FLT x252 = (x3 * x251) + (x64 * x238) + (-1 * x146 * (*_x0).Lighthouse.Rot[1]) + (x144 * (*_x0).Lighthouse.Rot[1]) + (-1 * x221 * x175) + (-1 * x198) + (x64 * x229) + (x232 * x176) + (-1 * x219 * x175) + (x246 * x118) + (-1 * x219 * x174) + (x221 * x169) + (-1 * x237 * x176) + x202 + (-1 * x64 * x241) + (-1 * x250 * x118); const FLT x253 = (x160 * (*_x0).Lighthouse.Rot[1]) + (-1 * x143 * (*_x0).Lighthouse.Rot[1]); const FLT x254 = (x219 * x201) + (-1 * x200 * x221) + x185 + (x250 * x153) + (-1 * x64 * x233) + x253 + (-1 * x228 * x176) + (x201 * x221) + (x221 * x199) + (-1 * x246 * x153) + (-1 * x64 * x239) + (x219 * x199) + (-1 * x68 * x251) + (x240 * x176); const FLT x255 = (-1 * x194 * ((x205 * x254) + (-1 * ((x242 * x196) + (x252 * x195)) * x197))) + (-1 * ((-1 * x252 * x187) + (x242 * x168)) * x190); const FLT x256 = x87 * (*_x0).Lighthouse.Rot[2]; const FLT x257 = x89 * (*_x0).Lighthouse.Rot[1]; const FLT x258 = x91 * (*_x0).Lighthouse.Rot[0]; const FLT x259 = x93 * (*_x0).Lighthouse.Rot[3]; const FLT x260 = x258 + (-1 * x259) + x256 + (-1 * x257); const FLT x261 = x260 * x104; const FLT x262 = ((-1 * x71 * x259) + (x71 * x256) + (-1 * x71 * x257) + (-1 * x67 * x261) + (-1 * x65 * x261) + (-1 * x66 * x261) + (-1 * x69 * x261) + (x71 * x258)) * x106; const FLT x263 = x64 * x262; const FLT x264 = x96 * x260; const FLT x265 = x3 * x264; const FLT x266 = x63 * x262; const FLT x267 = x64 * x264; const FLT x268 = x63 * x264; const FLT x269 = x3 * x262; const FLT x270 = (x269 * x102) + (-1 * x170) + (-1 * x268 * x100) + x171 + (-1 * x266 * x100) + (x265 * x102) + x172 + (-1 * x98 * x263) + (-1 * x98 * x267); const FLT x271 = x264 * x139; const FLT x272 = (x265 * x131) + (-1 * x263 * x128) + x178 + (x269 * x131) + x179 + (-1 * x177) + (-1 * x268 * x130) + (-1 * x266 * x130) + (-1 * x267 * x128); const FLT x273 = x262 * x120; const FLT x274 = x68 * x262; const FLT x275 = x68 * x264; const FLT x276 = x243 + (x263 * x130) + (-1 * x266 * x128) + (-1 * x268 * x128) + (-1 * x245) + (-1 * x274 * x131) + (-1 * x244) + (x267 * x130) + (-1 * x275 * x131); const FLT x277 = x264 * x125; const FLT x278 = x262 * x139; const FLT x279 = x262 * x125; const FLT x280 = x264 * x120; const FLT x281 = (-1 * x98 * x268) + (-1 * x98 * x266) + (-1 * x249) + (x267 * x100) + (-1 * x275 * x102) + (-1 * x274 * x102) + (-1 * x247) + (x263 * x100) + x248; const FLT x282 = (-1 * x281 * x153) + (-1 * x272 * x118) + (x68 * x273) + (x3 * x271) + (-1 * x86 * x262) + x182 + (-1 * x86 * x264) + (x270 * x118) + (x276 * x153) + (-1 * x68 * x277) + x253 + (x68 * x280) + (x3 * x278) + (-1 * x68 * x279); const FLT x283 = x116 + (x98 * x275) + (-1 * x269 * x100) + (x98 * x274) + (-1 * x268 * x102) + (-1 * x266 * x102) + (-1 * x265 * x100); const FLT x284 = x137 + (-1 * x269 * x130) + (-1 * x268 * x131) + (-1 * x265 * x130) + (x275 * x128) + (x274 * x128) + (-1 * x266 * x131); const FLT x285 = (x64 * x277) + (-1 * x147) + x145 + (x264 * x169) + (x281 * x176) + (-1 * x283 * x118) + (x183 * (*_x0).Lighthouse.Rot[1]) + (-1 * x64 * x280) + (-1 * x264 * x175) + (x262 * x169) + (-1 * x262 * x175) + (x64 * x279) + (-1 * x184 * (*_x0).Lighthouse.Rot[1]) + (x284 * x118) + (-1 * x64 * x273) + (-1 * x276 * x176); const FLT x286 = (-1 * x200 * x264) + (x262 * x199) + (-1 * x270 * x176) + (-1 * x184 * (*_x0).Lighthouse.Rot[2]) + (x201 * x262) + (x183 * (*_x0).Lighthouse.Rot[2]) + (x264 * x199) + (-1 * x284 * x153) + (x272 * x176) + (x201 * x264) + x234 + (-1 * x200 * x262) + (-1 * x220) + (-1 * x64 * x278) + (-1 * x64 * x271) + (x283 * x153); const FLT x287 = (-1 * x194 * ((x205 * x286) + (-1 * ((x282 * x196) + (x285 * x195)) * x197))) + (-1 * ((-1 * x285 * x187) + (x282 * x168)) * x190); const FLT x288 = 1. / x75; const FLT x289 = 2 * x288; const FLT x290 = x71 * x289; const FLT x291 = x64 * x68; const FLT x292 = x291 * x290; const FLT x293 = -1 * x292; const FLT x294 = x63 * x290; const FLT x295 = x3 * x294; const FLT x296 = -1 * x295; const FLT x297 = x296 + x293; const FLT x298 = x73 * x289; const FLT x299 = x72 * x289; const FLT x300 = -1 + x299 + x298; const FLT x301 = x64 * x294; const FLT x302 = x3 * x290; const FLT x303 = x68 * x302; const FLT x304 = -1 * x303; const FLT x305 = x304 + x301; const FLT x306 = (-1 * x194 * ((x205 * x305) + (-1 * ((x297 * x196) + (x300 * x195)) * x197))) + (-1 * ((-1 * x300 * x187) + (x297 * x168)) * x190); const FLT x307 = x68 * x294; const FLT x308 = x64 * x302; const FLT x309 = -1 * x308; const FLT x310 = x309 + x307; const FLT x311 = -1 * x301; const FLT x312 = x311 + x304; const FLT x313 = x74 * x289; const FLT x314 = -1 + x313; const FLT x315 = x314 + x299; const FLT x316 = (-1 * x194 * ((x205 * x315) + (-1 * ((x310 * x196) + (x312 * x195)) * x197))) + (-1 * ((-1 * x312 * x187) + (x310 * x168)) * x190); const FLT x317 = x314 + x298; const FLT x318 = x293 + x295; const FLT x319 = -1 * x307; const FLT x320 = x319 + x309; const FLT x321 = (-1 * x194 * ((x205 * x320) + (-1 * ((x317 * x196) + (x318 * x195)) * x197))) + (-1 * ((-1 * x318 * x187) + (x317 * x168)) * x190); const FLT x322 = x4 * x288; const FLT x323 = x71 * x322; const FLT x324 = x3 * x323; const FLT x325 = x63 * x324; const FLT x326 = x291 * x323; const FLT x327 = x326 + x325; const FLT x328 = -1 * x73 * x322; const FLT x329 = (-1 * x72 * x322) + x5; const FLT x330 = x329 + x328; const FLT x331 = x68 * x323; const FLT x332 = x3 * x331; const FLT x333 = x63 * x64 * x323; const FLT x334 = (-1 * x333) + x332; const FLT x335 = (-1 * x194 * ((x205 * x334) + (-1 * ((x327 * x196) + (x330 * x195)) * x197))) + (-1 * ((-1 * x330 * x187) + (x327 * x168)) * x190); const FLT x336 = x64 * x324; const FLT x337 = x63 * x331; const FLT x338 = (-1 * x337) + x336; const FLT x339 = x332 + x333; const FLT x340 = -1 * x74 * x322; const FLT x341 = x329 + x340; const FLT x342 = (-1 * x194 * ((x205 * x341) + (-1 * ((x338 * x196) + (x339 * x195)) * x197))) + (-1 * ((-1 * x339 * x187) + (x338 * x168)) * x190); const FLT x343 = x340 + x328 + x5; const FLT x344 = (-1 * x325) + x326; const FLT x345 = x336 + x337; const FLT x346 = (-1 * x194 * ((x205 * x345) + (-1 * ((x343 * x196) + (x344 * x195)) * x197))) + (-1 * ((-1 * x344 * x187) + (x343 * x168)) * x190); const FLT x347 = 1.0 * x15; const FLT x348 = 1.0 * x14; const FLT x349 = 1.0 * x10; const FLT x350 = 1.0 * (*_x0).Object.Pose.Rot[3]; const FLT x351 = (x12 * x350) + (-1 * x349 * (*_x0).Object.Pose.Rot[2]) + (x347 * (*_x0).Object.Pose.Rot[0]) + (-1 * x348 * (*_x0).Object.Pose.Rot[1]); const FLT x352 = 1. / (x16 * sqrt(x16)); const FLT x353 = x37 * x352; const FLT x354 = x351 * x353; const FLT x355 = 1.0/2.0 * x354; const FLT x356 = 1.0/2.0 * x14; const FLT x357 = x39 * x356; const FLT x358 = x32 * x352; const FLT x359 = x358 * x357; const FLT x360 = x359 * x351; const FLT x361 = 1.0/2.0 * x47; const FLT x362 = x351 * x358; const FLT x363 = x6 * x362; const FLT x364 = 1.0/2.0 * x45; const FLT x365 = x24 * x362; const FLT x366 = x33 * x39; const FLT x367 = x6 * x366; const FLT x368 = x24 * x366; const FLT x369 = x21 * x366; const FLT x370 = (-1 * x9 * x369) + (x7 * x52) + (-1 * x11 * x367) + (-1 * x8 * x368); const FLT x371 = (-1 * x364 * x365) + (x361 * x363) + x370 + (-1 * x42 * x355) + (-1 * x21 * x360); const FLT x372 = 2 * x60; const FLT x373 = x21 * x362; const FLT x374 = 1.0/2.0 * x42; const FLT x375 = x11 * x369; const FLT x376 = x7 * x368; const FLT x377 = x9 * x367; const FLT x378 = x8 * x52; const FLT x379 = (-1 * x378) + (-1 * x377) + x375 + (-1 * x376); const FLT x380 = x379 + (-1 * x45 * x355) + (x374 * x365) + (-1 * x373 * x361) + (-1 * x6 * x360); const FLT x381 = x7 * x369; const FLT x382 = x8 * x367; const FLT x383 = x11 * x368; const FLT x384 = x9 * x52; const FLT x385 = (-1 * x384) + (-1 * x383) + (-1 * x381) + x382; const FLT x386 = x385 + (x361 * x365) + (x374 * x373) + (x363 * x364) + (-1 * x354 * x357); const FLT x387 = x8 * x369; const FLT x388 = x9 * x368; const FLT x389 = x7 * x367; const FLT x390 = x52 * x11; const FLT x391 = (-1 * x47 * x355) + x390 + (-1 * x24 * x360) + (x373 * x364) + x387 + x389 + (-1 * x374 * x363) + (-1 * x388); const FLT x392 = (x391 * sensor_pt[2]) + (-1 * x380 * sensor_pt[1]) + (x386 * sensor_pt[0]); const FLT x393 = 2 * x49; const FLT x394 = 2 * x80; const FLT x395 = (x371 * sensor_pt[1]) + (-1 * x391 * sensor_pt[0]) + (x386 * sensor_pt[2]); const FLT x396 = 2 * x56; const FLT x397 = (x394 * x380) + (-1 * x372 * x371) + (-1 * x395 * x396) + (x393 * x392); const FLT x398 = 2 * x57; const FLT x399 = 2 * x59; const FLT x400 = (-1 * x371 * sensor_pt[2]) + (x380 * sensor_pt[0]) + (x386 * sensor_pt[1]); const FLT x401 = (x372 * x391) + (-1 * x400 * x393) + (-1 * x398 * x380) + (x399 * x395); const FLT x402 = (x371 * x398) + (x400 * x396) + (-1 * x394 * x391) + (-1 * x399 * x392); const FLT x403 = (-1 * x82 * x402) + (x84 * x397) + (x79 * x401); const FLT x404 = (-1 * x84 * x401) + (x79 * x397) + (x402 * x119); const FLT x405 = x402 + (x403 * x118) + (-1 * x404 * x153); const FLT x406 = (-1 * x397 * x119) + (x82 * x401) + (x79 * x402); const FLT x407 = x401 + (x404 * x176) + (-1 * x406 * x118); const FLT x408 = x397 + (x406 * x153) + (-1 * x403 * x176); const FLT x409 = (-1 * x194 * ((x408 * x205) + (-1 * ((x405 * x196) + (x407 * x195)) * x197))) + (-1 * ((-1 * x407 * x187) + (x405 * x168)) * x190); const FLT x410 = 1.0 * x12; const FLT x411 = (x410 * (*_x0).Object.Pose.Rot[0]) + (-1 * x348 * (*_x0).Object.Pose.Rot[2]) + (-1 * x15 * x350) + (x349 * (*_x0).Object.Pose.Rot[1]); const FLT x412 = x21 * x411; const FLT x413 = x6 * x411; const FLT x414 = x413 * x358; const FLT x415 = x411 * x353; const FLT x416 = 1.0/2.0 * x415; const FLT x417 = x364 * x358; const FLT x418 = x24 * x411; const FLT x419 = x388 + (-1 * x387) + (-1 * x390) + (-1 * x389); const FLT x420 = x419 + (-1 * x412 * x359) + (-1 * x417 * x418) + (-1 * x42 * x416) + (x414 * x361); const FLT x421 = x361 * x358; const FLT x422 = x374 * x358; const FLT x423 = (x418 * x422) + (-1 * x382) + x384 + (-1 * x45 * x416) + (-1 * x412 * x421) + (-1 * x413 * x359) + x383 + x381; const FLT x424 = (x418 * x421) + x379 + (-1 * x415 * x357) + (x412 * x422) + (x413 * x417); const FLT x425 = x370 + (-1 * x415 * x361) + (-1 * x418 * x359) + (x412 * x417) + (-1 * x414 * x374); const FLT x426 = (x425 * sensor_pt[2]) + (-1 * x423 * sensor_pt[1]) + (x424 * sensor_pt[0]); const FLT x427 = (x420 * sensor_pt[1]) + (-1 * x425 * sensor_pt[0]) + (x424 * sensor_pt[2]); const FLT x428 = (x426 * x393) + (-1 * x427 * x396) + (-1 * x420 * x372) + (x423 * x394); const FLT x429 = (x423 * sensor_pt[0]) + (-1 * x420 * sensor_pt[2]) + (x424 * sensor_pt[1]); const FLT x430 = (x427 * x399) + (x425 * x372) + (-1 * x423 * x398) + (-1 * x429 * x393); const FLT x431 = (x420 * x398) + (-1 * x426 * x399) + (-1 * x425 * x394) + (x429 * x396); const FLT x432 = (-1 * x82 * x431) + (x84 * x428) + (x79 * x430); const FLT x433 = (-1 * x84 * x430) + (x431 * x119) + (x79 * x428); const FLT x434 = (x432 * x118) + x431 + (-1 * x433 * x153); const FLT x435 = (-1 * x428 * x119) + (x79 * x431) + (x82 * x430); const FLT x436 = x430 + (x433 * x176) + (-1 * x435 * x118); const FLT x437 = (x435 * x153) + x428 + (-1 * x432 * x176); const FLT x438 = (-1 * x194 * ((x437 * x205) + (-1 * ((x434 * x196) + (x436 * x195)) * x197))) + (-1 * ((-1 * x436 * x187) + (x434 * x168)) * x190); const FLT x439 = (-1 * x410 * (*_x0).Object.Pose.Rot[1]) + (-1 * x14 * x350) + (x347 * (*_x0).Object.Pose.Rot[2]) + (x349 * (*_x0).Object.Pose.Rot[0]); const FLT x440 = x21 * x439; const FLT x441 = x439 * x353; const FLT x442 = 1.0/2.0 * x441; const FLT x443 = x6 * x439; const FLT x444 = x24 * x439; const FLT x445 = (-1 * x375) + (-1 * x42 * x442) + (-1 * x440 * x359) + (-1 * x417 * x444) + x376 + (x421 * x443) + x377 + x378; const FLT x446 = x440 * x358; const FLT x447 = 1.0/2.0 * x446; const FLT x448 = x444 * x358; const FLT x449 = (x448 * x361) + x419 + (x417 * x443) + (-1 * x441 * x357) + (x42 * x447); const FLT x450 = x385 + (-1 * x444 * x359) + (-1 * x47 * x442) + (-1 * x422 * x443) + (x45 * x447); const FLT x451 = (x445 * sensor_pt[1]) + (x449 * sensor_pt[2]) + (-1 * x450 * sensor_pt[0]); const FLT x452 = x370 + (x448 * x374) + (-1 * x443 * x359) + (-1 * x45 * x442) + (-1 * x446 * x361); const FLT x453 = (x450 * sensor_pt[2]) + (-1 * x452 * sensor_pt[1]) + (x449 * sensor_pt[0]); const FLT x454 = (x453 * x393) + (x452 * x394) + (-1 * x445 * x372) + (-1 * x451 * x396); const FLT x455 = (x452 * sensor_pt[0]) + (-1 * x445 * sensor_pt[2]) + (x449 * sensor_pt[1]); const FLT x456 = (x450 * x372) + (-1 * x452 * x398) + (x451 * x399) + (-1 * x455 * x393); const FLT x457 = (x455 * x396) + (-1 * x450 * x394) + (x445 * x398) + (-1 * x453 * x399); const FLT x458 = (-1 * x82 * x457) + (x84 * x454) + (x79 * x456); const FLT x459 = (-1 * x84 * x456) + (x457 * x119) + (x79 * x454); const FLT x460 = x457 + (x458 * x118) + (-1 * x459 * x153); const FLT x461 = (-1 * x454 * x119) + (x82 * x456) + (x79 * x457); const FLT x462 = (x459 * x176) + x456 + (-1 * x461 * x118); const FLT x463 = (x461 * x153) + x454 + (-1 * x458 * x176); const FLT x464 = (-1 * x194 * ((x463 * x205) + (-1 * ((x460 * x196) + (x462 * x195)) * x197))) + (-1 * ((-1 * x462 * x187) + (x460 * x168)) * x190); const FLT x465 = x292 + x295; const FLT x466 = -1 * x299; const FLT x467 = -1 * x298; const FLT x468 = 1 + x467 + x466; const FLT x469 = x311 + x303; const FLT x470 = (-1 * x194 * ((x469 * x205) + (-1 * ((x465 * x196) + (x468 * x195)) * x197))) + (-1 * ((-1 * x468 * x187) + (x465 * x168)) * x190); const FLT x471 = x319 + x308; const FLT x472 = x303 + x301; const FLT x473 = 1 + (-1 * x313); const FLT x474 = x473 + x466; const FLT x475 = (-1 * x194 * ((x474 * x205) + (-1 * ((x471 * x196) + (x472 * x195)) * x197))) + (-1 * ((-1 * x472 * x187) + (x471 * x168)) * x190); const FLT x476 = x473 + x467; const FLT x477 = x296 + x292; const FLT x478 = x308 + x307; const FLT x479 = (-1 * x194 * ((x478 * x205) + (-1 * ((x476 * x196) + (x477 * x195)) * x197))) + (-1 * ((-1 * x477 * x187) + (x476 * x168)) * x190); const FLT x480 = dt * dt * dt; const FLT x481 = 0.5 * x35 * x480; const FLT x482 = x21 * x481; const FLT x483 = x6 * x482; const FLT x484 = x58 * x483; const FLT x485 = x24 * x482; const FLT x486 = x46 * x485; const FLT x487 = x22 * x481; const FLT x488 = 1.0/2.0 * x10; const FLT x489 = x21 * x21 * x21; const FLT x490 = dt * dt * dt * dt; const FLT x491 = 2 * x34 * (1. / (x27 * x27)); const FLT x492 = x490 * x491; const FLT x493 = x30 * (1. / (x27 * sqrt(x27))); const FLT x494 = 1.0 * x37; const FLT x495 = x494 * x493; const FLT x496 = x495 * x490; const FLT x497 = x21 * x19; const FLT x498 = x25 * x492; const FLT x499 = 2 * x36; const FLT x500 = x18 * x499; const FLT x501 = x25 * x496; const FLT x502 = x31 * x494; const FLT x503 = x18 * x502; const FLT x504 = (-1 * x21 * x503) + (x21 * x501) + (x496 * x489) + (x496 * x497) + (-1 * x492 * x489) + (x21 * x500) + (-1 * x21 * x498) + (-1 * x497 * x492); const FLT x505 = 1. / (x38 * sqrt(x38)); const FLT x506 = x33 * x505; const FLT x507 = x504 * x506; const FLT x508 = x24 * x507; const FLT x509 = 1.0/2.0 * x12; const FLT x510 = x6 * x509; const FLT x511 = x42 * x17; const FLT x512 = 0.5 * x31; const FLT x513 = x18 * x512; const FLT x514 = x21 * x513; const FLT x515 = x44 * x505; const FLT x516 = 1.0/2.0 * x515; const FLT x517 = x516 * x504; const FLT x518 = x493 * x480; const FLT x519 = x24 * x518; const FLT x520 = x21 * x519; const FLT x521 = x50 * x520; const FLT x522 = x21 * x507; const FLT x523 = x47 * x17; const FLT x524 = x6 * x523; const FLT x525 = x21 * x518; const FLT x526 = x524 * x525; const FLT x527 = x22 * x518; const FLT x528 = x40 * x17; const FLT x529 = (-1 * x528 * x527) + x526 + x41 + (-1 * x522 * x356) + (x53 * x487) + (-1 * x484) + (x510 * x507) + (-1 * x488 * x508) + x486 + (-1 * x15 * x517) + (-1 * x514 * x511) + (-1 * x521); const FLT x530 = 1.0/2.0 * x15; const FLT x531 = x6 * x507; const FLT x532 = x21 * x509; const FLT x533 = (x511 * x520) + (-1 * x55 * x485); const FLT x534 = x6 * x525; const FLT x535 = (-1 * x528 * x534) + (x53 * x483); const FLT x536 = x535 + x533 + (-1 * x523 * x527) + (-1 * x50 * x514) + (x58 * x487) + (-1 * x531 * x356) + (-1 * x10 * x517) + x48 + (-1 * x507 * x532) + (x508 * x530); const FLT x537 = -1 * x43; const FLT x538 = x523 * x520; const FLT x539 = x24 * x509; const FLT x540 = x58 * x485; const FLT x541 = x21 * x530; const FLT x542 = x515 * x356; const FLT x543 = (x50 * x534) + (-1 * x46 * x483); const FLT x544 = x543 + (-1 * x504 * x542) + (-1 * x540) + x538 + (x507 * x541) + (-1 * x55 * x487) + (x511 * x527) + x537 + (-1 * x514 * x528) + (x507 * x539) + (x488 * x531); const FLT x545 = -1 * x51; const FLT x546 = x511 * x534; const FLT x547 = x55 * x483; const FLT x548 = (-1 * x520 * x528) + (x53 * x485); const FLT x549 = (x488 * x522) + x547 + (x50 * x527) + (-1 * x546) + x548 + x545 + (-1 * x46 * x487) + (-1 * x12 * x517) + (-1 * x514 * x523) + (-1 * x531 * x530) + (-1 * x508 * x356); const FLT x550 = (x549 * sensor_pt[2]) + (-1 * x536 * sensor_pt[1]) + (x544 * sensor_pt[0]); const FLT x551 = (x529 * sensor_pt[1]) + (-1 * x549 * sensor_pt[0]) + (x544 * sensor_pt[2]); const FLT x552 = (x550 * x393) + (-1 * x551 * x396) + (-1 * x529 * x372) + (x536 * x394); const FLT x553 = (x536 * sensor_pt[0]) + (-1 * x529 * sensor_pt[2]) + (x544 * sensor_pt[1]); const FLT x554 = (x549 * x372) + (-1 * x553 * x393) + (-1 * x536 * x398) + (x551 * x399); const FLT x555 = (x553 * x396) + (x529 * x398) + (-1 * x549 * x394) + (-1 * x550 * x399); const FLT x556 = (-1 * x82 * x555) + (x84 * x552) + (x79 * x554); const FLT x557 = (-1 * x84 * x554) + (x555 * x119) + (x79 * x552); const FLT x558 = (x556 * x118) + x555 + (-1 * x557 * x153); const FLT x559 = (-1 * x552 * x119) + (x82 * x554) + (x79 * x555); const FLT x560 = x554 + (x557 * x176) + (-1 * x559 * x118); const FLT x561 = (x559 * x153) + x552 + (-1 * x556 * x176); const FLT x562 = (-1 * x194 * ((x561 * x205) + (-1 * ((x558 * x196) + (x560 * x195)) * x197))) + (-1 * ((-1 * x560 * x187) + (x558 * x168)) * x190); const FLT x563 = x25 * x481; const FLT x564 = x25 * x518; const FLT x565 = x24 * x492; const FLT x566 = x24 * x496; const FLT x567 = x24 * x18; const FLT x568 = (x24 * x24 * x24) * x490; const FLT x569 = (x499 * x567) + (x495 * x568) + (-1 * x491 * x568) + (x22 * x566) + (-1 * x22 * x565) + (-1 * x502 * x567) + (x19 * x566) + (-1 * x19 * x565); const FLT x570 = x506 * x569; const FLT x571 = x570 * x356; const FLT x572 = x24 * x570; const FLT x573 = x515 * x569; const FLT x574 = 1.0/2.0 * x573; const FLT x575 = x512 * x567; const FLT x576 = x6 * x519; const FLT x577 = x6 * x24 * x481; const FLT x578 = (x53 * x577) + (-1 * x528 * x576); const FLT x579 = x578 + x537 + (-1 * x55 * x563) + (x511 * x564) + (-1 * x532 * x570) + (-1 * x50 * x575) + (-1 * x6 * x571) + (-1 * x10 * x574) + (-1 * x538) + (x530 * x572) + x540; const FLT x580 = x511 * x576; const FLT x581 = x6 * x570; const FLT x582 = x55 * x577; const FLT x583 = (x21 * x488 * x570) + (x53 * x563) + (-1 * x486) + (-1 * x12 * x574) + (-1 * x528 * x564) + (-1 * x580) + x521 + (-1 * x24 * x571) + x582 + x41 + (-1 * x530 * x581) + (-1 * x523 * x575); const FLT x584 = x50 * x576; const FLT x585 = x46 * x577; const FLT x586 = -1 * x48; const FLT x587 = (-1 * x528 * x575) + (-1 * x58 * x563) + (x509 * x572) + x586 + (x488 * x581) + (x541 * x570) + x584 + x533 + (-1 * x573 * x356) + (x523 * x564) + (-1 * x585); const FLT x588 = (x519 * x524) + (-1 * x58 * x577); const FLT x589 = x548 + (-1 * x15 * x574) + (-1 * x488 * x572) + (-1 * x511 * x575) + x51 + (-1 * x50 * x564) + x588 + (x46 * x563) + (x510 * x570) + (-1 * x21 * x571); const FLT x590 = (x589 * sensor_pt[1]) + (-1 * x583 * sensor_pt[0]) + (x587 * sensor_pt[2]); const FLT x591 = (x579 * sensor_pt[0]) + (-1 * x589 * sensor_pt[2]) + (x587 * sensor_pt[1]); const FLT x592 = (-1 * x591 * x393) + (x583 * x372) + (-1 * x579 * x398) + (x590 * x399); const FLT x593 = (x583 * sensor_pt[2]) + (-1 * x579 * sensor_pt[1]) + (x587 * sensor_pt[0]); const FLT x594 = (-1 * x590 * x396) + (-1 * x589 * x372) + (x593 * x393) + (x579 * x394); const FLT x595 = (-1 * x593 * x399) + (x591 * x396) + (-1 * x583 * x394) + (x589 * x398); const FLT x596 = x78 * x595; const FLT x597 = (-1 * x3 * x596) + (x79 * x592) + (x84 * x594); const FLT x598 = (-1 * x84 * x592) + (x68 * x596) + (x79 * x594); const FLT x599 = x595 + (x597 * x118) + (-1 * x598 * x153); const FLT x600 = (-1 * x594 * x119) + (x82 * x592) + (x63 * x596); const FLT x601 = x592 + (x598 * x176) + (-1 * x600 * x118); const FLT x602 = x594 + (-1 * x597 * x176) + (x600 * x153); const FLT x603 = (-1 * x194 * ((x602 * x205) + (-1 * ((x599 * x196) + (x601 * x195)) * x197))) + (-1 * ((-1 * x601 * x187) + (x599 * x168)) * x190); const FLT x604 = x19 * x518; const FLT x605 = x6 * x513; const FLT x606 = x19 * x481; const FLT x607 = x6 * x22; const FLT x608 = x6 * x6 * x6; const FLT x609 = (x496 * x608) + (-1 * x492 * x607) + (-1 * x492 * x608) + (x496 * x607) + (-1 * x6 * x503) + (x6 * x500) + (x6 * x501) + (-1 * x6 * x498); const FLT x610 = x609 * x506; const FLT x611 = x21 * x610; const FLT x612 = x609 * x516; const FLT x613 = x6 * x610; const FLT x614 = x24 * x610; const FLT x615 = x535 + (x613 * x509) + (-1 * x15 * x612) + (-1 * x611 * x356) + (-1 * x488 * x614) + (-1 * x584) + x585 + (x604 * x523) + (-1 * x58 * x606) + x586 + (-1 * x605 * x511); const FLT x616 = (-1 * x611 * x509) + x41 + (-1 * x50 * x605) + (-1 * x613 * x356) + x484 + x580 + (x614 * x530) + (-1 * x526) + (x53 * x606) + (-1 * x604 * x528) + (-1 * x582) + (-1 * x10 * x612); const FLT x617 = (-1 * x609 * x542) + (-1 * x605 * x528) + (x610 * x539) + x588 + (x50 * x604) + (-1 * x46 * x606) + x545 + (-1 * x547) + (x488 * x613) + (x610 * x541) + x546; const FLT x618 = x578 + (-1 * x12 * x612) + (-1 * x604 * x511) + (-1 * x513 * x524) + x43 + x543 + (x55 * x606) + (-1 * x613 * x530) + (x488 * x611) + (-1 * x614 * x356); const FLT x619 = (x617 * sensor_pt[2]) + (x615 * sensor_pt[1]) + (-1 * x618 * sensor_pt[0]); const FLT x620 = (x618 * sensor_pt[2]) + (-1 * x616 * sensor_pt[1]) + (x617 * sensor_pt[0]); const FLT x621 = (x620 * x393) + (-1 * x619 * x396) + (-1 * x615 * x372) + (x616 * x394); const FLT x622 = (x616 * sensor_pt[0]) + (-1 * x615 * sensor_pt[2]) + (x617 * sensor_pt[1]); const FLT x623 = (x618 * x372) + (-1 * x616 * x398) + (x619 * x399) + (-1 * x622 * x393); const FLT x624 = (-1 * x620 * x399) + (x622 * x396) + (-1 * x618 * x394) + (x615 * x398); const FLT x625 = (-1 * x82 * x624) + (x84 * x621) + (x79 * x623); const FLT x626 = (-1 * x84 * x623) + (x624 * x119) + (x79 * x621); const FLT x627 = (x625 * x118) + x624 + (-1 * x626 * x153); const FLT x628 = (x82 * x623) + (-1 * x621 * x119) + (x79 * x624); const FLT x629 = (x626 * x176) + x623 + (-1 * x628 * x118); const FLT x630 = x621 + (x628 * x153) + (-1 * x625 * x176); const FLT x631 = (-1 * x194 * ((x630 * x205) + (-1 * ((x627 * x196) + (x629 * x195)) * x197))) + (-1 * ((-1 * x629 * x187) + (x627 * x168)) * x190); const FLT x632 = dt * x295; const FLT x633 = dt * x292; const FLT x634 = x633 + x632; const FLT x635 = -1 * dt * x298; const FLT x636 = (-1 * dt * x299) + dt; const FLT x637 = x636 + x635; const FLT x638 = dt * x303; const FLT x639 = dt * x301; const FLT x640 = (-1 * x639) + x638; const FLT x641 = (-1 * x194 * ((x640 * x205) + (-1 * ((x634 * x196) + (x637 * x195)) * x197))) + (-1 * ((-1 * x637 * x187) + (x634 * x168)) * x190); const FLT x642 = dt * x308; const FLT x643 = dt * x307; const FLT x644 = (-1 * x643) + x642; const FLT x645 = x638 + x639; const FLT x646 = -1 * dt * x313; const FLT x647 = x636 + x646; const FLT x648 = (-1 * x194 * ((x647 * x205) + (-1 * ((x644 * x196) + (x645 * x195)) * x197))) + (-1 * ((-1 * x645 * x187) + (x644 * x168)) * x190); const FLT x649 = x646 + x635 + dt; const FLT x650 = (-1 * x632) + x633; const FLT x651 = x642 + x643; const FLT x652 = (-1 * x194 * ((x651 * x205) + (-1 * ((x649 * x196) + (x650 * x195)) * x197))) + (-1 * ((-1 * x650 * x187) + (x649 * x168)) * x190); const FLT x653 = x204 * x191 * x194; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (x209 * x206) + x206 + (((-1 * x203 * x187) + (x210 * x161)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x255 + (x209 * x255) + (((-1 * x254 * x187) + (x210 * x242)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x287 + (x209 * x287) + (((-1 * x286 * x187) + (x210 * x282)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x306 + (x209 * x306) + (((-1 * x305 * x187) + (x210 * x297)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x316 + (x209 * x316) + (((-1 * x315 * x187) + (x210 * x310)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x321 + (x209 * x321) + (((-1 * x320 * x187) + (x210 * x317)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x335 + (x209 * x335) + (((-1 * x334 * x187) + (x210 * x327)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x342 + (x209 * x342) + (((-1 * x341 * x187) + (x210 * x338)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x346 + (x209 * x346) + (((-1 * x345 * x187) + (x210 * x343)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x409 + (x409 * x209) + (((-1 * x408 * x187) + (x405 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x438 + (x438 * x209) + (((-1 * x437 * x187) + (x434 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x464 + (x464 * x209) + (((-1 * x463 * x187) + (x460 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x470 + (x470 * x209) + (((-1 * x469 * x187) + (x465 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x475 + (x475 * x209) + (((-1 * x474 * x187) + (x471 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x479 + (x479 * x209) + (((-1 * x478 * x187) + (x476 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x562 + (x562 * x209) + (((-1 * x561 * x187) + (x558 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), (x603 * x209) + x603 + (((-1 * x602 * x187) + (x599 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x631 + (x631 * x209) + (((-1 * x630 * x187) + (x627 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), (x641 * x209) + x641 + (((-1 * x640 * x187) + (x634 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x648 + (x648 * x209) + (((-1 * x647 * x187) + (x644 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x652 + (x652 * x209) + (((-1 * x651 * x187) + (x649 * x210)) * x212)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), x211 * x211); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), -1 * cos(x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), x209); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), -1 + (-1 * x209)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), (-1 * x653 * x209) + (-1 * x653)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x2 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x4 = dt * dt; const FLT x5 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x6 = x4 * (x5 * x5); const FLT x7 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x8 = x4 * (x7 * x7); const FLT x9 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x10 = x4 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = cos(x13); const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x16 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x17 = (x16 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x2 * (*_x0).Object.Pose.Rot[0]); const FLT x18 = (*_x0).Object.Pose.Rot[0] + (-1 * x16 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x2 * (*_x0).Object.Pose.Rot[3]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x19 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[2]); const FLT x20 = sin(x13); const FLT x21 = (x20 * x20) * (1. / x11); const FLT x22 = (1. / sqrt((x19 * x19) + (x18 * x18) + (x17 * x17) + (x3 * x3))) * (1. / sqrt((x8 * x21) + (x14 * x14) + (x6 * x21) + (x21 * x10))); const FLT x23 = x22 * x14; const FLT x24 = x3 * x23; const FLT x25 = dt * x22 * x20 * (1. / x12); const FLT x26 = x9 * x25; const FLT x27 = x26 * x18; const FLT x28 = x7 * x25; const FLT x29 = x28 * x17; const FLT x30 = x5 * x25; const FLT x31 = x30 * x19; const FLT x32 = (-1 * x31) + x29 + (-1 * x24) + (-1 * x27); const FLT x33 = x3 * x30; const FLT x34 = x26 * x17; const FLT x35 = x23 * x19; const FLT x36 = x28 * x18; const FLT x37 = x36 + x35 + (-1 * x33) + x34; const FLT x38 = 2 * x37; const FLT x39 = (-1 * x28 * x19) + (x23 * x18) + (-1 * x30 * x17) + (-1 * x3 * x26); const FLT x40 = x30 * x18; const FLT x41 = x26 * x19; const FLT x42 = x23 * x17; const FLT x43 = x3 * x28; const FLT x44 = x43 + x42 + x40 + (-1 * x41); const FLT x45 = 2 * x44; const FLT x46 = x45 * x39; const FLT x47 = x46 + (-1 * x32 * x38); const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x50 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x51 = (x50 * (*_x0).Lighthouse.Rot[1]) + (x49 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x48 * (*_x0).Lighthouse.Rot[2]); const FLT x52 = x51 * x51; const FLT x53 = (-1 * x48 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x50 * (*_x0).Lighthouse.Rot[2]); const FLT x54 = x53 * x53; const FLT x55 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x56 = (x50 * (*_x0).Lighthouse.Rot[0]) + (x55 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[2]; const FLT x57 = x56 * x56; const FLT x58 = (*_x0).Lighthouse.Rot[1] + (-1 * x55 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x48 * (*_x0).Lighthouse.Rot[0]) + (x49 * (*_x0).Lighthouse.Rot[2]); const FLT x59 = x58 * x58; const FLT x60 = x57 + x59 + x52 + x54; const FLT x61 = 1. / x60; const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x52) + (x61 * x54) + (x61 * x57))) * (1. / sqrt(x60)); const FLT x63 = x62 * x51; const FLT x64 = (-1 * x29) + x31 + x27 + x24; const FLT x65 = 2 * x64; const FLT x66 = 1 + (x65 * x32) + (-2 * (x44 * x44)); const FLT x67 = x62 * x53; const FLT x68 = x65 * x39; const FLT x69 = (x44 * x38) + (-1 * x68); const FLT x70 = x62 * x56; const FLT x71 = (-1 * x70 * x69) + (x63 * x47) + (x67 * x66); const FLT x72 = 2 * x70; const FLT x73 = x62 * x58; const FLT x74 = (-1 * x63 * x66) + (x73 * x69) + (x67 * x47); const FLT x75 = 2 * x73; const FLT x76 = x69 + (x71 * x72) + (-1 * x75 * x74); const FLT x77 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x78 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x79 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x80 = (x73 * x79) + (x77 * x67) + (-1 * x78 * x63); const FLT x81 = (x70 * x78) + (x79 * x67) + (-1 * x73 * x77); const FLT x82 = 1.0/2.0 * dt * fabs(dt); const FLT x83 = (-1 * x64 * sensor_pt[0]) + (x39 * sensor_pt[2]) + (x37 * sensor_pt[1]); const FLT x84 = (-1 * x44 * sensor_pt[1]) + (x39 * sensor_pt[0]) + (x64 * sensor_pt[2]); const FLT x85 = (*_x0).Object.Pose.Pos[1] + (x82 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x84 * x44) + (-1 * x83 * x37))); const FLT x86 = (-1 * x37 * sensor_pt[2]) + (x39 * sensor_pt[1]) + (x44 * sensor_pt[0]); const FLT x87 = (*_x0).Object.Pose.Pos[0] + (x82 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x83 * x64) + (-1 * x86 * x44))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])); const FLT x88 = (*_x0).Object.Pose.Pos[2] + (2 * ((x86 * x37) + (-1 * x84 * x64))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x82 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x89 = (x88 * x73) + (x85 * x67) + (-1 * x87 * x63); const FLT x90 = (x87 * x70) + (x88 * x67) + (-1 * x85 * x73); const FLT x91 = (-1 * (x78 + (2 * ((-1 * x81 * x70) + (x80 * x63))))) + x87 + (2 * ((-1 * x70 * x90) + (x89 * x63))); const FLT x92 = (x77 * x63) + (-1 * x70 * x79) + (x78 * x67); const FLT x93 = (x85 * x63) + (x87 * x67) + (-1 * x88 * x70); const FLT x94 = x88 + (-1 * (x79 + (2 * ((-1 * x80 * x73) + (x70 * x92))))) + (2 * ((-1 * x89 * x73) + (x70 * x93))); const FLT x95 = x94 * x94; const FLT x96 = 1. / x95; const FLT x97 = x91 * x96; const FLT x98 = 1. / x94; const FLT x99 = (-1 * x73 * x47) + (x70 * x66) + (x67 * x69); const FLT x100 = 2 * x63; const FLT x101 = x66 + (-1 * x72 * x99) + (x74 * x100); const FLT x102 = (x91 * x91) + x95; const FLT x103 = 1. / x102; const FLT x104 = x95 * x103; const FLT x105 = 2 * x91; const FLT x106 = 2 * x94; const FLT x107 = x85 + (2 * ((-1 * x63 * x93) + (x73 * x90))) + (-1 * (x77 + (2 * ((-1 * x63 * x92) + (x81 * x73))))); const FLT x108 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x109 = 1.0/2.0 * (1. / (x102 * sqrt(x102))) * x108 * x107; const FLT x110 = x47 + (-1 * x71 * x100) + (x75 * x99); const FLT x111 = (1. / sqrt(x102)) * x108; const FLT x112 = x107 * x107; const FLT x113 = 1. / sqrt(1 + (-1 * x103 * (x108 * x108) * x112)); const FLT x114 = (-1 * x113 * ((x110 * x111) + (-1 * x109 * ((x76 * x106) + (x101 * x105))))) + (-1 * x104 * ((-1 * x98 * x101) + (x76 * x97))); const FLT x115 = -1 * x94; const FLT x116 = sin(1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * atan2(x91, x115)) + (-1 * asin(x107 * x111))) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag); const FLT x117 = x96 * x107; const FLT x118 = 2 * x95 * (1. / (x95 + x112)) * atan2(x107, x115) * ((*error_model).BSD0.curve + (*_x0).BSD0.curve); const FLT x119 = (-1 * x40) + (-1 * x43) + x41 + (-1 * x42); const FLT x120 = 1 + (x45 * x119) + (-2 * (x37 * x37)); const FLT x121 = (x65 * x37) + (-1 * x46); const FLT x122 = x38 * x39; const FLT x123 = x122 + (-1 * x65 * x119); const FLT x124 = (-1 * x70 * x123) + (x63 * x120) + (x67 * x121); const FLT x125 = (-1 * x63 * x121) + (x73 * x123) + (x67 * x120); const FLT x126 = (x72 * x124) + x123 + (-1 * x75 * x125); const FLT x127 = (-1 * x73 * x120) + (x70 * x121) + (x67 * x123); const FLT x128 = (-1 * x72 * x127) + x121 + (x100 * x125); const FLT x129 = x120 + (-1 * x100 * x124) + (x75 * x127); const FLT x130 = (-1 * x113 * ((x111 * x129) + (-1 * ((x106 * x126) + (x105 * x128)) * x109))) + (-1 * ((-1 * x98 * x128) + (x97 * x126)) * x104); const FLT x131 = (x65 * x44) + (-1 * x122); const FLT x132 = (-1 * x35) + (-1 * x36) + (-1 * x34) + x33; const FLT x133 = x68 + (-1 * x45 * x132); const FLT x134 = 1 + (x38 * x132) + (-2 * (x64 * x64)); const FLT x135 = (-1 * x70 * x134) + (x63 * x131) + (x67 * x133); const FLT x136 = (-1 * x63 * x133) + (x73 * x134) + (x67 * x131); const FLT x137 = (x72 * x135) + x134 + (-1 * x75 * x136); const FLT x138 = x96 * x137; const FLT x139 = (-1 * x73 * x131) + (x70 * x133) + (x67 * x134); const FLT x140 = x133 + (-1 * x72 * x139) + (x100 * x136); const FLT x141 = x131 + (-1 * x100 * x135) + (x75 * x139); const FLT x142 = (-1 * x113 * ((x111 * x141) + (-1 * ((x106 * x137) + (x105 * x140)) * x109))) + (-1 * ((-1 * x98 * x140) + (x91 * x138)) * x104); cnMatrixOptionalSet(Hx, 0, 0, x114 + (x114 * x116) + (((-1 * x98 * x110) + (x76 * x117)) * x118)); cnMatrixOptionalSet(Hx, 0, 1, x130 + (x116 * x130) + (x118 * ((-1 * x98 * x129) + (x117 * x126)))); cnMatrixOptionalSet(Hx, 0, 2, (x116 * x142) + x142 + (x118 * ((-1 * x98 * x141) + (x107 * x138)))); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt); } } static inline FLT SurviveJointKalmanErrorModel_LightMeas_y_gen1(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x4 = (*_x0).Lighthouse.Rot[0] + (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x5 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x3 * (*_x0).Lighthouse.Rot[2]); const FLT x6 = x5 * x5; const FLT x7 = x4 * x4; const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x9 = x8 * x8; const FLT x10 = (*_x0).Lighthouse.Rot[1] + (x3 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]); const FLT x11 = x10 * x10; const FLT x12 = x11 + x9 + x6 + x7; const FLT x13 = 1. / x12; const FLT x14 = (1. / sqrt(x12)) * (1. / sqrt((x13 * x11) + (x7 * x13) + (x6 * x13) + (x9 * x13))); const FLT x15 = x4 * x14; const FLT x16 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x17 = x5 * x14; const FLT x18 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x19 = x14 * x10; const FLT x20 = (x0 * x15) + (x19 * x18) + (-1 * x17 * x16); const FLT x21 = x8 * x14; const FLT x22 = (x21 * x16) + (x15 * x18) + (-1 * x0 * x19); const FLT x23 = 1.0/2.0 * dt * fabs(dt); const FLT x24 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x27 = (x25 * (*_x0).Object.Pose.Rot[0]) + (x24 * (*_x0).Object.Pose.Rot[3]) + (-1 * x26 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[2]; const FLT x28 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x29 = dt * dt; const FLT x30 = (x28 * x28) * x29; const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x32 = (x31 * x31) * x29; const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x34 = (x33 * x33) * x29; const FLT x35 = 1e-10 + x34 + x30 + x32; const FLT x36 = sqrt(x35); const FLT x37 = 0.5 * x36; const FLT x38 = sin(x37); const FLT x39 = (x25 * (*_x0).Object.Pose.Rot[1]) + (-1 * x24 * (*_x0).Object.Pose.Rot[2]) + (*_x0).Object.Pose.Rot[3] + (x26 * (*_x0).Object.Pose.Rot[0]); const FLT x40 = (*_x0).Object.Pose.Rot[0] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (-1 * x24 * (*_x0).Object.Pose.Rot[1]) + (-1 * x25 * (*_x0).Object.Pose.Rot[2]); const FLT x41 = (*_x0).Object.Pose.Rot[1] + (x24 * (*_x0).Object.Pose.Rot[0]) + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (x26 * (*_x0).Object.Pose.Rot[2]); const FLT x42 = (1. / x35) * (x38 * x38); const FLT x43 = cos(x37); const FLT x44 = (1. / sqrt((x40 * x40) + (x39 * x39) + (x41 * x41) + (x27 * x27))) * (1. / sqrt((x43 * x43) + (x42 * x30) + (x42 * x32) + (x42 * x34))); const FLT x45 = dt * x44 * (1. / x36) * x38; const FLT x46 = x45 * x28; const FLT x47 = x45 * x33; const FLT x48 = x43 * x44; const FLT x49 = x45 * x31; const FLT x50 = (x41 * x48) + (-1 * x46 * x27) + (x40 * x49) + (x47 * x39); const FLT x51 = (x40 * x48) + (-1 * x41 * x49) + (-1 * x46 * x39) + (-1 * x47 * x27); const FLT x52 = (-1 * x49 * x39) + (x41 * x46) + (x40 * x47) + (x48 * x27); const FLT x53 = (x51 * sensor_pt[2]) + (-1 * x52 * sensor_pt[0]) + (x50 * sensor_pt[1]); const FLT x54 = (x49 * x27) + (x48 * x39) + (x40 * x46) + (-1 * x41 * x47); const FLT x55 = (-1 * x54 * sensor_pt[1]) + (x51 * sensor_pt[0]) + (x52 * sensor_pt[2]); const FLT x56 = (*_x0).Object.Pose.Pos[1] + (x23 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x54 * x55) + (-1 * x50 * x53))); const FLT x57 = (x51 * sensor_pt[1]) + (-1 * x50 * sensor_pt[2]) + (x54 * sensor_pt[0]); const FLT x58 = (*_x0).Object.Pose.Pos[0] + (x23 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x53 * x52) + (-1 * x54 * x57))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])); const FLT x59 = (*_x0).Object.Pose.Pos[2] + (2 * ((x50 * x57) + (-1 * x52 * x55))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x23 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x60 = (x59 * x19) + (x56 * x15) + (-1 * x58 * x17); const FLT x61 = (x58 * x21) + (x59 * x15) + (-1 * x56 * x19); const FLT x62 = x58 + (-1 * (x16 + (2 * ((-1 * x22 * x21) + (x20 * x17))))) + (2 * ((-1 * x61 * x21) + (x60 * x17))); const FLT x63 = (x0 * x17) + (-1 * x21 * x18) + (x15 * x16); const FLT x64 = (x56 * x17) + (x58 * x15) + (-1 * x59 * x21); const FLT x65 = x59 + (-1 * (x18 + (2 * ((-1 * x20 * x19) + (x63 * x21))))) + (2 * ((-1 * x60 * x19) + (x64 * x21))); const FLT x66 = -1 * x65; const FLT x67 = x56 + (2 * ((-1 * x64 * x17) + (x61 * x19))) + (-1 * (x0 + (2 * ((-1 * x63 * x17) + (x22 * x19))))); const FLT x68 = (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (-1 * atan2(-1 * x67, x66)) + (-1 * asin((1. / sqrt((x67 * x67) + (x65 * x65))) * x62 * ((*error_model).BSD1.tilt + (*_x0).BSD1.tilt))); return x68 + ((atan2(x62, x66) * atan2(x62, x66)) * ((*error_model).BSD1.curve + (*_x0).BSD1.curve)) + (-1 * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * cos(1.5707963267949 + x68 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha)); } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[0]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x6 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x9 = (-1 * x8 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x5 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x10 = x9 * x9; const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x1 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x8 * (*_x0).Lighthouse.Rot[2]); const FLT x12 = x11 * x11; const FLT x13 = x10 + x12 + x4 + x7; const FLT x14 = 1. / x13; const FLT x15 = x4 * x14; const FLT x16 = x14 * x10; const FLT x17 = x14 * x12; const FLT x18 = x17 + (x7 * x14) + x15 + x16; const FLT x19 = 1. / x18; const FLT x20 = 2 * x11; const FLT x21 = x20 * x14; const FLT x22 = x21 * x19; const FLT x23 = x3 * x22; const FLT x24 = -1 * x23; const FLT x25 = 2 * x6; const FLT x26 = x25 * x14; const FLT x27 = x9 * x19; const FLT x28 = x26 * x27; const FLT x29 = -1 * x28; const FLT x30 = x29 + x24; const FLT x31 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x32 = 1. / sqrt(x18); const FLT x33 = 1. / sqrt(x13); const FLT x34 = x32 * x33; const FLT x35 = x31 * x34; const FLT x36 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x37 = x34 * x36; const FLT x38 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x39 = x34 * x38; const FLT x40 = (x39 * x11) + (x6 * x35) + (-1 * x3 * x37); const FLT x41 = x3 * x34; const FLT x42 = (x9 * x37) + (x6 * x39) + (-1 * x35 * x11); const FLT x43 = x9 * x34; const FLT x44 = dt * fabs(dt); const FLT x45 = 1.0/2.0 * x44; const FLT x46 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x47 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x48 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x49 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x50 = (x47 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x49 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x48 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2]; const FLT x51 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x52 = (x49 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x51 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x48 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x53 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x54 = (-1 * x53 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[0] + (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x51 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x55 = (*_x0).Object.Pose.Rot[1] + (x53 * (*_x0).Object.Pose.Rot[0]) + (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x51 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x56 = (x55 * x55) + (x54 * x54) + (x52 * x52) + (x50 * x50); const FLT x57 = 1. / sqrt(x56); const FLT x58 = dt * dt; const FLT x59 = x46 * x46; const FLT x60 = x58 * x59; const FLT x61 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x62 = x61 * x61; const FLT x63 = x62 * x58; const FLT x64 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x65 = x64 * x64; const FLT x66 = x65 * x58; const FLT x67 = 1e-10 + x66 + x60 + x63; const FLT x68 = sqrt(x67); const FLT x69 = 0.5 * x68; const FLT x70 = sin(x69); const FLT x71 = x70 * x70; const FLT x72 = 1. / x67; const FLT x73 = x71 * x72; const FLT x74 = cos(x69); const FLT x75 = (x73 * x63) + (x73 * x60) + (x74 * x74) + (x73 * x66); const FLT x76 = 1. / sqrt(x75); const FLT x77 = x76 * x57; const FLT x78 = x70 * (1. / x68); const FLT x79 = dt * x78; const FLT x80 = x79 * x77; const FLT x81 = x80 * x50; const FLT x82 = x80 * x52; const FLT x83 = x74 * x77; const FLT x84 = x83 * x55; const FLT x85 = x80 * x54; const FLT x86 = x84 + (-1 * x81 * x46) + (x85 * x61) + (x82 * x64); const FLT x87 = x83 * x54; const FLT x88 = x77 * x55; const FLT x89 = x88 * x79; const FLT x90 = (-1 * x89 * x61) + x87 + (-1 * x82 * x46) + (-1 * x81 * x64); const FLT x91 = x83 * x50; const FLT x92 = (x89 * x46) + (x85 * x64) + (-1 * x82 * x61) + x91; const FLT x93 = (-1 * x92 * sensor_pt[0]) + (x90 * sensor_pt[2]) + (x86 * sensor_pt[1]); const FLT x94 = x83 * x52; const FLT x95 = (x81 * x61) + x94 + (x85 * x46) + (-1 * x89 * x64); const FLT x96 = (x90 * sensor_pt[0]) + (-1 * x95 * sensor_pt[1]) + (x92 * sensor_pt[2]); const FLT x97 = (*_x0).Object.Pose.Pos[1] + (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x96 * x95) + (-1 * x86 * x93))); const FLT x98 = x97 * x34; const FLT x99 = (-1 * x86 * sensor_pt[2]) + (x90 * sensor_pt[1]) + (x95 * sensor_pt[0]); const FLT x100 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x93 * x92) + (-1 * x99 * x95))); const FLT x101 = x34 * x100; const FLT x102 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x86 * x99) + (-1 * x92 * x96))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x103 = x34 * x102; const FLT x104 = (x11 * x103) + (x6 * x98) + (-1 * x3 * x101); const FLT x105 = (x9 * x101) + (x6 * x103) + (-1 * x98 * x11); const FLT x106 = x100 + (-1 * (x36 + (2 * ((-1 * x42 * x43) + (x40 * x41))))) + (2 * ((-1 * x43 * x105) + (x41 * x104))); const FLT x107 = (x3 * x35) + (-1 * x9 * x39) + (x6 * x37); const FLT x108 = x34 * x11; const FLT x109 = (x3 * x98) + (x6 * x101) + (-1 * x9 * x103); const FLT x110 = x102 + (-1 * (x38 + (2 * ((-1 * x40 * x108) + (x43 * x107))))) + (2 * ((-1 * x108 * x104) + (x43 * x109))); const FLT x111 = x110 * x110; const FLT x112 = 1. / x111; const FLT x113 = x106 * x112; const FLT x114 = 2 * x19; const FLT x115 = x16 * x114; const FLT x116 = x15 * x114; const FLT x117 = -1 + x116; const FLT x118 = x117 + x115; const FLT x119 = 1. / x110; const FLT x120 = -1 * x110; const FLT x121 = atan2(x106, x120); const FLT x122 = x106 * x106; const FLT x123 = 2 * (1. / (x111 + x122)) * x111 * x121 * ((*error_model).BSD1.curve + (*_x0).BSD1.curve); const FLT x124 = x97 + (2 * ((-1 * x41 * x109) + (x108 * x105))) + (-1 * (x31 + (2 * ((-1 * x41 * x107) + (x42 * x108))))); const FLT x125 = x112 * x124; const FLT x126 = 2 * x3; const FLT x127 = x14 * x126; const FLT x128 = x6 * x19 * x127; const FLT x129 = x21 * x27; const FLT x130 = -1 * x129; const FLT x131 = x130 + x128; const FLT x132 = (x124 * x124) + x111; const FLT x133 = 1. / x132; const FLT x134 = x111 * x133; const FLT x135 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt; const FLT x136 = 1. / sqrt(1 + (-1 * x122 * x133 * (x135 * x135))); const FLT x137 = 1. / sqrt(x132); const FLT x138 = x137 * x135; const FLT x139 = 2 * x124; const FLT x140 = 2 * x110; const FLT x141 = 1.0/2.0 * x106 * (1. / (x132 * sqrt(x132))) * x135; const FLT x142 = (-1 * x136 * ((-1 * x141 * ((x30 * x140) + (x131 * x139))) + (x118 * x138))) + (-1 * x134 * ((x119 * x131) + (-1 * x30 * x125))); const FLT x143 = 1.5707963267949 + (*_x0).BSD1.gibpha + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x124, x120)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (-1 * asin(x106 * x138)); const FLT x144 = sin(x143) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag); const FLT x145 = x6 * x22; const FLT x146 = x27 * x127; const FLT x147 = -1 * x146; const FLT x148 = x147 + x145; const FLT x149 = -1 * x128; const FLT x150 = x149 + x130; const FLT x151 = x17 * x114; const FLT x152 = x117 + x151; const FLT x153 = (-1 * x136 * ((-1 * ((x140 * x148) + (x139 * x152)) * x141) + (x138 * x150))) + (-1 * ((x119 * x152) + (-1 * x125 * x148)) * x134); const FLT x154 = -1 + x115 + x151; const FLT x155 = x24 + x28; const FLT x156 = -1 * x145; const FLT x157 = x156 + x147; const FLT x158 = (-1 * x136 * ((-1 * ((x140 * x154) + (x139 * x157)) * x141) + (x138 * x155))) + (-1 * ((x119 * x157) + (-1 * x125 * x154)) * x134); const FLT x159 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x160 = x9 * x159; const FLT x161 = 1.0 * x11; const FLT x162 = x161 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x163 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x164 = x3 * x163; const FLT x165 = x25 + x164 + x160 + x162; const FLT x166 = x32 * (1. / (x13 * sqrt(x13))); const FLT x167 = x166 * x165; const FLT x168 = x11 * x104; const FLT x169 = x9 * x109; const FLT x170 = 1. / (x13 * x13); const FLT x171 = x7 * x170; const FLT x172 = x4 * x170; const FLT x173 = x12 * x170; const FLT x174 = x10 * x170; const FLT x175 = x26 + (x14 * x160) + (-1 * x165 * x174) + (-1 * x165 * x172) + (-1 * x165 * x173) + (x14 * x162) + (-1 * x165 * x171) + (x14 * x164); const FLT x176 = x33 * (1. / (x18 * sqrt(x18))); const FLT x177 = x9 * x176; const FLT x178 = x177 * x175; const FLT x179 = x40 * x11; const FLT x180 = x9 * x107; const FLT x181 = x34 * x109; const FLT x182 = x181 * x159; const FLT x183 = x176 * x175; const FLT x184 = x34 * x107; const FLT x185 = x184 * x159; const FLT x186 = 1.0/2.0 * x11; const FLT x187 = x186 * x183; const FLT x188 = x0 * x103; const FLT x189 = x3 * x167; const FLT x190 = 1.0/2.0 * x100; const FLT x191 = 1.0/2.0 * x6; const FLT x192 = x183 * x191; const FLT x193 = x102 * x186; const FLT x194 = x97 * x167; const FLT x195 = x3 * x176; const FLT x196 = x175 * x195; const FLT x197 = 1.0/2.0 * x196; const FLT x198 = -1 * x8 * x101; const FLT x199 = x98 + x198; const FLT x200 = (-1 * x191 * x194) + x188 + (-1 * x102 * x187) + (-1 * x97 * x192) + (x100 * x197) + (x189 * x190) + x199 + (-1 * x167 * x193); const FLT x201 = x34 * x20; const FLT x202 = 1.0/2.0 * x36; const FLT x203 = x167 * x186; const FLT x204 = x167 * x191; const FLT x205 = x0 * x39; const FLT x206 = -1 * x8 * x37; const FLT x207 = x206 + x35; const FLT x208 = x205 + (-1 * x38 * x187) + (-1 * x31 * x192) + (x202 * x189) + (-1 * x38 * x203) + (x36 * x197) + x207 + (-1 * x31 * x204); const FLT x209 = 1.0/2.0 * x31; const FLT x210 = x8 * x35; const FLT x211 = 1.0/2.0 * x178; const FLT x212 = 1.0/2.0 * x38; const FLT x213 = x9 * x167; const FLT x214 = -1 * x2 * x39; const FLT x215 = x214 + x37; const FLT x216 = x215 + (x213 * x212) + (-1 * x36 * x192) + x210 + (-1 * x209 * x189) + (x38 * x211) + (-1 * x31 * x197) + (-1 * x36 * x204); const FLT x217 = 2 * x9; const FLT x218 = x34 * x217; const FLT x219 = 1.0/2.0 * x97; const FLT x220 = x8 * x98; const FLT x221 = -1 * x2 * x103; const FLT x222 = 1.0/2.0 * x102; const FLT x223 = x9 * x222; const FLT x224 = (x211 * x102) + x101 + (x223 * x167) + x221 + (-1 * x219 * x189) + (-1 * x97 * x197) + (-1 * x100 * x192) + x220 + (-1 * x204 * x100); const FLT x225 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x226 = x34 * x104; const FLT x227 = x40 * x34; const FLT x228 = (x227 * x225) + (-1 * x225 * x226); const FLT x229 = x228 + (-1 * x218 * x216) + (x167 * x180) + (x218 * x224) + (x201 * x208) + (-1 * x167 * x179) + (-1 * x185) + (-1 * x109 * x178) + x182 + (-1 * x169 * x167) + (x180 * x183) + (-1 * x179 * x183) + (-1 * x200 * x201) + (x168 * x167) + (x168 * x183); const FLT x230 = x34 * x126; const FLT x231 = x226 * x163; const FLT x232 = x3 * x40; const FLT x233 = x227 * x163; const FLT x234 = x9 * x105; const FLT x235 = x2 * x37; const FLT x236 = -1 * x0 * x35; const FLT x237 = x236 + (x31 * x187) + x235 + (-1 * x38 * x204) + (-1 * x36 * x211) + (-1 * x213 * x202) + (x31 * x203) + x39 + (-1 * x38 * x192); const FLT x238 = x9 * x42; const FLT x239 = x9 * x190; const FLT x240 = x102 * x191; const FLT x241 = x2 * x101; const FLT x242 = -1 * x0 * x98; const FLT x243 = x242 + x103; const FLT x244 = x243 + x241 + (-1 * x240 * x167) + (-1 * x211 * x100) + (-1 * x239 * x167) + (x97 * x187) + (x186 * x194) + (-1 * x102 * x192); const FLT x245 = x42 * x34; const FLT x246 = x34 * x105; const FLT x247 = (-1 * x246 * x159) + (x245 * x159); const FLT x248 = (-1 * x104 * x196) + (-1 * x104 * x189) + x247 + (x105 * x178) + (-1 * x238 * x183) + (-1 * x238 * x167) + (x230 * x200) + (-1 * x230 * x208) + (-1 * x218 * x244) + x231 + (-1 * x233) + (x232 * x167) + (x234 * x167) + (x218 * x237) + (x40 * x196); const FLT x249 = x42 * x11; const FLT x250 = x225 * x245; const FLT x251 = x11 * x105; const FLT x252 = x225 * x246; const FLT x253 = (x163 * x184) + (-1 * x163 * x181); const FLT x254 = (-1 * x237 * x201) + (x249 * x183) + x253 + (x249 * x167) + (x201 * x244) + (-1 * x107 * x189) + (-1 * x251 * x167) + (-1 * x250) + (-1 * x251 * x183) + (-1 * x230 * x224) + (-1 * x107 * x196) + x252 + (x109 * x196) + (x216 * x230) + (x109 * x189); const FLT x255 = (-1 * x136 * ((-1 * ((x229 * x140) + (x254 * x139)) * x141) + (x248 * x138))) + (-1 * ((x254 * x119) + (-1 * x229 * x125)) * x134); const FLT x256 = x3 * x159; const FLT x257 = x9 * x163; const FLT x258 = x6 * x225; const FLT x259 = (-1 * x258) + x256 + (-1 * x257) + x20; const FLT x260 = x259 * x166; const FLT x261 = -1 * x241; const FLT x262 = x11 * x176; const FLT x263 = x259 * x170; const FLT x264 = (-1 * x10 * x263) + x21 + (-1 * x12 * x263) + (-1 * x4 * x263) + (-1 * x14 * x257) + (-1 * x7 * x263) + (-1 * x14 * x258) + (x14 * x256); const FLT x265 = x264 * x222; const FLT x266 = x260 * x186; const FLT x267 = x3 * x260; const FLT x268 = x264 * x176; const FLT x269 = x97 * x191; const FLT x270 = x264 * x195; const FLT x271 = (x270 * x190) + (-1 * x265 * x262) + x261 + (-1 * x269 * x260) + (-1 * x266 * x102) + x243 + (-1 * x268 * x269) + (x267 * x190); const FLT x272 = x264 * x109; const FLT x273 = 2 * x34; const FLT x274 = x273 * x104; const FLT x275 = x40 * x273; const FLT x276 = x260 * x191; const FLT x277 = x264 * x262; const FLT x278 = x268 * x191; const FLT x279 = (-1 * x235) + x236; const FLT x280 = (-1 * x31 * x278) + (x202 * x270) + x39 + (-1 * x38 * x266) + (x202 * x267) + x279 + (-1 * x31 * x276) + (-1 * x212 * x277); const FLT x281 = x2 * x35; const FLT x282 = x264 * x177; const FLT x283 = x8 * x39; const FLT x284 = x9 * x260; const FLT x285 = x0 * x37; const FLT x286 = (-1 * x36 * x278) + (-1 * x285) + (-1 * x36 * x276) + (-1 * x209 * x267) + (x212 * x282) + (-1 * x209 * x270) + (x212 * x284) + x281 + x283; const FLT x287 = x264 * x104; const FLT x288 = x2 * x98; const FLT x289 = x0 * x101; const FLT x290 = x8 * x103; const FLT x291 = x290 + (x265 * x177) + (-1 * x278 * x100) + (x260 * x223) + (-1 * x289) + (-1 * x219 * x270) + (-1 * x219 * x267) + x288 + (-1 * x276 * x100); const FLT x292 = x253 + (x260 * x168) + (x268 * x180) + (x218 * x291) + x275 + (-1 * x268 * x179) + (-1 * x260 * x179) + (-1 * x274) + (-1 * x260 * x169) + (-1 * x201 * x271) + (-1 * x218 * x286) + (-1 * x272 * x177) + (x262 * x287) + (x201 * x280) + (x260 * x180); const FLT x293 = -1 * x205; const FLT x294 = (x209 * x277) + (-1 * x202 * x282) + x206 + (-1 * x38 * x278) + (-1 * x35) + (-1 * x202 * x284) + (x31 * x266) + (-1 * x38 * x276) + x293; const FLT x295 = x264 * x105; const FLT x296 = -1 * x188; const FLT x297 = x198 + (x219 * x277) + (-1 * x282 * x190) + (-1 * x239 * x260) + (-1 * x260 * x240) + (x97 * x266) + x296 + (-1 * x98) + (-1 * x268 * x240); const FLT x298 = (x226 * x159) + (-1 * x227 * x159); const FLT x299 = (-1 * x245 * x163) + (x246 * x163); const FLT x300 = x298 + (-1 * x218 * x297) + (x40 * x270) + x299 + (-1 * x287 * x195) + (-1 * x238 * x268) + (-1 * x238 * x260) + (-1 * x267 * x104) + (x232 * x260) + (-1 * x230 * x280) + (x295 * x177) + (x218 * x294) + (x230 * x271) + (x234 * x260); const FLT x301 = x273 * x105; const FLT x302 = x42 * x273; const FLT x303 = (-1 * x230 * x291) + (-1 * x182) + (-1 * x302) + (x272 * x195) + (-1 * x201 * x294) + (x201 * x297) + (x260 * x249) + (-1 * x267 * x107) + (x267 * x109) + (-1 * x270 * x107) + (x230 * x286) + (-1 * x295 * x262) + x185 + x301 + (x268 * x249) + (-1 * x260 * x251); const FLT x304 = (-1 * x136 * ((-1 * ((x292 * x140) + (x303 * x139)) * x141) + (x300 * x138))) + (-1 * ((x303 * x119) + (-1 * x292 * x125)) * x134); const FLT x305 = x161 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x306 = x3 * x225; const FLT x307 = x6 * x159; const FLT x308 = (-1 * x307) + (-1 * x306) + x217 + x305; const FLT x309 = x308 * x166; const FLT x310 = ((x14 * x217) + (-1 * x308 * x174) + (-1 * x308 * x173) + (x14 * x305) + (-1 * x308 * x171) + (-1 * x14 * x307) + (-1 * x308 * x172) + (-1 * x14 * x306)) * x176; const FLT x311 = x3 * x309; const FLT x312 = x309 * x191; const FLT x313 = x3 * x310; const FLT x314 = (x313 * x190) + x289 + (-1 * x97 * x312) + (-1 * x288) + x290 + (-1 * x310 * x193) + (-1 * x309 * x193) + (x311 * x190) + (-1 * x269 * x310); const FLT x315 = x273 * x109; const FLT x316 = x9 * x310; const FLT x317 = x9 * x309; const FLT x318 = x310 * x191; const FLT x319 = x279 + (-1 * x209 * x311) + (x212 * x316) + (-1 * x36 * x312) + (-1 * x209 * x313) + (-1 * x39) + (-1 * x36 * x318) + (x212 * x317); const FLT x320 = x309 * x186; const FLT x321 = x310 * x186; const FLT x322 = x285 + (-1 * x281) + x283 + (x202 * x313) + (-1 * x38 * x320) + (-1 * x31 * x312) + (-1 * x38 * x321) + (-1 * x31 * x318) + (x202 * x311); const FLT x323 = x273 * x107; const FLT x324 = (-1 * x103) + (-1 * x219 * x311) + (-1 * x312 * x100) + x242 + (x222 * x316) + (-1 * x219 * x313) + (x222 * x317) + (-1 * x318 * x100) + x261; const FLT x325 = (x309 * x168) + (-1 * x309 * x179) + (-1 * x316 * x109) + x233 + x315 + (x309 * x180) + (-1 * x231) + (-1 * x218 * x319) + (-1 * x323) + (-1 * x309 * x169) + (-1 * x201 * x314) + (x218 * x324) + (x310 * x168) + (x310 * x180) + (-1 * x310 * x179) + (x201 * x322); const FLT x326 = 1.0/2.0 * x317; const FLT x327 = -1 * x210; const FLT x328 = x215 + x327 + (x31 * x321) + (-1 * x38 * x318) + (-1 * x202 * x316) + (x31 * x320) + (-1 * x38 * x312) + (-1 * x36 * x326); const FLT x329 = x221 + (-1 * x220); const FLT x330 = x329 + x101 + (-1 * x240 * x309) + (x97 * x321) + (-1 * x326 * x100) + (-1 * x316 * x190) + (x97 * x320) + (-1 * x240 * x310); const FLT x331 = x228 + (x316 * x105) + (x234 * x309) + (-1 * x238 * x309) + (-1 * x313 * x104) + (x218 * x328) + (x40 * x311) + (x232 * x310) + (-1 * x301) + (-1 * x238 * x310) + (-1 * x311 * x104) + (-1 * x230 * x322) + (-1 * x218 * x330) + (x230 * x314) + x302; const FLT x332 = (-1 * x225 * x184) + (x225 * x181); const FLT x333 = x299 + (x249 * x309) + (-1 * x201 * x328) + (x230 * x319) + (x249 * x310) + (x311 * x109) + (-1 * x230 * x324) + x332 + (-1 * x311 * x107) + (-1 * x251 * x310) + (x201 * x330) + (x313 * x109) + (-1 * x251 * x309) + (-1 * x313 * x107); const FLT x334 = (-1 * x136 * ((-1 * ((x325 * x140) + (x333 * x139)) * x141) + (x331 * x138))) + (-1 * ((x333 * x119) + (-1 * x325 * x125)) * x134); const FLT x335 = x11 * x159; const FLT x336 = x9 * x225; const FLT x337 = x6 * x163; const FLT x338 = (-1 * x337) + (-1 * x335) + x126 + x336; const FLT x339 = x338 * x166; const FLT x340 = (-1 * x14 * x337) + (-1 * x338 * x172) + (x14 * x336) + (-1 * x338 * x173) + (-1 * x14 * x335) + (-1 * x338 * x171) + x127 + (-1 * x338 * x174); const FLT x341 = x262 * x340; const FLT x342 = x3 * x339; const FLT x343 = 1.0/2.0 * x342; const FLT x344 = x340 * x176; const FLT x345 = x340 * x195; const FLT x346 = (-1 * x101) + (-1 * x339 * x193) + x329 + (x343 * x100) + (-1 * x269 * x339) + (x345 * x190) + (-1 * x222 * x341) + (-1 * x269 * x344); const FLT x347 = x344 * x191; const FLT x348 = x339 * x191; const FLT x349 = x340 * x177; const FLT x350 = (x222 * x349) + (x223 * x339) + x296 + x199 + (-1 * x219 * x342) + (-1 * x347 * x100) + (-1 * x219 * x345) + (-1 * x348 * x100); const FLT x351 = x339 * x186; const FLT x352 = (-1 * x37) + (x202 * x345) + (-1 * x212 * x341) + (-1 * x31 * x347) + x214 + (-1 * x38 * x351) + x327 + (-1 * x31 * x348) + (x36 * x343); const FLT x353 = x9 * x339; const FLT x354 = x207 + (-1 * x36 * x347) + (x212 * x353) + (-1 * x36 * x348) + (-1 * x31 * x343) + (-1 * x209 * x345) + (x212 * x349) + x293; const FLT x355 = (x341 * x104) + (x201 * x352) + (-1 * x339 * x179) + (x218 * x350) + (-1 * x339 * x169) + x298 + (-1 * x201 * x346) + (x339 * x180) + x332 + (-1 * x344 * x179) + (-1 * x218 * x354) + (x339 * x168) + (-1 * x349 * x109) + (x344 * x180); const FLT x356 = (-1 * x38 * x347) + x281 + (-1 * x283) + x285 + (-1 * x202 * x349) + (x31 * x351) + (x209 * x341) + (-1 * x38 * x348) + (-1 * x202 * x353); const FLT x357 = x340 * x105; const FLT x358 = (-1 * x290) + (-1 * x349 * x190) + x288 + (x97 * x351) + (-1 * x240 * x344) + x289 + (-1 * x353 * x190) + (x219 * x341) + (-1 * x240 * x339); const FLT x359 = (x234 * x339) + (x357 * x177) + (-1 * x345 * x104) + (-1 * x230 * x352) + (x40 * x345) + (-1 * x342 * x104) + (x40 * x342) + x250 + x274 + (x230 * x346) + (-1 * x252) + (-1 * x275) + (-1 * x238 * x344) + (-1 * x218 * x358) + (-1 * x238 * x339) + (x218 * x356); const FLT x360 = x247 + x323 + (-1 * x315) + (x342 * x109) + (-1 * x251 * x339) + (-1 * x342 * x107) + (-1 * x262 * x357) + (x230 * x354) + (x249 * x339) + (x201 * x358) + (-1 * x345 * x107) + (x345 * x109) + (-1 * x201 * x356) + (x249 * x344) + (-1 * x230 * x350); const FLT x361 = (-1 * x136 * ((-1 * ((x355 * x140) + (x360 * x139)) * x141) + (x359 * x138))) + (-1 * ((x360 * x119) + (-1 * x355 * x125)) * x134); const FLT x362 = x44 * x19; const FLT x363 = x14 * x362; const FLT x364 = x9 * x363; const FLT x365 = x6 * x364; const FLT x366 = x3 * x11 * x363; const FLT x367 = x366 + x365; const FLT x368 = -1 * x16 * x362; const FLT x369 = -1 * x15 * x362; const FLT x370 = x45 + x369 + x368; const FLT x371 = x11 * x364; const FLT x372 = x6 * x363; const FLT x373 = x3 * x372; const FLT x374 = (-1 * x373) + x371; const FLT x375 = (-1 * x136 * ((-1 * ((x367 * x140) + (x374 * x139)) * x141) + (x370 * x138))) + (-1 * ((x374 * x119) + (-1 * x367 * x125)) * x134); const FLT x376 = x3 * x364; const FLT x377 = x11 * x372; const FLT x378 = (-1 * x377) + x376; const FLT x379 = x371 + x373; const FLT x380 = (-1 * x17 * x362) + x45; const FLT x381 = x380 + x369; const FLT x382 = (-1 * x136 * ((-1 * ((x378 * x140) + (x381 * x139)) * x141) + (x379 * x138))) + (-1 * ((x381 * x119) + (-1 * x378 * x125)) * x134); const FLT x383 = x380 + x368; const FLT x384 = (-1 * x365) + x366; const FLT x385 = x376 + x377; const FLT x386 = (-1 * x136 * ((-1 * ((x383 * x140) + (x385 * x139)) * x141) + (x384 * x138))) + (-1 * ((x385 * x119) + (-1 * x383 * x125)) * x134); const FLT x387 = x23 + x28; const FLT x388 = -1 * x115; const FLT x389 = 1 + (-1 * x116); const FLT x390 = x389 + x388; const FLT x391 = x149 + x129; const FLT x392 = (-1 * x136 * ((-1 * ((x387 * x140) + (x391 * x139)) * x141) + (x390 * x138))) + (-1 * ((x391 * x119) + (-1 * x387 * x125)) * x134); const FLT x393 = x156 + x146; const FLT x394 = x129 + x128; const FLT x395 = -1 * x151; const FLT x396 = x389 + x395; const FLT x397 = (-1 * x136 * ((-1 * ((x393 * x140) + (x396 * x139)) * x141) + (x394 * x138))) + (-1 * ((x396 * x119) + (-1 * x393 * x125)) * x134); const FLT x398 = 1 + x395 + x388; const FLT x399 = x29 + x23; const FLT x400 = x146 + x145; const FLT x401 = (-1 * x136 * ((-1 * ((x398 * x140) + (x400 * x139)) * x141) + (x399 * x138))) + (-1 * ((x400 * x119) + (-1 * x398 * x125)) * x134); const FLT x402 = 0.5 * x83; const FLT x403 = x402 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x404 = x80 * x61; const FLT x405 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x406 = x405 * x404; const FLT x407 = 1.0 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x408 = 1.0 * x52; const FLT x409 = 1.0 * x50; const FLT x410 = x76 * (1. / (x56 * sqrt(x56))); const FLT x411 = x410 * ((x409 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x408 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x55 * x407) + (2 * x54)); const FLT x412 = 1.0/2.0 * x79; const FLT x413 = x61 * x412; const FLT x414 = x413 * x411; const FLT x415 = 1.0/2.0 * x74; const FLT x416 = x52 * x415; const FLT x417 = x46 * x412; const FLT x418 = x411 * x417; const FLT x419 = x64 * x411; const FLT x420 = x412 * x419; const FLT x421 = x80 * x64; const FLT x422 = -1 * x53 * x421; const FLT x423 = x80 * x46; const FLT x424 = x423 + x422; const FLT x425 = x424 + (x55 * x420) + x406 + x403 + (-1 * x50 * x414) + (-1 * x54 * x418) + (-1 * x411 * x416); const FLT x426 = 2 * x99; const FLT x427 = x50 * x415; const FLT x428 = x53 * x423; const FLT x429 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x430 = -1 * x404 * x429; const FLT x431 = x402 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x432 = x431 + x430; const FLT x433 = x432 + (-1 * x55 * x418) + x421 + (-1 * x54 * x420) + x428 + (x52 * x414) + (-1 * x411 * x427); const FLT x434 = x53 * x404; const FLT x435 = -1 * x434; const FLT x436 = x54 * x415; const FLT x437 = x52 * x417; const FLT x438 = x405 * x421; const FLT x439 = -1 * x438; const FLT x440 = x50 * x412; const FLT x441 = x423 * x429; const FLT x442 = x83 + (-1 * x441); const FLT x443 = (x419 * x440) + (-1 * x411 * x436) + x442 + x435 + (x55 * x414) + x439 + (x411 * x437); const FLT x444 = x83 * x53; const FLT x445 = x421 * x429; const FLT x446 = x55 * x415; const FLT x447 = x46 * x440; const FLT x448 = -1 * x405 * x423; const FLT x449 = x404 + x448; const FLT x450 = (-1 * x52 * x420) + x445 + (-1 * x411 * x446) + x449 + x444 + (-1 * x54 * x414) + (x411 * x447); const FLT x451 = (x450 * sensor_pt[1]) + (-1 * x433 * sensor_pt[0]) + (x443 * sensor_pt[2]); const FLT x452 = 2 * x92; const FLT x453 = (x425 * sensor_pt[0]) + (-1 * x450 * sensor_pt[2]) + (x443 * sensor_pt[1]); const FLT x454 = 2 * x95; const FLT x455 = 2 * x93; const FLT x456 = (x455 * x433) + (-1 * x453 * x454) + (-1 * x425 * x426) + (x451 * x452); const FLT x457 = x6 * x34; const FLT x458 = 2 * x86; const FLT x459 = 2 * x96; const FLT x460 = (x433 * sensor_pt[2]) + (x443 * sensor_pt[0]) + (-1 * x425 * sensor_pt[1]); const FLT x461 = (x454 * x460) + (x459 * x425) + (-1 * x455 * x450) + (-1 * x451 * x458); const FLT x462 = (x453 * x458) + (x450 * x426) + (-1 * x459 * x433) + (-1 * x452 * x460); const FLT x463 = (x456 * x457) + (-1 * x43 * x462) + (x41 * x461); const FLT x464 = (x462 * x108) + (-1 * x41 * x456) + (x457 * x461); const FLT x465 = x462 + (x463 * x218) + (-1 * x464 * x201); const FLT x466 = (-1 * x461 * x108) + (x43 * x456) + (x457 * x462); const FLT x467 = x456 + (-1 * x466 * x218) + (x464 * x230); const FLT x468 = x461 + (x466 * x201) + (-1 * x463 * x230); const FLT x469 = (-1 * x136 * ((-1 * ((x465 * x140) + (x468 * x139)) * x141) + (x467 * x138))) + (-1 * ((x468 * x119) + (-1 * x465 * x125)) * x134); const FLT x470 = 1.0 * x54; const FLT x471 = x410 * ((-1 * x409 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x55) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x408 * (*error_model).Object.Pose.AxisAngleRot[1])); const FLT x472 = x471 * x413; const FLT x473 = x64 * x412; const FLT x474 = x473 * x471; const FLT x475 = x471 * x415; const FLT x476 = x441 + x83; const FLT x477 = x476 + x435 + (-1 * x55 * x475) + x438 + (-1 * x54 * x472) + (x471 * x447) + (-1 * x52 * x474); const FLT x478 = -1 * x406; const FLT x479 = -1 * x403; const FLT x480 = x54 * x473; const FLT x481 = x471 * x417; const FLT x482 = (-1 * x50 * x475) + (x52 * x472) + x479 + x478 + (-1 * x471 * x480) + x424 + (-1 * x55 * x481); const FLT x483 = x64 * x440; const FLT x484 = -1 * x444; const FLT x485 = x448 + (-1 * x404); const FLT x486 = x484 + (-1 * x471 * x436) + (x55 * x472) + x445 + (x52 * x481) + x485 + (x471 * x483); const FLT x487 = (x477 * sensor_pt[1]) + (-1 * x482 * sensor_pt[0]) + (x486 * sensor_pt[2]); const FLT x488 = -1 * x428; const FLT x489 = -1 * x421; const FLT x490 = (-1 * x52 * x475) + x432 + (-1 * x50 * x472) + (x55 * x474) + x488 + (-1 * x54 * x481) + x489; const FLT x491 = (x482 * sensor_pt[2]) + (-1 * x490 * sensor_pt[1]) + (x486 * sensor_pt[0]); const FLT x492 = (x491 * x454) + (x490 * x459) + (-1 * x455 * x477) + (-1 * x458 * x487); const FLT x493 = (x490 * sensor_pt[0]) + (x486 * sensor_pt[1]) + (-1 * x477 * sensor_pt[2]); const FLT x494 = (-1 * x493 * x454) + (x455 * x482) + (-1 * x490 * x426) + (x452 * x487); const FLT x495 = (x493 * x458) + (x477 * x426) + (-1 * x459 * x482) + (-1 * x491 * x452); const FLT x496 = (-1 * x43 * x495) + (x41 * x492) + (x494 * x457); const FLT x497 = (-1 * x41 * x494) + (x495 * x108) + (x492 * x457); const FLT x498 = x495 + (x496 * x218) + (-1 * x497 * x201); const FLT x499 = (-1 * x492 * x108) + (x43 * x494) + (x495 * x457); const FLT x500 = (x497 * x230) + x494 + (-1 * x499 * x218); const FLT x501 = x492 + (-1 * x496 * x230) + (x499 * x201); const FLT x502 = (-1 * x136 * ((-1 * ((x498 * x140) + (x501 * x139)) * x141) + (x500 * x138))) + (-1 * ((x501 * x119) + (-1 * x498 * x125)) * x134); const FLT x503 = 1.0 * x55; const FLT x504 = x410 * ((2 * x50) + (x503 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x52 * x407)); const FLT x505 = x413 * x504; const FLT x506 = x473 * x504; const FLT x507 = (-1 * x423) + x422; const FLT x508 = x507 + (-1 * x52 * x506) + (-1 * x446 * x504) + x478 + (-1 * x54 * x505) + (x447 * x504) + x403; const FLT x509 = -1 * x445; const FLT x510 = x415 * x504; const FLT x511 = x417 * x504; const FLT x512 = x449 + (-1 * x54 * x511) + x484 + x509 + (-1 * x50 * x505) + (-1 * x52 * x510) + (x55 * x506); const FLT x513 = (-1 * x431) + x430; const FLT x514 = x513 + (x55 * x505) + x489 + x428 + (-1 * x436 * x504) + (x52 * x511) + (x483 * x504); const FLT x515 = x476 + (-1 * x50 * x510) + x439 + (x52 * x505) + (-1 * x55 * x511) + x434 + (-1 * x54 * x506); const FLT x516 = 2 * ((x515 * sensor_pt[2]) + (-1 * x512 * sensor_pt[1]) + (x514 * sensor_pt[0])); const FLT x517 = (x508 * sensor_pt[1]) + (-1 * x515 * sensor_pt[0]) + (x514 * sensor_pt[2]); const FLT x518 = (-1 * x458 * x517) + (x459 * x512) + (-1 * x455 * x508) + (x95 * x516); const FLT x519 = (-1 * x508 * sensor_pt[2]) + (x512 * sensor_pt[0]) + (x514 * sensor_pt[1]); const FLT x520 = (-1 * x454 * x519) + (-1 * x426 * x512) + (x455 * x515) + (x452 * x517); const FLT x521 = (-1 * x92 * x516) + (-1 * x459 * x515) + (x458 * x519) + (x426 * x508); const FLT x522 = (x41 * x518) + (-1 * x43 * x521) + (x457 * x520); const FLT x523 = (x521 * x108) + (-1 * x41 * x520) + (x457 * x518); const FLT x524 = (x522 * x218) + x521 + (-1 * x523 * x201); const FLT x525 = (-1 * x518 * x108) + (x43 * x520) + (x457 * x521); const FLT x526 = (x523 * x230) + x520 + (-1 * x525 * x218); const FLT x527 = x518 + (x525 * x201) + (-1 * x522 * x230); const FLT x528 = (-1 * x136 * ((-1 * ((x524 * x140) + (x527 * x139)) * x141) + (x526 * x138))) + (-1 * ((x527 * x119) + (-1 * x524 * x125)) * x134); const FLT x529 = x410 * ((2 * x52) + (x50 * x407) + (-1 * x503 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[2])); const FLT x530 = x413 * x529; const FLT x531 = x46 * x529; const FLT x532 = x64 * x529; const FLT x533 = x412 * x532; const FLT x534 = x513 + (-1 * x52 * x533) + x488 + x421 + (-1 * x446 * x529) + (-1 * x54 * x530) + (x440 * x531); const FLT x535 = x412 * x531; const FLT x536 = x479 + (-1 * x436 * x529) + x507 + x406 + (x55 * x530) + (x440 * x532) + (x52 * x535); const FLT x537 = x485 + (-1 * x54 * x533) + (-1 * x427 * x529) + (-1 * x55 * x535) + (x52 * x530) + x444 + x509; const FLT x538 = (x534 * sensor_pt[1]) + (x536 * sensor_pt[2]) + (-1 * x537 * sensor_pt[0]); const FLT x539 = x442 + x438 + (-1 * x50 * x530) + x434 + (-1 * x416 * x529) + (-1 * x54 * x535) + (x55 * x533); const FLT x540 = (x537 * sensor_pt[2]) + (-1 * x539 * sensor_pt[1]) + (x536 * sensor_pt[0]); const FLT x541 = (x454 * x540) + (x459 * x539) + (-1 * x455 * x534) + (-1 * x458 * x538); const FLT x542 = (x539 * sensor_pt[0]) + (-1 * x534 * sensor_pt[2]) + (x536 * sensor_pt[1]); const FLT x543 = (-1 * x454 * x542) + (x455 * x537) + (-1 * x426 * x539) + (x452 * x538); const FLT x544 = (x458 * x542) + (x426 * x534) + (-1 * x459 * x537) + (-1 * x452 * x540); const FLT x545 = (x41 * x541) + (-1 * x43 * x544) + (x457 * x543); const FLT x546 = (-1 * x41 * x543) + (x544 * x108) + (x457 * x541); const FLT x547 = x544 + (x545 * x218) + (-1 * x546 * x201); const FLT x548 = (-1 * x541 * x108) + (x43 * x543) + (x457 * x544); const FLT x549 = x543 + (x546 * x230) + (-1 * x548 * x218); const FLT x550 = x541 + (-1 * x545 * x230) + (x548 * x201); const FLT x551 = (-1 * x136 * ((-1 * ((x547 * x140) + (x550 * x139)) * x141) + (x549 * x138))) + (-1 * ((x550 * x119) + (-1 * x547 * x125)) * x134); const FLT x552 = dt * dt * dt; const FLT x553 = 0.5 * x72 * x552; const FLT x554 = x61 * x553; const FLT x555 = x46 * x554; const FLT x556 = x91 * x555; const FLT x557 = x64 * x554; const FLT x558 = x94 * x557; const FLT x559 = x62 * x553; const FLT x560 = x61 * x61 * x61; const FLT x561 = dt * dt * dt * dt; const FLT x562 = 2 * x71 * (1. / (x67 * x67)); const FLT x563 = x561 * x562; const FLT x564 = 1.0 * x74; const FLT x565 = x70 * (1. / (x67 * sqrt(x67))); const FLT x566 = x564 * x565; const FLT x567 = x561 * x566; const FLT x568 = x61 * x567; const FLT x569 = x61 * x563; const FLT x570 = 2 * x73; const FLT x571 = x58 * x570; const FLT x572 = x78 * x564; const FLT x573 = x58 * x572; const FLT x574 = (1. / (x75 * sqrt(x75))) * x57; const FLT x575 = ((-1 * x61 * x573) + (x560 * x567) + (x65 * x568) + (x59 * x568) + (-1 * x560 * x563) + (-1 * x59 * x569) + (-1 * x65 * x569) + (x61 * x571)) * x574; const FLT x576 = x52 * x575; const FLT x577 = 0.5 * x78; const FLT x578 = x58 * x577; const FLT x579 = x88 * x578; const FLT x580 = x77 * x52; const FLT x581 = x61 * x580; const FLT x582 = x552 * x565; const FLT x583 = x64 * x582; const FLT x584 = x581 * x583; const FLT x585 = x413 * x575; const FLT x586 = x77 * x50; const FLT x587 = x46 * x582; const FLT x588 = x61 * x587; const FLT x589 = x586 * x588; const FLT x590 = x77 * x54; const FLT x591 = x62 * x582; const FLT x592 = (-1 * x446 * x575) + x558 + (x87 * x559) + x85 + x589 + (-1 * x556) + (-1 * x591 * x590) + (x447 * x575) + (-1 * x473 * x576) + (-1 * x61 * x579) + (-1 * x584) + (-1 * x54 * x585); const FLT x593 = x417 * x575; const FLT x594 = x61 * x583; const FLT x595 = (-1 * x84 * x557) + (x88 * x594); const FLT x596 = x61 * x590; const FLT x597 = (-1 * x596 * x587) + (x87 * x555); const FLT x598 = x597 + (-1 * x591 * x586) + (-1 * x416 * x575) + (-1 * x578 * x581) + (x91 * x559) + x81 + (x55 * x473 * x575) + x595 + (-1 * x50 * x585) + (-1 * x54 * x593); const FLT x599 = x594 * x586; const FLT x600 = x91 * x557; const FLT x601 = -1 * x89; const FLT x602 = (x581 * x587) + (-1 * x94 * x555); const FLT x603 = x602 + (-1 * x436 * x575) + x601 + (x88 * x591) + x599 + (x55 * x585) + (x483 * x575) + (-1 * x596 * x578) + (-1 * x84 * x559) + (-1 * x600) + (x417 * x576); const FLT x604 = -1 * x82; const FLT x605 = x88 * x588; const FLT x606 = x84 * x555; const FLT x607 = (-1 * x596 * x583) + (x87 * x557); const FLT x608 = (x413 * x576) + (-1 * x605) + (-1 * x427 * x575) + x606 + (-1 * x61 * x578 * x586) + (-1 * x55 * x593) + x604 + x607 + (x591 * x580) + (-1 * x480 * x575) + (-1 * x94 * x559); const FLT x609 = 2 * ((-1 * x598 * sensor_pt[1]) + (x608 * sensor_pt[2]) + (x603 * sensor_pt[0])); const FLT x610 = (x592 * sensor_pt[1]) + (-1 * x608 * sensor_pt[0]) + (x603 * sensor_pt[2]); const FLT x611 = (x95 * x609) + (-1 * x455 * x592) + (-1 * x458 * x610) + (x459 * x598); const FLT x612 = 2 * ((x598 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x603 * sensor_pt[1])); const FLT x613 = (-1 * x95 * x612) + (x455 * x608) + (-1 * x426 * x598) + (x452 * x610); const FLT x614 = (x86 * x612) + (x426 * x592) + (-1 * x459 * x608) + (-1 * x92 * x609); const FLT x615 = (-1 * x43 * x614) + (x41 * x611) + (x457 * x613); const FLT x616 = (-1 * x41 * x613) + (x614 * x108) + (x457 * x611); const FLT x617 = x614 + (x615 * x218) + (-1 * x616 * x201); const FLT x618 = (-1 * x611 * x108) + (x43 * x613) + (x457 * x614); const FLT x619 = x613 + (x616 * x230) + (-1 * x618 * x218); const FLT x620 = x611 + (x618 * x201) + (-1 * x615 * x230); const FLT x621 = (-1 * x136 * ((-1 * ((x617 * x140) + (x620 * x139)) * x141) + (x619 * x138))) + (-1 * ((x620 * x119) + (-1 * x617 * x125)) * x134); const FLT x622 = x65 * x553; const FLT x623 = x65 * x582; const FLT x624 = x62 * x563; const FLT x625 = x62 * x567; const FLT x626 = x64 * x58; const FLT x627 = x64 * x59; const FLT x628 = (x64 * x64 * x64) * x561; const FLT x629 = ((x626 * x570) + (x628 * x566) + (x64 * x625) + (-1 * x64 * x624) + (-1 * x627 * x563) + (-1 * x626 * x572) + (-1 * x628 * x562) + (x627 * x567)) * x574; const FLT x630 = x413 * x629; const FLT x631 = x417 * x629; const FLT x632 = x64 * x629; const FLT x633 = x412 * x632; const FLT x634 = x415 * x629; const FLT x635 = x626 * x577; const FLT x636 = x64 * x46 * x553; const FLT x637 = x64 * x587; const FLT x638 = (-1 * x637 * x590) + (x87 * x636); const FLT x639 = x638 + (x55 * x633) + (-1 * x635 * x580) + (-1 * x599) + (-1 * x54 * x631) + (-1 * x52 * x634) + (-1 * x50 * x630) + (-1 * x84 * x622) + x601 + (x88 * x623) + x600; const FLT x640 = x88 * x637; const FLT x641 = x84 * x636; const FLT x642 = (-1 * x640) + x584 + (-1 * x55 * x631) + (-1 * x558) + (x52 * x630) + x85 + (x87 * x622) + (-1 * x623 * x590) + (-1 * x54 * x633) + (-1 * x50 * x634) + (-1 * x635 * x586) + x641; const FLT x643 = x637 * x580; const FLT x644 = x94 * x636; const FLT x645 = -1 * x81; const FLT x646 = (-1 * x635 * x590) + (x440 * x632) + x645 + (x437 * x629) + (x623 * x586) + (-1 * x436 * x629) + (x55 * x630) + x595 + (-1 * x91 * x622) + x643 + (-1 * x644); const FLT x647 = (x637 * x586) + (-1 * x91 * x636); const FLT x648 = x647 + (-1 * x446 * x629) + x607 + x82 + (-1 * x54 * x630) + (-1 * x88 * x635) + (-1 * x52 * x633) + (x94 * x622) + (-1 * x623 * x580) + (x447 * x629); const FLT x649 = (x648 * sensor_pt[1]) + (-1 * x642 * sensor_pt[0]) + (x646 * sensor_pt[2]); const FLT x650 = (x639 * sensor_pt[0]) + (-1 * x648 * sensor_pt[2]) + (x646 * sensor_pt[1]); const FLT x651 = (-1 * x454 * x650) + (-1 * x426 * x639) + (x455 * x642) + (x452 * x649); const FLT x652 = (x642 * sensor_pt[2]) + (-1 * x639 * sensor_pt[1]) + (x646 * sensor_pt[0]); const FLT x653 = (x454 * x652) + (-1 * x455 * x648) + (-1 * x458 * x649) + (x459 * x639); const FLT x654 = (x458 * x650) + (-1 * x452 * x652) + (-1 * x459 * x642) + (x426 * x648); const FLT x655 = (-1 * x43 * x654) + (x457 * x651) + (x41 * x653); const FLT x656 = (-1 * x41 * x651) + (x654 * x108) + (x457 * x653); const FLT x657 = x654 + (x655 * x218) + (-1 * x656 * x201); const FLT x658 = (-1 * x653 * x108) + (x43 * x651) + (x457 * x654); const FLT x659 = x651 + (x656 * x230) + (-1 * x658 * x218); const FLT x660 = x653 + (-1 * x655 * x230) + (x658 * x201); const FLT x661 = (-1 * x136 * ((-1 * ((x657 * x140) + (x660 * x139)) * x141) + (x659 * x138))) + (-1 * ((x660 * x119) + (-1 * x657 * x125)) * x134); const FLT x662 = x59 * x582; const FLT x663 = x59 * x553; const FLT x664 = x65 * x46; const FLT x665 = x46 * x46 * x46; const FLT x666 = ((x664 * x567) + (x665 * x567) + (-1 * x665 * x563) + (-1 * x46 * x624) + (-1 * x664 * x563) + (x46 * x625) + (-1 * x46 * x573) + (x46 * x571)) * x574; const FLT x667 = x413 * x666; const FLT x668 = x440 * x666; const FLT x669 = x412 * x666; const FLT x670 = x64 * x669; const FLT x671 = x644 + (-1 * x52 * x670) + (x46 * x668) + (-1 * x54 * x667) + (-1 * x643) + (-1 * x446 * x666) + (x662 * x586) + x645 + x597 + (-1 * x91 * x663) + (-1 * x46 * x579); const FLT x672 = x46 * x669; const FLT x673 = x46 * x578; const FLT x674 = (-1 * x54 * x672) + (-1 * x416 * x666) + (-1 * x50 * x667) + (-1 * x673 * x580) + x85 + (-1 * x662 * x590) + (-1 * x589) + x556 + x640 + (x87 * x663) + (x55 * x670) + (-1 * x641); const FLT x675 = (-1 * x673 * x590) + (x662 * x580) + (x55 * x667) + (-1 * x606) + x647 + x604 + (-1 * x436 * x666) + (-1 * x94 * x663) + (x52 * x672) + (x64 * x668) + x605; const FLT x676 = x638 + (-1 * x88 * x662) + (-1 * x673 * x586) + x602 + (x52 * x667) + (-1 * x54 * x670) + x89 + (-1 * x55 * x672) + (-1 * x427 * x666) + (x84 * x663); const FLT x677 = (x671 * sensor_pt[1]) + (x675 * sensor_pt[2]) + (-1 * x676 * sensor_pt[0]); const FLT x678 = (x676 * sensor_pt[2]) + (-1 * x674 * sensor_pt[1]) + (x675 * sensor_pt[0]); const FLT x679 = (-1 * x455 * x671) + (x454 * x678) + (-1 * x458 * x677) + (x459 * x674); const FLT x680 = (x674 * sensor_pt[0]) + (-1 * x671 * sensor_pt[2]) + (x675 * sensor_pt[1]); const FLT x681 = (x452 * x677) + (-1 * x426 * x674) + (x455 * x676) + (-1 * x454 * x680); const FLT x682 = (-1 * x452 * x678) + (x458 * x680) + (-1 * x459 * x676) + (x426 * x671); const FLT x683 = (-1 * x43 * x682) + (x41 * x679) + (x457 * x681); const FLT x684 = (-1 * x41 * x681) + (x682 * x108) + (x457 * x679); const FLT x685 = x682 + (x683 * x218) + (-1 * x684 * x201); const FLT x686 = (-1 * x679 * x108) + (x43 * x681) + (x457 * x682); const FLT x687 = x681 + (x684 * x230) + (-1 * x686 * x218); const FLT x688 = x679 + (x686 * x201) + (-1 * x683 * x230); const FLT x689 = (-1 * x136 * ((-1 * ((x685 * x140) + (x688 * x139)) * x141) + (x687 * x138))) + (-1 * ((x688 * x119) + (-1 * x685 * x125)) * x134); const FLT x690 = dt * x28; const FLT x691 = dt * x23; const FLT x692 = x691 + x690; const FLT x693 = -1 * dt * x115; const FLT x694 = -1 * dt * x116; const FLT x695 = x694 + x693 + dt; const FLT x696 = dt * x129; const FLT x697 = dt * x128; const FLT x698 = (-1 * x697) + x696; const FLT x699 = (-1 * x136 * ((-1 * ((x692 * x140) + (x698 * x139)) * x141) + (x695 * x138))) + (-1 * ((x698 * x119) + (-1 * x692 * x125)) * x134); const FLT x700 = dt * x146; const FLT x701 = dt * x145; const FLT x702 = (-1 * x701) + x700; const FLT x703 = x696 + x697; const FLT x704 = (-1 * dt * x151) + dt; const FLT x705 = x704 + x694; const FLT x706 = (-1 * x136 * ((-1 * ((x702 * x140) + (x705 * x139)) * x141) + (x703 * x138))) + (-1 * ((x705 * x119) + (-1 * x702 * x125)) * x134); const FLT x707 = x704 + x693; const FLT x708 = (-1 * x690) + x691; const FLT x709 = x700 + x701; const FLT x710 = (-1 * x136 * ((-1 * ((x707 * x140) + (x709 * x139)) * x141) + (x708 * x138))) + (-1 * ((x709 * x119) + (-1 * x707 * x125)) * x134); const FLT x711 = x106 * x137 * x136; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), (x123 * ((-1 * x118 * x119) + (x30 * x113))) + x142 + (x142 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), (((-1 * x119 * x150) + (x113 * x148)) * x123) + x153 + (x144 * x153)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x158 + (((-1 * x119 * x155) + (x113 * x154)) * x123) + (x144 * x158)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x255 + (((-1 * x248 * x119) + (x229 * x113)) * x123) + (x255 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x304 + (((-1 * x300 * x119) + (x292 * x113)) * x123) + (x304 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (((-1 * x331 * x119) + (x325 * x113)) * x123) + x334 + (x334 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x361 + (((-1 * x359 * x119) + (x355 * x113)) * x123) + (x361 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x375 + (((-1 * x370 * x119) + (x367 * x113)) * x123) + (x375 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x382 + (((-1 * x379 * x119) + (x378 * x113)) * x123) + (x382 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x386 + (((-1 * x384 * x119) + (x383 * x113)) * x123) + (x386 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), (((-1 * x390 * x119) + (x387 * x113)) * x123) + x392 + (x392 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x397 + (((-1 * x394 * x119) + (x393 * x113)) * x123) + (x397 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x401 + (((-1 * x399 * x119) + (x398 * x113)) * x123) + (x401 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x469 + (((-1 * x467 * x119) + (x465 * x113)) * x123) + (x469 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x502 + (((-1 * x500 * x119) + (x498 * x113)) * x123) + (x502 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (((-1 * x526 * x119) + (x524 * x113)) * x123) + x528 + (x528 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x551 + (((-1 * x549 * x119) + (x547 * x113)) * x123) + (x551 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x621 + (((-1 * x619 * x119) + (x617 * x113)) * x123) + (x621 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x661 + (((-1 * x659 * x119) + (x657 * x113)) * x123) + (x661 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x689 + (((-1 * x687 * x119) + (x685 * x113)) * x123) + (x689 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x699 + (((-1 * x695 * x119) + (x692 * x113)) * x123) + (x699 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x706 + (((-1 * x703 * x119) + (x702 * x113)) * x123) + (x706 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x710 + (((-1 * x708 * x119) + (x707 * x113)) * x123) + (x710 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), x121 * x121); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), -1 * cos(x143)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), x144); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1 + (-1 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), (-1 * x711 * x144) + (-1 * x711)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x3 = (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = 1.0 * x3; const FLT x5 = x4 * (*_x0).Lighthouse.Rot[0]; const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[0]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x7 = 1.0 * (*_x0).Lighthouse.Rot[3]; const FLT x8 = x6 * x7; const FLT x9 = (x0 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x10 = 1.0 * (*_x0).Lighthouse.Rot[2]; const FLT x11 = x9 * x10; const FLT x12 = (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]); const FLT x13 = 1.0 * x12; const FLT x14 = x13 * (*_x0).Lighthouse.Rot[1]; const FLT x15 = (-1 * x11) + (-1 * x14) + x5 + x8; const FLT x16 = x9 * x9; const FLT x17 = x12 * x12; const FLT x18 = x6 * x6; const FLT x19 = x3 * x3; const FLT x20 = x19 + x18 + x16 + x17; const FLT x21 = 1. / x20; const FLT x22 = x21 * x16; const FLT x23 = x21 * x18; const FLT x24 = x21 * x19; const FLT x25 = x24 + (x21 * x17) + x22 + x23; const FLT x26 = 1. / sqrt(x25); const FLT x27 = (1. / (x20 * sqrt(x20))) * x26; const FLT x28 = x27 * x15; const FLT x29 = dt * fabs(dt); const FLT x30 = 1.0/2.0 * x29; const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x32 = dt * dt; const FLT x33 = x31 * x31; const FLT x34 = x32 * x33; const FLT x35 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x36 = x35 * x35; const FLT x37 = x32 * x36; const FLT x38 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x39 = x38 * x38; const FLT x40 = x32 * x39; const FLT x41 = 1e-10 + x40 + x34 + x37; const FLT x42 = sqrt(x41); const FLT x43 = 0.5 * x42; const FLT x44 = sin(x43); const FLT x45 = (1. / x42) * x44; const FLT x46 = x45 * x31; const FLT x47 = dt * x46; const FLT x48 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x49 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x50 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x51 = (-1 * x49 * (*_x0).Object.Pose.Rot[2]) + (x50 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (x48 * (*_x0).Object.Pose.Rot[0]); const FLT x52 = (-1 * x48 * (*_x0).Object.Pose.Rot[1]) + (x50 * (*_x0).Object.Pose.Rot[0]) + (x49 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[2]; const FLT x53 = (-1 * x49 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[0] + (-1 * x48 * (*_x0).Object.Pose.Rot[3]) + (-1 * x50 * (*_x0).Object.Pose.Rot[2]); const FLT x54 = (*_x0).Object.Pose.Rot[1] + (x49 * (*_x0).Object.Pose.Rot[0]) + (-1 * x50 * (*_x0).Object.Pose.Rot[3]) + (x48 * (*_x0).Object.Pose.Rot[2]); const FLT x55 = (x54 * x54) + (x51 * x51) + (x53 * x53) + (x52 * x52); const FLT x56 = 1. / sqrt(x55); const FLT x57 = x44 * x44; const FLT x58 = 1. / x41; const FLT x59 = x58 * x57; const FLT x60 = cos(x43); const FLT x61 = (x59 * x37) + (x60 * x60) + (x59 * x34) + (x59 * x40); const FLT x62 = 1. / sqrt(x61); const FLT x63 = x62 * x53; const FLT x64 = x63 * x56; const FLT x65 = x62 * x54; const FLT x66 = dt * x45; const FLT x67 = x66 * x56; const FLT x68 = x67 * x65; const FLT x69 = x60 * x56; const FLT x70 = x62 * x51; const FLT x71 = x70 * x69; const FLT x72 = x62 * x52; const FLT x73 = x72 * x67; const FLT x74 = (x73 * x35) + x71 + (x64 * x47) + (-1 * x68 * x38); const FLT x75 = x70 * x56; const FLT x76 = x62 * x69; const FLT x77 = x76 * x53; const FLT x78 = x77 + (-1 * x68 * x35) + (-1 * x75 * x47) + (-1 * x73 * x38); const FLT x79 = x72 * x56; const FLT x80 = x75 * x66; const FLT x81 = x76 * x54; const FLT x82 = x63 * x67; const FLT x83 = x81 + (-1 * x79 * x47) + (x82 * x35) + (x80 * x38); const FLT x84 = (-1 * x83 * sensor_pt[2]) + (x78 * sensor_pt[1]) + (x74 * sensor_pt[0]); const FLT x85 = x76 * x52; const FLT x86 = x65 * x56; const FLT x87 = (-1 * x80 * x35) + (x82 * x38) + (x86 * x47) + x85; const FLT x88 = (-1 * x87 * sensor_pt[0]) + (x78 * sensor_pt[2]) + (x83 * sensor_pt[1]); const FLT x89 = (*_x0).Object.Pose.Pos[0] + (x30 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (2 * ((x88 * x87) + (-1 * x84 * x74))) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])); const FLT x90 = 1. / sqrt(x20); const FLT x91 = x90 * x26; const FLT x92 = x91 * x12; const FLT x93 = (-1 * x74 * sensor_pt[1]) + (x78 * sensor_pt[0]) + (x87 * sensor_pt[2]); const FLT x94 = (2 * ((x83 * x84) + (-1 * x87 * x93))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (*_x0).Object.Pose.Pos[2] + (x30 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x95 = x6 * x91; const FLT x96 = (*_x0).Object.Pose.Pos[1] + (*error_model).Object.Pose.Pos[1] + (x30 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (2 * ((x74 * x93) + (-1 * x83 * x88))) + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])); const FLT x97 = x9 * x91; const FLT x98 = (x97 * x96) + (x89 * x92) + (-1 * x95 * x94); const FLT x99 = x6 * x98; const FLT x100 = 1.0/2.0 * x96; const FLT x101 = x9 * x28; const FLT x102 = 1.0/2.0 * x89; const FLT x103 = x28 * x102; const FLT x104 = 1.0/2.0 * x94; const FLT x105 = x28 * x104; const FLT x106 = x90 * (1. / (x25 * sqrt(x25))); const FLT x107 = x12 * x106; const FLT x108 = 1. / (x20 * x20); const FLT x109 = x17 * x108; const FLT x110 = x15 * x108; const FLT x111 = (-1 * x21 * x14) + (-1 * x18 * x110) + (x5 * x21) + (-1 * x15 * x109) + (-1 * x21 * x11) + (x8 * x21) + (-1 * x16 * x110) + (-1 * x19 * x110); const FLT x112 = x102 * x111; const FLT x113 = x9 * x106; const FLT x114 = x100 * x111; const FLT x115 = x6 * x106; const FLT x116 = x104 * x111; const FLT x117 = 0.5 * x91; const FLT x118 = x96 * x117; const FLT x119 = x89 * x117; const FLT x120 = x94 * x117; const FLT x121 = (-1 * x120 * (*_x0).Lighthouse.Rot[3]) + (-1 * x118 * (*_x0).Lighthouse.Rot[2]) + (-1 * x119 * (*_x0).Lighthouse.Rot[1]); const FLT x122 = x121 + (x115 * x116) + (-1 * x12 * x103) + (-1 * x107 * x112) + (x6 * x105) + (-1 * x101 * x100) + (-1 * x113 * x114); const FLT x123 = 2 * x95; const FLT x124 = x3 * x91; const FLT x125 = (x92 * x96) + (x94 * x124) + (-1 * x89 * x97); const FLT x126 = x3 * x125; const FLT x127 = x3 * x106; const FLT x128 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x129 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x130 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x131 = (x124 * x130) + (x92 * x128) + (-1 * x97 * x129); const FLT x132 = x111 * x131; const FLT x133 = x28 * x131; const FLT x134 = x111 * x125; const FLT x135 = 1.0/2.0 * x111; const FLT x136 = x128 * x135; const FLT x137 = 1.0/2.0 * x129; const FLT x138 = x28 * x12; const FLT x139 = x130 * x135; const FLT x140 = 1.0/2.0 * x128; const FLT x141 = x28 * x140; const FLT x142 = 1.0/2.0 * x130; const FLT x143 = x6 * x28; const FLT x144 = x129 * x135; const FLT x145 = x117 * x129; const FLT x146 = x117 * (*_x0).Lighthouse.Rot[2]; const FLT x147 = x117 * (*_x0).Lighthouse.Rot[3]; const FLT x148 = (-1 * x130 * x147) + (-1 * x145 * (*_x0).Lighthouse.Rot[1]) + (-1 * x128 * x146); const FLT x149 = (-1 * x107 * x144) + (-1 * x137 * x138) + (x115 * x139) + (x142 * x143) + x148 + (-1 * x113 * x136) + (-1 * x9 * x141); const FLT x150 = (x97 * x128) + (-1 * x95 * x130) + (x92 * x129); const FLT x151 = x111 * x150; const FLT x152 = x28 * x150; const FLT x153 = x91 * x98; const FLT x154 = 1.0 * (*_x0).Lighthouse.Rot[0]; const FLT x155 = x91 * x125; const FLT x156 = x154 * x155; const FLT x157 = x98 * x111; const FLT x158 = x91 * x131; const FLT x159 = x154 * x158; const FLT x160 = x120 * (*_x0).Lighthouse.Rot[0]; const FLT x161 = x118 * (*_x0).Lighthouse.Rot[1]; const FLT x162 = x119 * (*_x0).Lighthouse.Rot[2]; const FLT x163 = (x112 * x113) + x162 + (-1 * x107 * x114) + (x9 * x103) + (-1 * x116 * x127) + x160 + (-1 * x3 * x105) + (-1 * x161) + (-1 * x100 * x138); const FLT x164 = 2 * x124; const FLT x165 = x3 * x142; const FLT x166 = x117 * (*_x0).Lighthouse.Rot[0]; const FLT x167 = x166 * x130; const FLT x168 = x127 * x135; const FLT x169 = x117 * (*_x0).Lighthouse.Rot[1]; const FLT x170 = x128 * x169; const FLT x171 = x145 * (*_x0).Lighthouse.Rot[2]; const FLT x172 = x171 + (x113 * x144) + (-1 * x28 * x165) + (-1 * x12 * x141) + (x101 * x137) + (-1 * x170) + x167 + (-1 * x107 * x136) + (-1 * x168 * x130); const FLT x173 = x91 * x150; const FLT x174 = (-1 * x7 * x173) + (x164 * x172) + (-1 * x164 * x163) + (-1 * x115 * x157) + (-1 * x3 * x133) + (-1 * x123 * x149) + (x28 * x126) + (-1 * x99 * x28) + (x123 * x122) + (x127 * x134) + x159 + (x7 * x153) + (-1 * x127 * x132) + (x115 * x151) + (x6 * x152) + (-1 * x156); const FLT x175 = (x95 * x129) + (x92 * x130) + (-1 * x124 * x128); const FLT x176 = (x89 * x95) + (x92 * x94) + (-1 * x96 * x124); const FLT x177 = x89 + (-1 * (x129 + (2 * ((-1 * x95 * x175) + (x97 * x131))))) + (2 * ((-1 * x95 * x176) + (x97 * x125))); const FLT x178 = x94 + (-1 * (x130 + (2 * ((-1 * x124 * x131) + (x95 * x150))))) + (2 * ((-1 * x124 * x125) + (x98 * x95))); const FLT x179 = x178 * x178; const FLT x180 = 1. / x179; const FLT x181 = x177 * x180; const FLT x182 = x115 * x176; const FLT x183 = x119 * (*_x0).Lighthouse.Rot[3]; const FLT x184 = x120 * (*_x0).Lighthouse.Rot[1]; const FLT x185 = x118 * (*_x0).Lighthouse.Rot[0]; const FLT x186 = (-1 * x112 * x115) + (-1 * x185) + (x114 * x127) + x183 + (-1 * x107 * x116) + (-1 * x102 * x143) + (-1 * x12 * x105) + (x3 * x28 * x100) + (-1 * x184); const FLT x187 = x9 * x125; const FLT x188 = x28 * x175; const FLT x189 = x111 * x175; const FLT x190 = x6 * x176; const FLT x191 = 2 * x97; const FLT x192 = x145 * (*_x0).Lighthouse.Rot[3]; const FLT x193 = x169 * x130; const FLT x194 = x128 * x166; const FLT x195 = (x128 * x168) + (-1 * x137 * x143) + (-1 * x194) + x192 + (-1 * x115 * x144) + (-1 * x107 * x139) + (x3 * x141) + (-1 * x193) + (-1 * x138 * x142); const FLT x196 = (x10 * x158) + (-1 * x10 * x155); const FLT x197 = x91 * x176; const FLT x198 = x91 * x175; const FLT x199 = (x7 * x198) + (-1 * x7 * x197); const FLT x200 = x199 + (-1 * x113 * x134) + (-1 * x115 * x189) + (-1 * x6 * x188) + (x9 * x133) + (x111 * x182) + x196 + (x28 * x190) + (-1 * x28 * x187) + (x113 * x132) + (x163 * x191) + (-1 * x123 * x186) + (-1 * x172 * x191) + (x123 * x195); const FLT x201 = 1. / x178; const FLT x202 = -1 * x178; const FLT x203 = atan2(x177, x202); const FLT x204 = x177 * x177; const FLT x205 = 2 * (1. / (x179 + x204)) * x203 * x179 * ((*error_model).BSD1.curve + (*_x0).BSD1.curve); const FLT x206 = x96 + (2 * ((-1 * x98 * x97) + (x124 * x176))) + (-1 * (x128 + (2 * ((-1 * x97 * x150) + (x124 * x175))))); const FLT x207 = x206 * x180; const FLT x208 = x197 * x154; const FLT x209 = x3 * x176; const FLT x210 = x9 * x98; const FLT x211 = x198 * x154; const FLT x212 = x209 * x106; const FLT x213 = (-1 * x212 * x111) + x208 + (x164 * x186) + (x28 * x210) + (x10 * x153) + (-1 * x113 * x151) + (x191 * x149) + (x113 * x157) + (-1 * x211) + (-1 * x164 * x195) + (x3 * x188) + (-1 * x122 * x191) + (x127 * x189) + (-1 * x10 * x173) + (-1 * x28 * x209) + (-1 * x9 * x152); const FLT x214 = x179 + (x206 * x206); const FLT x215 = 1. / x214; const FLT x216 = x215 * x179; const FLT x217 = 1. / sqrt(x214); const FLT x218 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt; const FLT x219 = x218 * x217; const FLT x220 = 2 * x206; const FLT x221 = 2 * x178; const FLT x222 = 1.0/2.0 * x218 * (1. / (x214 * sqrt(x214))) * x177; const FLT x223 = 1. / sqrt(1 + (-1 * (x218 * x218) * x215 * x204)); const FLT x224 = (-1 * x223 * ((-1 * ((x221 * x174) + (x213 * x220)) * x222) + (x219 * x200))) + (-1 * ((x213 * x201) + (-1 * x207 * x174)) * x216); const FLT x225 = 1.5707963267949 + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x206, x202)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (*_x0).BSD1.gibpha + (-1 * asin(x219 * x177)); const FLT x226 = sin(x225) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag); const FLT x227 = x3 * x7; const FLT x228 = x6 * x154; const FLT x229 = 1.0 * (*_x0).Lighthouse.Rot[1]; const FLT x230 = x9 * x229; const FLT x231 = x13 * (*_x0).Lighthouse.Rot[2]; const FLT x232 = (-1 * x231) + x230 + (-1 * x227) + x228; const FLT x233 = x232 * x108; const FLT x234 = x106 * ((-1 * x18 * x233) + (x21 * x228) + (x21 * x230) + (-1 * x19 * x233) + (-1 * x21 * x227) + (-1 * x16 * x233) + (-1 * x21 * x231) + (-1 * x232 * x109)); const FLT x235 = x6 * x234; const FLT x236 = x153 * x154; const FLT x237 = x27 * x232; const FLT x238 = x237 * x100; const FLT x239 = x9 * x234; const FLT x240 = x6 * x237; const FLT x241 = x12 * x237; const FLT x242 = x12 * x234; const FLT x243 = (-1 * x162) + (x235 * x104) + (-1 * x160) + (-1 * x242 * x102) + (-1 * x9 * x238) + x161 + (x240 * x104) + (-1 * x239 * x100) + (-1 * x241 * x102); const FLT x244 = x237 * x131; const FLT x245 = x9 * x237; const FLT x246 = x3 * x237; const FLT x247 = x3 * x234; const FLT x248 = x121 + (x239 * x102) + (-1 * x242 * x100) + (-1 * x247 * x104) + (x245 * x102) + (-1 * x246 * x104) + (-1 * x241 * x100); const FLT x249 = x237 * x150; const FLT x250 = x173 * x154; const FLT x251 = x148 + (x239 * x137) + (-1 * x247 * x142) + (-1 * x241 * x140) + (x245 * x137) + (-1 * x237 * x165) + (-1 * x242 * x140); const FLT x252 = (-1 * x242 * x137) + (x240 * x142) + (-1 * x167) + x170 + (-1 * x171) + (-1 * x245 * x140) + (x235 * x142) + (-1 * x241 * x137) + (-1 * x239 * x140); const FLT x253 = (-1 * x252 * x123) + (-1 * x99 * x237) + (-1 * x3 * x244) + (x243 * x123) + (x234 * x126) + (x237 * x126) + (-1 * x7 * x158) + (x7 * x155) + (-1 * x98 * x235) + x236 + (-1 * x248 * x164) + (x6 * x249) + (-1 * x250) + (x235 * x150) + (x251 * x164) + (-1 * x247 * x131); const FLT x254 = x130 * x146; const FLT x255 = x128 * x147; const FLT x256 = x145 * (*_x0).Lighthouse.Rot[0]; const FLT x257 = x256 + (-1 * x235 * x137) + (x247 * x140) + x255 + (-1 * x240 * x137) + (x246 * x140) + (-1 * x241 * x142) + (-1 * x242 * x142) + (-1 * x254); const FLT x258 = x119 * (*_x0).Lighthouse.Rot[0]; const FLT x259 = x120 * (*_x0).Lighthouse.Rot[2]; const FLT x260 = x118 * (*_x0).Lighthouse.Rot[3]; const FLT x261 = (x247 * x100) + x260 + (-1 * x241 * x104) + (-1 * x259) + x258 + (-1 * x242 * x104) + (-1 * x240 * x102) + (-1 * x235 * x102) + (x3 * x238); const FLT x262 = x237 * x175; const FLT x263 = (x235 * x176) + (x239 * x131) + (-1 * x229 * x158) + (-1 * x234 * x187) + (-1 * x261 * x123) + (x229 * x155) + (-1 * x6 * x262) + (x248 * x191) + (-1 * x235 * x175) + x211 + (-1 * x251 * x191) + (x257 * x123) + (x9 * x244) + (-1 * x237 * x187) + (x237 * x190) + (-1 * x208); const FLT x264 = (x229 * x173) + (-1 * x229 * x153); const FLT x265 = x199 + (x247 * x175) + (-1 * x243 * x191) + (x3 * x262) + (-1 * x9 * x249) + (x252 * x191) + (-1 * x239 * x150) + (x261 * x164) + (-1 * x237 * x209) + (x210 * x234) + (x210 * x237) + (-1 * x234 * x209) + x264 + (-1 * x257 * x164); const FLT x266 = (-1 * x223 * ((-1 * ((x253 * x221) + (x265 * x220)) * x222) + (x219 * x263))) + (-1 * ((x201 * x265) + (-1 * x207 * x253)) * x216); const FLT x267 = x4 * (*_x0).Lighthouse.Rot[2]; const FLT x268 = x6 * x229; const FLT x269 = x9 * x154; const FLT x270 = x13 * (*_x0).Lighthouse.Rot[3]; const FLT x271 = x269 + (-1 * x270) + x267 + (-1 * x268); const FLT x272 = x271 * x108; const FLT x273 = (-1 * x17 * x272) + (-1 * x21 * x268) + (x21 * x267) + (x21 * x269) + (-1 * x19 * x272) + (-1 * x21 * x270) + (-1 * x18 * x272) + (-1 * x16 * x272); const FLT x274 = x98 * x273; const FLT x275 = x273 * x100; const FLT x276 = x27 * x271; const FLT x277 = x276 * x104; const FLT x278 = x273 * x102; const FLT x279 = x276 * x100; const FLT x280 = x276 * x102; const FLT x281 = x273 * x115; const FLT x282 = x184 + (x281 * x104) + (-1 * x183) + x185 + (-1 * x275 * x113) + (-1 * x12 * x280) + (x6 * x277) + (-1 * x278 * x107) + (-1 * x9 * x279); const FLT x283 = x276 * x150; const FLT x284 = x273 * x107; const FLT x285 = x276 * x140; const FLT x286 = x273 * x113; const FLT x287 = x12 * x276; const FLT x288 = x6 * x276; const FLT x289 = (-1 * x287 * x137) + (-1 * x286 * x140) + x193 + (x288 * x142) + (x281 * x142) + (-1 * x192) + x194 + (-1 * x284 * x137) + (-1 * x9 * x285); const FLT x290 = x273 * x125; const FLT x291 = x273 * x127; const FLT x292 = (x286 * x137) + (-1 * x284 * x140) + x254 + (-1 * x291 * x142) + (-1 * x12 * x285) + (-1 * x276 * x165) + (-1 * x255) + (-1 * x256) + (x9 * x276 * x137); const FLT x293 = x276 * x131; const FLT x294 = x273 * x150; const FLT x295 = x273 * x131; const FLT x296 = x273 * x104; const FLT x297 = (-1 * x258) + (-1 * x275 * x107) + (-1 * x260) + (x9 * x280) + (-1 * x296 * x127) + (-1 * x3 * x277) + (-1 * x12 * x279) + (x278 * x113) + x259; const FLT x298 = x264 + x196 + (-1 * x297 * x164) + (x290 * x127) + (x6 * x283) + (-1 * x289 * x123) + (-1 * x274 * x115) + (x282 * x123) + (x292 * x164) + (x276 * x126) + (-1 * x99 * x276) + (x294 * x115) + (-1 * x3 * x293) + (-1 * x295 * x127); const FLT x299 = x121 + (x275 * x127) + (x3 * x279) + (-1 * x281 * x102) + (-1 * x296 * x107) + (-1 * x12 * x277) + (-1 * x6 * x280); const FLT x300 = x148 + (-1 * x287 * x142) + (x291 * x140) + (-1 * x288 * x137) + (-1 * x281 * x137) + (x3 * x285) + (-1 * x284 * x142); const FLT x301 = x273 * x175; const FLT x302 = x276 * x175; const FLT x303 = (-1 * x159) + (-1 * x299 * x123) + x156 + (x276 * x190) + (-1 * x276 * x187) + (x229 * x197) + (x297 * x191) + (x295 * x113) + (-1 * x229 * x198) + (x9 * x293) + (-1 * x292 * x191) + (x300 * x123) + (x273 * x182) + (-1 * x290 * x113) + (-1 * x6 * x302) + (-1 * x301 * x115); const FLT x304 = (-1 * x209 * x276) + (x274 * x113) + (-1 * x282 * x191) + (-1 * x10 * x198) + (x210 * x276) + x250 + (-1 * x9 * x283) + (x10 * x197) + (x3 * x302) + (x299 * x164) + (-1 * x300 * x164) + (-1 * x236) + (x301 * x127) + (x289 * x191) + (-1 * x294 * x113) + (-1 * x212 * x273); const FLT x305 = (-1 * x223 * ((-1 * ((x298 * x221) + (x220 * x304)) * x222) + (x219 * x303))) + (-1 * ((x201 * x304) + (-1 * x207 * x298)) * x216); const FLT x306 = 1. / x25; const FLT x307 = 2 * x306; const FLT x308 = x21 * x307; const FLT x309 = x3 * x9; const FLT x310 = x309 * x308; const FLT x311 = -1 * x310; const FLT x312 = x12 * x308; const FLT x313 = x6 * x312; const FLT x314 = -1 * x313; const FLT x315 = x314 + x311; const FLT x316 = x23 * x307; const FLT x317 = x22 * x307; const FLT x318 = -1 + x317 + x316; const FLT x319 = x9 * x312; const FLT x320 = x6 * x308; const FLT x321 = x3 * x320; const FLT x322 = -1 * x321; const FLT x323 = x322 + x319; const FLT x324 = (-1 * x223 * ((-1 * ((x221 * x315) + (x220 * x323)) * x222) + (x219 * x318))) + (-1 * ((x201 * x323) + (-1 * x207 * x315)) * x216); const FLT x325 = x3 * x312; const FLT x326 = x9 * x320; const FLT x327 = -1 * x326; const FLT x328 = x327 + x325; const FLT x329 = -1 * x319; const FLT x330 = x329 + x322; const FLT x331 = x24 * x307; const FLT x332 = -1 + x331; const FLT x333 = x332 + x317; const FLT x334 = (-1 * x223 * ((-1 * ((x221 * x328) + (x220 * x333)) * x222) + (x219 * x330))) + (-1 * ((x201 * x333) + (-1 * x207 * x328)) * x216); const FLT x335 = x332 + x316; const FLT x336 = x311 + x313; const FLT x337 = -1 * x325; const FLT x338 = x337 + x327; const FLT x339 = (-1 * x223 * ((-1 * ((x221 * x335) + (x220 * x338)) * x222) + (x219 * x336))) + (-1 * ((x201 * x338) + (-1 * x207 * x335)) * x216); const FLT x340 = x29 * x306; const FLT x341 = x21 * x340; const FLT x342 = x12 * x341; const FLT x343 = x6 * x342; const FLT x344 = x309 * x341; const FLT x345 = x344 + x343; const FLT x346 = -1 * x23 * x340; const FLT x347 = -1 * x22 * x340; const FLT x348 = x347 + x30 + x346; const FLT x349 = x6 * x341; const FLT x350 = x3 * x349; const FLT x351 = x9 * x342; const FLT x352 = (-1 * x351) + x350; const FLT x353 = (-1 * x223 * ((-1 * ((x221 * x345) + (x220 * x352)) * x222) + (x219 * x348))) + (-1 * ((x201 * x352) + (-1 * x207 * x345)) * x216); const FLT x354 = x9 * x349; const FLT x355 = x3 * x342; const FLT x356 = (-1 * x355) + x354; const FLT x357 = x350 + x351; const FLT x358 = (-1 * x24 * x340) + x30; const FLT x359 = x358 + x347; const FLT x360 = (-1 * x223 * ((-1 * ((x221 * x356) + (x220 * x359)) * x222) + (x219 * x357))) + (-1 * ((x201 * x359) + (-1 * x207 * x356)) * x216); const FLT x361 = x358 + x346; const FLT x362 = (-1 * x343) + x344; const FLT x363 = x354 + x355; const FLT x364 = (-1 * x223 * ((-1 * ((x221 * x361) + (x220 * x363)) * x222) + (x219 * x362))) + (-1 * ((x201 * x363) + (-1 * x207 * x361)) * x216); const FLT x365 = 1.0 * x54; const FLT x366 = 1.0 * (*_x0).Object.Pose.Rot[1]; const FLT x367 = 1.0 * x51; const FLT x368 = 1.0 * x52; const FLT x369 = (x368 * (*_x0).Object.Pose.Rot[3]) + (-1 * x367 * (*_x0).Object.Pose.Rot[2]) + (x365 * (*_x0).Object.Pose.Rot[0]) + (-1 * x53 * x366); const FLT x370 = 1.0/2.0 * (1. / (x55 * sqrt(x55))); const FLT x371 = x60 * x370; const FLT x372 = x371 * x369; const FLT x373 = x62 * x372; const FLT x374 = x66 * x370; const FLT x375 = x374 * x369; const FLT x376 = x35 * x375; const FLT x377 = x47 * x370; const FLT x378 = x377 * x369; const FLT x379 = x38 * x375; const FLT x380 = x56 * x47; const FLT x381 = 0.5 * x62; const FLT x382 = x381 * (*_x0).Object.Pose.Rot[3]; const FLT x383 = x67 * x38; const FLT x384 = x381 * (*_x0).Object.Pose.Rot[2]; const FLT x385 = 0.5 * x76; const FLT x386 = x381 * (*_x0).Object.Pose.Rot[1]; const FLT x387 = x67 * x35; const FLT x388 = (-1 * x387 * x386) + (x385 * (*_x0).Object.Pose.Rot[0]) + (-1 * x382 * x380) + (-1 * x383 * x384); const FLT x389 = (-1 * x70 * x379) + (x72 * x378) + x388 + (-1 * x54 * x373) + (-1 * x63 * x376); const FLT x390 = 2 * x88; const FLT x391 = x382 * x387; const FLT x392 = x381 * (*_x0).Object.Pose.Rot[0]; const FLT x393 = x392 * x383; const FLT x394 = x381 * x380; const FLT x395 = x394 * (*_x0).Object.Pose.Rot[1]; const FLT x396 = x385 * (*_x0).Object.Pose.Rot[2]; const FLT x397 = (-1 * x396) + (-1 * x395) + x391 + (-1 * x393); const FLT x398 = x397 + (x65 * x379) + (-1 * x72 * x376) + (-1 * x70 * x372) + (-1 * x63 * x378); const FLT x399 = x392 * x387; const FLT x400 = x394 * (*_x0).Object.Pose.Rot[2]; const FLT x401 = x383 * x382; const FLT x402 = x385 * (*_x0).Object.Pose.Rot[1]; const FLT x403 = (-1 * x401) + (-1 * x402) + (-1 * x399) + x400; const FLT x404 = x403 + (x72 * x379) + (x70 * x378) + (x65 * x376) + (-1 * x63 * x372); const FLT x405 = x384 * x387; const FLT x406 = x383 * x386; const FLT x407 = x394 * (*_x0).Object.Pose.Rot[0]; const FLT x408 = x385 * (*_x0).Object.Pose.Rot[3]; const FLT x409 = (-1 * x52 * x373) + x408 + x407 + (-1 * x63 * x379) + (-1 * x406) + x405 + (x70 * x376) + (-1 * x65 * x378); const FLT x410 = (x409 * sensor_pt[2]) + (-1 * x398 * sensor_pt[1]) + (x404 * sensor_pt[0]); const FLT x411 = 2 * x74; const FLT x412 = 2 * x93; const FLT x413 = (-1 * x409 * sensor_pt[0]) + (x389 * sensor_pt[1]) + (x404 * sensor_pt[2]); const FLT x414 = 2 * x83; const FLT x415 = (x412 * x398) + (-1 * x413 * x414) + (-1 * x390 * x389) + (x410 * x411); const FLT x416 = 2 * x84; const FLT x417 = 2 * x87; const FLT x418 = (x398 * sensor_pt[0]) + (-1 * x389 * sensor_pt[2]) + (x404 * sensor_pt[1]); const FLT x419 = (x409 * x390) + (-1 * x416 * x398) + (-1 * x411 * x418) + (x413 * x417); const FLT x420 = (x414 * x418) + (x416 * x389) + (-1 * x409 * x412) + (-1 * x410 * x417); const FLT x421 = (-1 * x95 * x420) + (x97 * x415) + (x92 * x419); const FLT x422 = (x92 * x415) + (-1 * x97 * x419) + (x420 * x124); const FLT x423 = x420 + (x421 * x123) + (-1 * x422 * x164); const FLT x424 = (x95 * x419) + (-1 * x415 * x124) + (x92 * x420); const FLT x425 = x419 + (x422 * x191) + (-1 * x424 * x123); const FLT x426 = x415 + (x424 * x164) + (-1 * x421 * x191); const FLT x427 = (-1 * x223 * ((-1 * ((x423 * x221) + (x426 * x220)) * x222) + (x425 * x219))) + (-1 * ((x426 * x201) + (-1 * x423 * x207)) * x216); const FLT x428 = 1.0 * x53; const FLT x429 = (x368 * (*_x0).Object.Pose.Rot[0]) + (-1 * x428 * (*_x0).Object.Pose.Rot[2]) + (-1 * x365 * (*_x0).Object.Pose.Rot[3]) + (x51 * x366); const FLT x430 = x63 * x429; const FLT x431 = x35 * x374; const FLT x432 = x429 * x377; const FLT x433 = x429 * x371; const FLT x434 = x38 * x374; const FLT x435 = x429 * x434; const FLT x436 = x406 + (-1 * x407) + (-1 * x408) + (-1 * x405); const FLT x437 = (-1 * x70 * x435) + (-1 * x65 * x433) + x436 + (-1 * x430 * x431) + (x72 * x432); const FLT x438 = x429 * x431; const FLT x439 = (-1 * x400) + (-1 * x430 * x377) + x402 + (x65 * x435) + (-1 * x72 * x438) + (-1 * x70 * x433) + x401 + x399; const FLT x440 = x397 + (-1 * x63 * x433) + (x72 * x435) + (x65 * x438) + (x70 * x432); const FLT x441 = (-1 * x434 * x430) + (-1 * x72 * x433) + (x70 * x438) + x388 + (-1 * x65 * x432); const FLT x442 = (x441 * sensor_pt[2]) + (-1 * x439 * sensor_pt[1]) + (x440 * sensor_pt[0]); const FLT x443 = (x437 * sensor_pt[1]) + (-1 * x441 * sensor_pt[0]) + (x440 * sensor_pt[2]); const FLT x444 = (-1 * x414 * x443) + (x411 * x442) + (-1 * x437 * x390) + (x412 * x439); const FLT x445 = (x439 * sensor_pt[0]) + (-1 * x437 * sensor_pt[2]) + (x440 * sensor_pt[1]); const FLT x446 = (x417 * x443) + (x441 * x390) + (-1 * x416 * x439) + (-1 * x411 * x445); const FLT x447 = (x416 * x437) + (-1 * x417 * x442) + (-1 * x412 * x441) + (x414 * x445); const FLT x448 = (-1 * x95 * x447) + (x97 * x444) + (x92 * x446); const FLT x449 = (-1 * x97 * x446) + (x447 * x124) + (x92 * x444); const FLT x450 = x447 + (x448 * x123) + (-1 * x449 * x164); const FLT x451 = (-1 * x444 * x124) + (x92 * x447) + (x95 * x446); const FLT x452 = (x449 * x191) + x446 + (-1 * x451 * x123); const FLT x453 = (x451 * x164) + x444 + (-1 * x448 * x191); const FLT x454 = (-1 * x223 * ((-1 * ((x450 * x221) + (x453 * x220)) * x222) + (x452 * x219))) + (-1 * ((x453 * x201) + (-1 * x450 * x207)) * x216); const FLT x455 = (-1 * x52 * x366) + (-1 * x428 * (*_x0).Object.Pose.Rot[3]) + (x365 * (*_x0).Object.Pose.Rot[2]) + (x367 * (*_x0).Object.Pose.Rot[0]); const FLT x456 = x455 * x374; const FLT x457 = x35 * x456; const FLT x458 = x455 * x371; const FLT x459 = x62 * x458; const FLT x460 = x455 * x377; const FLT x461 = x38 * x456; const FLT x462 = (-1 * x70 * x461) + x393 + (-1 * x391) + (-1 * x54 * x459) + (-1 * x63 * x457) + (x72 * x460) + x395 + x396; const FLT x463 = (x72 * x461) + (x70 * x460) + x436 + (-1 * x63 * x458) + (x65 * x457); const FLT x464 = (-1 * x52 * x459) + x403 + (-1 * x65 * x460) + (-1 * x63 * x461) + (x70 * x457); const FLT x465 = (x462 * sensor_pt[1]) + (x463 * sensor_pt[2]) + (-1 * x464 * sensor_pt[0]); const FLT x466 = (-1 * x70 * x458) + (-1 * x63 * x460) + x388 + (x65 * x461) + (-1 * x72 * x457); const FLT x467 = (x464 * sensor_pt[2]) + (-1 * x466 * sensor_pt[1]) + (x463 * sensor_pt[0]); const FLT x468 = (x467 * x411) + (x466 * x412) + (-1 * x462 * x390) + (-1 * x465 * x414); const FLT x469 = (x466 * sensor_pt[0]) + (-1 * x462 * sensor_pt[2]) + (x463 * sensor_pt[1]); const FLT x470 = (x465 * x417) + (x464 * x390) + (-1 * x466 * x416) + (-1 * x469 * x411); const FLT x471 = (x469 * x414) + (x462 * x416) + (-1 * x464 * x412) + (-1 * x467 * x417); const FLT x472 = (-1 * x95 * x471) + (x97 * x468) + (x92 * x470); const FLT x473 = (-1 * x97 * x470) + (x471 * x124) + (x92 * x468); const FLT x474 = x471 + (x472 * x123) + (-1 * x473 * x164); const FLT x475 = (x95 * x470) + (-1 * x468 * x124) + (x92 * x471); const FLT x476 = x470 + (x473 * x191) + (-1 * x475 * x123); const FLT x477 = x468 + (x475 * x164) + (-1 * x472 * x191); const FLT x478 = (-1 * x223 * ((-1 * ((x474 * x221) + (x477 * x220)) * x222) + (x476 * x219))) + (-1 * ((x477 * x201) + (-1 * x474 * x207)) * x216); const FLT x479 = x310 + x313; const FLT x480 = -1 * x317; const FLT x481 = -1 * x316; const FLT x482 = 1 + x481 + x480; const FLT x483 = x329 + x321; const FLT x484 = (-1 * x223 * ((-1 * ((x479 * x221) + (x483 * x220)) * x222) + (x482 * x219))) + (-1 * ((x483 * x201) + (-1 * x479 * x207)) * x216); const FLT x485 = x337 + x326; const FLT x486 = x321 + x319; const FLT x487 = 1 + (-1 * x331); const FLT x488 = x487 + x480; const FLT x489 = (-1 * x223 * ((-1 * ((x485 * x221) + (x488 * x220)) * x222) + (x486 * x219))) + (-1 * ((x488 * x201) + (-1 * x485 * x207)) * x216); const FLT x490 = x487 + x481; const FLT x491 = x314 + x310; const FLT x492 = x326 + x325; const FLT x493 = (-1 * x223 * ((-1 * ((x490 * x221) + (x492 * x220)) * x222) + (x491 * x219))) + (-1 * ((x492 * x201) + (-1 * x490 * x207)) * x216); const FLT x494 = dt * dt * dt; const FLT x495 = 0.5 * x58 * x494; const FLT x496 = x31 * x35; const FLT x497 = x495 * x496; const FLT x498 = x85 * x497; const FLT x499 = x38 * x495; const FLT x500 = x35 * x499; const FLT x501 = x71 * x500; const FLT x502 = x36 * x495; const FLT x503 = x35 * x35 * x35; const FLT x504 = dt * dt * dt * dt; const FLT x505 = 2 * x57 * (1. / (x41 * x41)); const FLT x506 = x504 * x505; const FLT x507 = x33 * x35; const FLT x508 = 1.0 * x60; const FLT x509 = (1. / (x41 * sqrt(x41))) * x44; const FLT x510 = x508 * x509; const FLT x511 = x510 * x504; const FLT x512 = x39 * x506; const FLT x513 = 2 * x59; const FLT x514 = x32 * x513; const FLT x515 = x39 * x511; const FLT x516 = x45 * x508; const FLT x517 = x32 * x35; const FLT x518 = (-1 * x517 * x516) + (x511 * x503) + (x35 * x515) + (x511 * x507) + (-1 * x503 * x506) + (-1 * x35 * x512) + (x35 * x514) + (-1 * x506 * x507); const FLT x519 = 1.0/2.0 * (1. / (x61 * sqrt(x61))); const FLT x520 = x519 * x383; const FLT x521 = x518 * x520; const FLT x522 = x519 * x380; const FLT x523 = x518 * x522; const FLT x524 = 0.5 * x86; const FLT x525 = x45 * x517; const FLT x526 = x69 * x519; const FLT x527 = x518 * x526; const FLT x528 = x494 * x509; const FLT x529 = x38 * x528; const FLT x530 = x35 * x529; const FLT x531 = x75 * x530; const FLT x532 = x519 * x387; const FLT x533 = x518 * x532; const FLT x534 = x496 * x528; const FLT x535 = x79 * x534; const FLT x536 = x36 * x528; const FLT x537 = (-1 * x64 * x536) + (-1 * x51 * x521) + (x77 * x502) + (-1 * x53 * x533) + x501 + x82 + x535 + (x52 * x523) + (-1 * x531) + (-1 * x524 * x525) + (-1 * x498) + (-1 * x54 * x527); const FLT x538 = 0.5 * x525; const FLT x539 = (-1 * x64 * x534) + (x77 * x497); const FLT x540 = (-1 * x81 * x500) + (x86 * x530); const FLT x541 = (-1 * x51 * x527) + x539 + (-1 * x75 * x538) + (x85 * x502) + x540 + (x54 * x521) + (-1 * x79 * x536) + x73 + (-1 * x53 * x523) + (-1 * x52 * x533); const FLT x542 = -1 * x68; const FLT x543 = x79 * x530; const FLT x544 = x85 * x500; const FLT x545 = (x75 * x534) + (-1 * x71 * x497); const FLT x546 = x545 + (-1 * x81 * x502) + x543 + (-1 * x64 * x538) + (x54 * x533) + (-1 * x544) + (x86 * x536) + x542 + (-1 * x53 * x527) + (x52 * x521) + (x51 * x523); const FLT x547 = -1 * x80; const FLT x548 = x86 * x534; const FLT x549 = x81 * x497; const FLT x550 = (-1 * x64 * x530) + (x77 * x500); const FLT x551 = x550 + x549 + (x75 * x536) + (-1 * x548) + (-1 * x52 * x527) + (x51 * x533) + (-1 * x79 * x538) + (-1 * x54 * x523) + (-1 * x71 * x502) + x547 + (-1 * x53 * x521); const FLT x552 = 2 * ((-1 * x541 * sensor_pt[1]) + (x551 * sensor_pt[2]) + (x546 * sensor_pt[0])); const FLT x553 = (x537 * sensor_pt[1]) + (-1 * x551 * sensor_pt[0]) + (x546 * sensor_pt[2]); const FLT x554 = (-1 * x414 * x553) + (x74 * x552) + (-1 * x537 * x390) + (x412 * x541); const FLT x555 = (x541 * sensor_pt[0]) + (-1 * x537 * sensor_pt[2]) + (x546 * sensor_pt[1]); const FLT x556 = (x551 * x390) + (-1 * x411 * x555) + (-1 * x416 * x541) + (x417 * x553); const FLT x557 = (x414 * x555) + (x416 * x537) + (-1 * x412 * x551) + (-1 * x87 * x552); const FLT x558 = (-1 * x95 * x557) + (x97 * x554) + (x92 * x556); const FLT x559 = (-1 * x97 * x556) + (x557 * x124) + (x92 * x554); const FLT x560 = x557 + (x558 * x123) + (-1 * x559 * x164); const FLT x561 = (x95 * x556) + (-1 * x554 * x124) + (x92 * x557); const FLT x562 = x556 + (x559 * x191) + (-1 * x561 * x123); const FLT x563 = x554 + (x561 * x164) + (-1 * x558 * x191); const FLT x564 = (-1 * x223 * ((-1 * ((x560 * x221) + (x563 * x220)) * x222) + (x562 * x219))) + (-1 * ((x563 * x201) + (-1 * x560 * x207)) * x216); const FLT x565 = x39 * x495; const FLT x566 = x39 * x528; const FLT x567 = x38 * x506; const FLT x568 = x38 * x511; const FLT x569 = x32 * x38; const FLT x570 = (x38 * x38 * x38) * x504; const FLT x571 = (x513 * x569) + (x510 * x570) + (x36 * x568) + (-1 * x36 * x567) + (-1 * x516 * x569) + (x33 * x568) + (-1 * x505 * x570) + (-1 * x33 * x567); const FLT x572 = x532 * x571; const FLT x573 = x53 * x571; const FLT x574 = x54 * x571; const FLT x575 = x526 * x571; const FLT x576 = x45 * x569; const FLT x577 = 0.5 * x576; const FLT x578 = x31 * x529; const FLT x579 = x31 * x499; const FLT x580 = (x77 * x579) + (-1 * x64 * x578); const FLT x581 = x542 + x580 + (-1 * x75 * x577) + (-1 * x51 * x575) + (-1 * x543) + (x520 * x574) + (x86 * x566) + (-1 * x81 * x565) + x544 + (-1 * x52 * x572) + (-1 * x522 * x573); const FLT x582 = x86 * x578; const FLT x583 = x81 * x579; const FLT x584 = (x51 * x572) + (x77 * x565) + (-1 * x582) + (-1 * x64 * x566) + x82 + (-1 * x501) + x531 + (-1 * x52 * x575) + (-1 * x522 * x574) + (-1 * x520 * x573) + (-1 * x79 * x577) + x583; const FLT x585 = -1 * x73; const FLT x586 = x75 * x578; const FLT x587 = x71 * x579; const FLT x588 = x51 * x571; const FLT x589 = x52 * x571; const FLT x590 = (x532 * x574) + (x522 * x588) + (-1 * x53 * x575) + (-1 * x85 * x565) + x586 + x540 + (x520 * x589) + x585 + (-1 * x64 * x577) + (-1 * x587) + (x79 * x566); const FLT x591 = (x79 * x578) + (-1 * x85 * x579); const FLT x592 = x550 + (-1 * x524 * x576) + (-1 * x520 * x588) + x591 + x80 + (-1 * x54 * x575) + (-1 * x532 * x573) + (-1 * x75 * x566) + (x71 * x565) + (x522 * x589); const FLT x593 = (x592 * sensor_pt[1]) + (-1 * x584 * sensor_pt[0]) + (x590 * sensor_pt[2]); const FLT x594 = (x581 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x590 * sensor_pt[1]); const FLT x595 = (-1 * x411 * x594) + (-1 * x416 * x581) + (x584 * x390) + (x417 * x593); const FLT x596 = (x584 * sensor_pt[2]) + (-1 * x581 * sensor_pt[1]) + (x590 * sensor_pt[0]); const FLT x597 = (x411 * x596) + (-1 * x414 * x593) + (-1 * x592 * x390) + (x412 * x581); const FLT x598 = (-1 * x417 * x596) + (x414 * x594) + (-1 * x412 * x584) + (x416 * x592); const FLT x599 = (-1 * x95 * x598) + (x92 * x595) + (x97 * x597); const FLT x600 = (-1 * x97 * x595) + (x598 * x124) + (x92 * x597); const FLT x601 = x598 + (x599 * x123) + (-1 * x600 * x164); const FLT x602 = (-1 * x597 * x124) + (x95 * x595) + (x92 * x598); const FLT x603 = x595 + (x600 * x191) + (-1 * x602 * x123); const FLT x604 = x597 + (-1 * x599 * x191) + (x602 * x164); const FLT x605 = (-1 * x223 * ((-1 * ((x601 * x221) + (x604 * x220)) * x222) + (x603 * x219))) + (-1 * ((x604 * x201) + (-1 * x601 * x207)) * x216); const FLT x606 = x33 * x528; const FLT x607 = x46 * x32; const FLT x608 = x33 * x495; const FLT x609 = x31 * x36; const FLT x610 = x31 * x31 * x31; const FLT x611 = (x610 * x511) + (-1 * x610 * x506) + (x31 * x515) + (x609 * x511) + (-1 * x609 * x506) + (-1 * x607 * x508) + (x31 * x514) + (-1 * x31 * x512); const FLT x612 = x611 * x519; const FLT x613 = x612 * x387; const FLT x614 = x611 * x526; const FLT x615 = x612 * x380; const FLT x616 = x612 * x383; const FLT x617 = x539 + x587 + (-1 * x607 * x524) + (-1 * x51 * x616) + (x52 * x615) + x585 + (x79 * x606) + (-1 * x54 * x614) + (-1 * x53 * x613) + (-1 * x586) + (-1 * x85 * x608); const FLT x618 = 0.5 * x607; const FLT x619 = (-1 * x64 * x606) + (-1 * x53 * x615) + x498 + (-1 * x583) + (-1 * x52 * x613) + (-1 * x535) + x582 + (x54 * x616) + x82 + (x77 * x608) + (-1 * x75 * x618) + (-1 * x51 * x614); const FLT x620 = x591 + (-1 * x64 * x618) + (x75 * x606) + (x54 * x613) + (-1 * x71 * x608) + (x52 * x616) + (x51 * x615) + x548 + (-1 * x549) + (-1 * x53 * x614) + x547; const FLT x621 = x545 + (-1 * x86 * x606) + x580 + x68 + (x51 * x613) + (-1 * x52 * x614) + (-1 * x79 * x618) + (-1 * x54 * x615) + (-1 * x53 * x616) + (x81 * x608); const FLT x622 = (x617 * sensor_pt[1]) + (x620 * sensor_pt[2]) + (-1 * x621 * sensor_pt[0]); const FLT x623 = (x621 * sensor_pt[2]) + (-1 * x619 * sensor_pt[1]) + (x620 * sensor_pt[0]); const FLT x624 = (x411 * x623) + (-1 * x414 * x622) + (-1 * x617 * x390) + (x412 * x619); const FLT x625 = (x619 * sensor_pt[0]) + (-1 * x617 * sensor_pt[2]) + (x620 * sensor_pt[1]); const FLT x626 = (x417 * x622) + (x621 * x390) + (-1 * x416 * x619) + (-1 * x411 * x625); const FLT x627 = (x414 * x625) + (-1 * x417 * x623) + (-1 * x412 * x621) + (x416 * x617); const FLT x628 = x91 * x627; const FLT x629 = (x97 * x624) + (-1 * x6 * x628) + (x92 * x626); const FLT x630 = (x3 * x628) + (-1 * x97 * x626) + (x92 * x624); const FLT x631 = x627 + (x629 * x123) + (-1 * x630 * x164); const FLT x632 = (-1 * x624 * x124) + (x95 * x626) + (x92 * x627); const FLT x633 = x626 + (x630 * x191) + (-1 * x632 * x123); const FLT x634 = x624 + (x632 * x164) + (-1 * x629 * x191); const FLT x635 = (-1 * x223 * ((-1 * ((x631 * x221) + (x634 * x220)) * x222) + (x633 * x219))) + (-1 * ((x634 * x201) + (-1 * x631 * x207)) * x216); const FLT x636 = dt * x313; const FLT x637 = dt * x310; const FLT x638 = x637 + x636; const FLT x639 = -1 * dt * x316; const FLT x640 = (-1 * dt * x317) + dt; const FLT x641 = x640 + x639; const FLT x642 = dt * x321; const FLT x643 = dt * x319; const FLT x644 = (-1 * x643) + x642; const FLT x645 = (-1 * x223 * ((-1 * ((x638 * x221) + (x644 * x220)) * x222) + (x641 * x219))) + (-1 * ((x644 * x201) + (-1 * x638 * x207)) * x216); const FLT x646 = dt * x326; const FLT x647 = dt * x325; const FLT x648 = (-1 * x647) + x646; const FLT x649 = x642 + x643; const FLT x650 = -1 * dt * x331; const FLT x651 = x640 + x650; const FLT x652 = (-1 * x223 * ((-1 * ((x648 * x221) + (x651 * x220)) * x222) + (x649 * x219))) + (-1 * ((x651 * x201) + (-1 * x648 * x207)) * x216); const FLT x653 = x650 + x639 + dt; const FLT x654 = (-1 * x636) + x637; const FLT x655 = x646 + x647; const FLT x656 = (-1 * x223 * ((-1 * ((x653 * x221) + (x655 * x220)) * x222) + (x654 * x219))) + (-1 * ((x655 * x201) + (-1 * x653 * x207)) * x216); const FLT x657 = x217 * x223 * x177; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x224 + (((-1 * x200 * x201) + (x174 * x181)) * x205) + (x224 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x266 + (((-1 * x201 * x263) + (x253 * x181)) * x205) + (x266 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (((-1 * x201 * x303) + (x298 * x181)) * x205) + x305 + (x226 * x305)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x324 + (((-1 * x201 * x318) + (x315 * x181)) * x205) + (x226 * x324)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x334 + (((-1 * x201 * x330) + (x328 * x181)) * x205) + (x226 * x334)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x339 + (((-1 * x201 * x336) + (x335 * x181)) * x205) + (x226 * x339)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x353 + (((-1 * x201 * x348) + (x345 * x181)) * x205) + (x226 * x353)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x360 + (((-1 * x201 * x357) + (x356 * x181)) * x205) + (x226 * x360)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x364 + (((-1 * x201 * x362) + (x361 * x181)) * x205) + (x226 * x364)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (((-1 * x425 * x201) + (x423 * x181)) * x205) + x427 + (x427 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x454 + (((-1 * x452 * x201) + (x450 * x181)) * x205) + (x454 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x478 + (((-1 * x476 * x201) + (x474 * x181)) * x205) + (x478 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x484 + (((-1 * x482 * x201) + (x479 * x181)) * x205) + (x484 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), (((-1 * x486 * x201) + (x485 * x181)) * x205) + x489 + (x489 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x493 + (((-1 * x491 * x201) + (x490 * x181)) * x205) + (x493 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x564 + (((-1 * x562 * x201) + (x560 * x181)) * x205) + (x564 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x605 + (((-1 * x603 * x201) + (x601 * x181)) * x205) + (x605 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), (((-1 * x633 * x201) + (x631 * x181)) * x205) + x635 + (x635 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x645 + (((-1 * x641 * x201) + (x638 * x181)) * x205) + (x645 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x652 + (((-1 * x649 * x201) + (x648 * x181)) * x205) + (x652 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x656 + (((-1 * x654 * x201) + (x653 * x181)) * x205) + (x656 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), x203 * x203); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), -1 * cos(x225)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), x226); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), -1 + (-1 * x226)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), (-1 * x657 * x226) + (-1 * x657)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x3 = (x1 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x4 = dt * dt; const FLT x5 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x6 = x4 * (x5 * x5); const FLT x7 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x8 = x4 * (x7 * x7); const FLT x9 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x10 = x4 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = cos(x13); const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x19 = sin(x13); const FLT x20 = (1. / x11) * (x19 * x19); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x16 * x16) + (x3 * x3))) * (1. / sqrt((x8 * x20) + (x14 * x14) + (x6 * x20) + (x20 * x10))); const FLT x22 = x21 * x14; const FLT x23 = x3 * x22; const FLT x24 = dt * x21 * (1. / x12) * x19; const FLT x25 = x9 * x24; const FLT x26 = x25 * x17; const FLT x27 = x24 * x16; const FLT x28 = x7 * x27; const FLT x29 = x5 * x24; const FLT x30 = x29 * x18; const FLT x31 = (-1 * x30) + x28 + (-1 * x23) + (-1 * x26); const FLT x32 = x3 * x29; const FLT x33 = x9 * x27; const FLT x34 = x22 * x18; const FLT x35 = x7 * x24; const FLT x36 = x35 * x17; const FLT x37 = x34 + x36 + (-1 * x32) + x33; const FLT x38 = 2 * x37; const FLT x39 = (-1 * x35 * x18) + (x22 * x17) + (-1 * x5 * x27) + (-1 * x3 * x25); const FLT x40 = x29 * x17; const FLT x41 = x25 * x18; const FLT x42 = x22 * x16; const FLT x43 = x3 * x35; const FLT x44 = x43 + x42 + x40 + (-1 * x41); const FLT x45 = 2 * x44; const FLT x46 = x45 * x39; const FLT x47 = x46 + (-1 * x31 * x38); const FLT x48 = 0.5 * (*_x0).Lighthouse.Rot[2]; const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x50 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x51 = (x50 * (*_x0).Lighthouse.Rot[1]) + (x49 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x48 * (*error_model).Lighthouse.AxisAngleRot[0]); const FLT x52 = x51 * x51; const FLT x53 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x54 = (-1 * x53 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*error_model).Lighthouse.AxisAngleRot[1]); const FLT x55 = x54 * x54; const FLT x56 = (x50 * (*_x0).Lighthouse.Rot[0]) + (x53 * (*_x0).Lighthouse.Rot[3]) + (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[2]; const FLT x57 = x56 * x56; const FLT x58 = (*_x0).Lighthouse.Rot[1] + (x53 * (*_x0).Lighthouse.Rot[0]) + (-1 * x50 * (*_x0).Lighthouse.Rot[3]) + (x49 * (*_x0).Lighthouse.Rot[2]); const FLT x59 = x58 * x58; const FLT x60 = x57 + x59 + x52 + x55; const FLT x61 = 1. / x60; const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x52) + (x61 * x55) + (x61 * x57))) * (1. / sqrt(x60)); const FLT x63 = x62 * x51; const FLT x64 = (-1 * x28) + x30 + x26 + x23; const FLT x65 = 2 * x64; const FLT x66 = 1 + (x65 * x31) + (-2 * (x44 * x44)); const FLT x67 = x62 * x54; const FLT x68 = x65 * x39; const FLT x69 = (x44 * x38) + (-1 * x68); const FLT x70 = x62 * x56; const FLT x71 = (-1 * x70 * x69) + (x63 * x47) + (x67 * x66); const FLT x72 = 2 * x70; const FLT x73 = x62 * x58; const FLT x74 = (-1 * x63 * x66) + (x73 * x69) + (x67 * x47); const FLT x75 = 2 * x73; const FLT x76 = x69 + (x71 * x72) + (-1 * x75 * x74); const FLT x77 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x78 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x79 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x80 = (x79 * x63) + (-1 * x70 * x77) + (x78 * x67); const FLT x81 = (x79 * x67) + (x73 * x77) + (-1 * x78 * x63); const FLT x82 = 1.0/2.0 * dt * fabs(dt); const FLT x83 = (-1 * x37 * sensor_pt[2]) + (x39 * sensor_pt[1]) + (x44 * sensor_pt[0]); const FLT x84 = (-1 * x64 * sensor_pt[0]) + (x39 * sensor_pt[2]) + (x37 * sensor_pt[1]); const FLT x85 = (*_x0).Object.Pose.Pos[0] + (x82 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x84 * x64) + (-1 * x83 * x44))); const FLT x86 = x85 * x62; const FLT x87 = (-1 * x44 * sensor_pt[1]) + (x39 * sensor_pt[0]) + (x64 * sensor_pt[2]); const FLT x88 = (*_x0).Object.Pose.Pos[2] + (2 * ((x83 * x37) + (-1 * x87 * x64))) + (x82 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])); const FLT x89 = x88 * x62; const FLT x90 = (*_x0).Object.Pose.Pos[1] + (x82 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x87 * x44) + (-1 * x84 * x37))); const FLT x91 = x62 * ((x63 * x90) + (x86 * x54) + (-1 * x89 * x56)); const FLT x92 = (x89 * x58) + (x67 * x90) + (-1 * x86 * x51); const FLT x93 = x88 + (-1 * (x77 + (2 * ((-1 * x81 * x73) + (x80 * x70))))) + (2 * ((-1 * x73 * x92) + (x56 * x91))); const FLT x94 = x93 * x93; const FLT x95 = 1. / x94; const FLT x96 = (x77 * x67) + (x70 * x78) + (-1 * x73 * x79); const FLT x97 = (x86 * x56) + (x89 * x54) + (-1 * x73 * x90); const FLT x98 = x85 + (-1 * (x78 + (2 * ((-1 * x70 * x96) + (x81 * x63))))) + (2 * ((-1 * x70 * x97) + (x63 * x92))); const FLT x99 = x98 * x95; const FLT x100 = 1. / x93; const FLT x101 = (-1 * x73 * x47) + (x70 * x66) + (x67 * x69); const FLT x102 = 2 * x63; const FLT x103 = x66 + (-1 * x72 * x101) + (x74 * x102); const FLT x104 = -1 * x93; const FLT x105 = x98 * x98; const FLT x106 = 2 * x94 * atan2(x98, x104) * (1. / (x94 + x105)) * ((*error_model).BSD1.curve + (*_x0).BSD1.curve); const FLT x107 = x90 + (2 * ((-1 * x51 * x91) + (x73 * x97))) + (-1 * (x79 + (2 * ((-1 * x80 * x63) + (x73 * x96))))); const FLT x108 = x95 * x107; const FLT x109 = x47 + (-1 * x71 * x102) + (x75 * x101); const FLT x110 = (x107 * x107) + x94; const FLT x111 = 1. / x110; const FLT x112 = x94 * x111; const FLT x113 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt; const FLT x114 = (1. / sqrt(x110)) * x113; const FLT x115 = 2 * x107; const FLT x116 = 2 * x93; const FLT x117 = 1.0/2.0 * x98 * (1. / (x110 * sqrt(x110))) * x113; const FLT x118 = 1. / sqrt(1 + (-1 * x105 * x111 * (x113 * x113))); const FLT x119 = (-1 * x118 * ((-1 * x117 * ((x76 * x116) + (x109 * x115))) + (x103 * x114))) + (-1 * x112 * ((x100 * x109) + (-1 * x76 * x108))); const FLT x120 = sin(1.5707963267949 + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x107, x104)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (*_x0).BSD1.gibpha + (-1 * asin(x98 * x114))) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag); const FLT x121 = (-1 * x40) + (-1 * x43) + x41 + (-1 * x42); const FLT x122 = 1 + (x45 * x121) + (-2 * (x37 * x37)); const FLT x123 = (x65 * x37) + (-1 * x46); const FLT x124 = x38 * x39; const FLT x125 = x124 + (-1 * x65 * x121); const FLT x126 = (-1 * x70 * x125) + (x63 * x122) + (x67 * x123); const FLT x127 = (-1 * x63 * x123) + (x73 * x125) + (x67 * x122); const FLT x128 = x125 + (x72 * x126) + (-1 * x75 * x127); const FLT x129 = (-1 * x73 * x122) + (x70 * x123) + (x67 * x125); const FLT x130 = x123 + (-1 * x72 * x129) + (x102 * x127); const FLT x131 = (-1 * x102 * x126) + x122 + (x75 * x129); const FLT x132 = (-1 * x118 * ((-1 * ((x116 * x128) + (x115 * x131)) * x117) + (x114 * x130))) + (-1 * ((x100 * x131) + (-1 * x108 * x128)) * x112); const FLT x133 = (x65 * x44) + (-1 * x124); const FLT x134 = (-1 * x34) + (-1 * x33) + (-1 * x36) + x32; const FLT x135 = x68 + (-1 * x45 * x134); const FLT x136 = 1 + (x38 * x134) + (-2 * (x64 * x64)); const FLT x137 = (-1 * x70 * x136) + (x63 * x133) + (x67 * x135); const FLT x138 = x62 * x136; const FLT x139 = (-1 * x63 * x135) + (x58 * x138) + (x67 * x133); const FLT x140 = x136 + (x72 * x137) + (-1 * x75 * x139); const FLT x141 = x95 * x140; const FLT x142 = (-1 * x73 * x133) + (x70 * x135) + (x54 * x138); const FLT x143 = x135 + (-1 * x72 * x142) + (x102 * x139); const FLT x144 = x133 + (-1 * x102 * x137) + (x75 * x142); const FLT x145 = (-1 * x118 * ((-1 * ((x116 * x140) + (x115 * x144)) * x117) + (x114 * x143))) + (-1 * ((x100 * x144) + (-1 * x107 * x141)) * x112); cnMatrixOptionalSet(Hx, 0, 0, x119 + (x106 * ((-1 * x100 * x103) + (x76 * x99))) + (x119 * x120)); cnMatrixOptionalSet(Hx, 0, 1, x132 + (x106 * ((-1 * x100 * x130) + (x99 * x128))) + (x120 * x132)); cnMatrixOptionalSet(Hx, 0, 2, x145 + (x106 * ((-1 * x100 * x143) + (x98 * x141))) + (x120 * x145)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt); } } static inline FLT SurviveJointKalmanErrorModel_LightMeas_x_gen2(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x4 = (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x6 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x5 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = (-1 * x5 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x9 = x8 * x8; const FLT x10 = x4 * x4; const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x3 * (*_x0).Lighthouse.Rot[2]); const FLT x12 = x11 * x11; const FLT x13 = x12 + x7 + x10 + x9; const FLT x14 = 1. / x13; const FLT x15 = (1. / sqrt((x14 * x12) + (x9 * x14) + (x7 * x14) + (x14 * x10))) * (1. / sqrt(x13)); const FLT x16 = x4 * x15; const FLT x17 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x18 = x8 * x15; const FLT x19 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x20 = x6 * x15; const FLT x21 = (x20 * x19) + (-1 * x0 * x16) + (x18 * x17); const FLT x22 = x15 * x11; const FLT x23 = (x0 * x22) + (x19 * x18) + (-1 * x20 * x17); const FLT x24 = 1.0/2.0 * dt * fabs(dt); const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x27 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x28 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (-1 * x26 * (*_x0).Object.Pose.Rot[2]); const FLT x29 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x30 = dt * dt; const FLT x31 = x30 * (x29 * x29); const FLT x32 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x33 = x30 * (x32 * x32); const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x35 = x30 * (x34 * x34); const FLT x36 = 1e-10 + x35 + x31 + x33; const FLT x37 = sqrt(x36); const FLT x38 = 0.5 * x37; const FLT x39 = sin(x38); const FLT x40 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x41 = (-1 * x40 * (*_x0).Object.Pose.Rot[2]) + (x26 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (x25 * (*_x0).Object.Pose.Rot[0]); const FLT x42 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x26 * (*_x0).Object.Pose.Rot[0]) + (x40 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[2]; const FLT x43 = (x40 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[1] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (x25 * (*_x0).Object.Pose.Rot[2]); const FLT x44 = (1. / x36) * (x39 * x39); const FLT x45 = cos(x38); const FLT x46 = (1. / sqrt((x43 * x43) + (x28 * x28) + (x41 * x41) + (x42 * x42))) * (1. / sqrt((x44 * x33) + (x45 * x45) + (x44 * x31) + (x44 * x35))); const FLT x47 = dt * x46 * (1. / x37) * x39; const FLT x48 = x47 * x29; const FLT x49 = x47 * x34; const FLT x50 = x45 * x46; const FLT x51 = x47 * x32; const FLT x52 = (x51 * x42) + (x48 * x28) + (x50 * x41) + (-1 * x43 * x49); const FLT x53 = (-1 * x51 * x43) + (-1 * x41 * x48) + (x50 * x28) + (-1 * x42 * x49); const FLT x54 = (x50 * x43) + (-1 * x42 * x48) + (x51 * x28) + (x41 * x49); const FLT x55 = (-1 * x54 * sensor_pt[2]) + (x53 * sensor_pt[1]) + (x52 * sensor_pt[0]); const FLT x56 = (-1 * x51 * x41) + (x43 * x48) + (x49 * x28) + (x50 * x42); const FLT x57 = (-1 * x56 * sensor_pt[0]) + (x53 * sensor_pt[2]) + (x54 * sensor_pt[1]); const FLT x58 = (*_x0).Object.Pose.Pos[0] + (x24 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x57 * x56) + (-1 * x52 * x55))); const FLT x59 = (-1 * x52 * sensor_pt[1]) + (x53 * sensor_pt[0]) + (x56 * sensor_pt[2]); const FLT x60 = (*_x0).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (2 * ((x54 * x55) + (-1 * x56 * x59))) + (x24 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2]; const FLT x61 = (*_x0).Object.Pose.Pos[1] + (x24 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (2 * ((x52 * x59) + (-1 * x54 * x57))) + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])); const FLT x62 = (x61 * x20) + (x58 * x18) + (-1 * x60 * x16); const FLT x63 = x15 * ((x61 * x18) + (x60 * x22) + (-1 * x58 * x20)); const FLT x64 = (-1 * (x0 + (2 * ((-1 * x22 * x23) + (x21 * x16))))) + x60 + (2 * ((-1 * x63 * x11) + (x62 * x16))); const FLT x65 = (x17 * x16) + (x0 * x18) + (-1 * x22 * x19); const FLT x66 = (x60 * x18) + (x58 * x16) + (-1 * x61 * x22); const FLT x67 = x58 + (-1 * (x17 + (2 * ((-1 * x65 * x16) + (x23 * x20))))) + (2 * ((-1 * x66 * x16) + (x6 * x63))); const FLT x68 = atan2(-1 * x64, x67); const FLT x69 = x61 + (2 * ((-1 * x62 * x20) + (x66 * x22))) + (-1 * (x19 + (2 * ((-1 * x20 * x21) + (x65 * x22))))); const FLT x70 = (x67 * x67) + (x64 * x64); const FLT x71 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x72 = cos(x71); const FLT x73 = asin((1. / x72) * x69 * (1. / sqrt(x70 + (x69 * x69)))); const FLT x74 = 0.0028679863 + (x73 * (-8.0108022e-06 + (-8.0108022e-06 * x73))); const FLT x75 = 5.3685255e-06 + (x73 * x74); const FLT x76 = 0.0076069798 + (x73 * x75); const FLT x77 = (1. / sqrt(x70)) * x69 * tan(x71); const FLT x78 = (*_x0).BSD0.curve + (sin(x68 + (-1 * asin(x77)) + (*error_model).BSD0.ogeephase + (*_x0).BSD0.ogeephase) * ((*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag)) + (*error_model).BSD0.curve; const FLT x79 = (-1 * asin(x77 + ((x73 * x73) * x78 * x76 * (1. / (x72 + (-1 * x78 * sin(x71) * ((x73 * (x76 + (x73 * (x75 + (x73 * (x74 + (x73 * (-8.0108022e-06 + (-1.60216044e-05 * x73))))))))) + (x73 * x76)))))))) + x68; return -1.5707963267949 + x79 + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (sin(x79 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag)); } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x4 = (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x5 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x6 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x1 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x3 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x9 = x8 * x8; const FLT x10 = x4 * x4; const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x3 * (*_x0).Lighthouse.Rot[2]); const FLT x12 = x11 * x11; const FLT x13 = x12 + x7 + x10 + x9; const FLT x14 = 1. / x13; const FLT x15 = x7 * x14; const FLT x16 = x14 * x10; const FLT x17 = x14 * x12; const FLT x18 = x17 + (x9 * x14) + x15 + x16; const FLT x19 = 1. / sqrt(x18); const FLT x20 = 1. / sqrt(x13); const FLT x21 = x20 * x19; const FLT x22 = x5 * x21; const FLT x23 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x24 = x23 * x21; const FLT x25 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x26 = x25 * x21; const FLT x27 = (x6 * x26) + (-1 * x4 * x22) + (x8 * x24); const FLT x28 = x4 * x21; const FLT x29 = (x22 * x11) + (x8 * x26) + (-1 * x6 * x24); const FLT x30 = x21 * x11; const FLT x31 = dt * fabs(dt); const FLT x32 = 1.0/2.0 * x31; const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x34 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x35 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x36 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x37 = (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x34 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x38 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x39 = (x36 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x38 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x40 = (x38 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x34 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[2]) + (*_x0).Object.Pose.Rot[2]; const FLT x41 = (*_x0).Object.Pose.Rot[1] + (-1 * x34 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x38 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x35 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x42 = (x41 * x41) + (x39 * x39) + (x37 * x37) + (x40 * x40); const FLT x43 = 1. / sqrt(x42); const FLT x44 = dt * dt; const FLT x45 = x33 * x33; const FLT x46 = x44 * x45; const FLT x47 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x48 = x47 * x47; const FLT x49 = x44 * x48; const FLT x50 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x51 = x50 * x50; const FLT x52 = x51 * x44; const FLT x53 = 1e-10 + x52 + x46 + x49; const FLT x54 = sqrt(x53); const FLT x55 = 0.5 * x54; const FLT x56 = sin(x55); const FLT x57 = x56 * x56; const FLT x58 = 1. / x53; const FLT x59 = x58 * x57; const FLT x60 = cos(x55); const FLT x61 = (x59 * x49) + (x60 * x60) + (x59 * x46) + (x52 * x59); const FLT x62 = 1. / sqrt(x61); const FLT x63 = x62 * x43; const FLT x64 = (1. / x54) * x56; const FLT x65 = dt * x64; const FLT x66 = x63 * x65; const FLT x67 = x66 * x37; const FLT x68 = x64 * x50; const FLT x69 = dt * x68; const FLT x70 = x63 * x69; const FLT x71 = x60 * x62; const FLT x72 = x71 * x43; const FLT x73 = x72 * x39; const FLT x74 = x66 * x40; const FLT x75 = (x74 * x47) + x73 + (x67 * x33) + (-1 * x70 * x41); const FLT x76 = x63 * x39; const FLT x77 = x76 * x65; const FLT x78 = x72 * x37; const FLT x79 = x66 * x41; const FLT x80 = (-1 * x79 * x47) + (-1 * x77 * x33) + x78 + (-1 * x70 * x40); const FLT x81 = x72 * x41; const FLT x82 = (x67 * x47) + x81 + (-1 * x74 * x33) + (x76 * x69); const FLT x83 = (-1 * x82 * sensor_pt[2]) + (x80 * sensor_pt[1]) + (x75 * sensor_pt[0]); const FLT x84 = x72 * x40; const FLT x85 = (-1 * x77 * x47) + (x79 * x33) + (x70 * x37) + x84; const FLT x86 = (-1 * x85 * sensor_pt[0]) + (x80 * sensor_pt[2]) + (x82 * sensor_pt[1]); const FLT x87 = (*_x0).Object.Pose.Pos[0] + (x32 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x85 * x86) + (-1 * x83 * x75))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])); const FLT x88 = x87 * x21; const FLT x89 = (x80 * sensor_pt[0]) + (-1 * x75 * sensor_pt[1]) + (x85 * sensor_pt[2]); const FLT x90 = (*_x0).Object.Pose.Pos[2] + (2 * ((x82 * x83) + (-1 * x89 * x85))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x32 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x91 = x90 * x21; const FLT x92 = (x32 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*_x0).Object.Pose.Pos[1] + (2 * ((x89 * x75) + (-1 * x82 * x86))) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])); const FLT x93 = x92 * x21; const FLT x94 = (x6 * x93) + (x8 * x88) + (-1 * x4 * x91); const FLT x95 = (x91 * x11) + (x8 * x93) + (-1 * x6 * x88); const FLT x96 = x90 + (-1 * (x5 + (2 * ((-1 * x30 * x29) + (x28 * x27))))) + (2 * ((-1 * x95 * x30) + (x94 * x28))); const FLT x97 = x6 * x21; const FLT x98 = (x4 * x24) + (x8 * x22) + (-1 * x26 * x11); const FLT x99 = (x4 * x88) + (x8 * x91) + (-1 * x93 * x11); const FLT x100 = x99 * x21; const FLT x101 = x87 + (-1 * (x23 + (2 * ((-1 * x98 * x28) + (x97 * x29))))) + (2 * ((-1 * x4 * x100) + (x97 * x95))); const FLT x102 = atan2(-1 * x96, x101); const FLT x103 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x104 = tan(x103); const FLT x105 = x92 + (2 * ((-1 * x97 * x94) + (x11 * x100))) + (-1 * (x25 + (2 * ((-1 * x97 * x27) + (x98 * x30))))); const FLT x106 = x101 * x101; const FLT x107 = x106 + (x96 * x96); const FLT x108 = 1. / sqrt(x107); const FLT x109 = x108 * x105; const FLT x110 = x109 * x104; const FLT x111 = asin(x110) + (-1 * x102) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase); const FLT x112 = sin(x111); const FLT x113 = (*_x0).BSD0.curve + (-1 * x0 * x112) + (*error_model).BSD0.curve; const FLT x114 = x105 * x105; const FLT x115 = x107 + x114; const FLT x116 = 1. / sqrt(x115); const FLT x117 = cos(x103); const FLT x118 = 1. / x117; const FLT x119 = x118 * x116; const FLT x120 = asin(x105 * x119); const FLT x121 = 8.0108022e-06 * x120; const FLT x122 = -8.0108022e-06 + (-1 * x121); const FLT x123 = 0.0028679863 + (x120 * x122); const FLT x124 = 5.3685255e-06 + (x120 * x123); const FLT x125 = 0.0076069798 + (x120 * x124); const FLT x126 = x120 * x125; const FLT x127 = -8.0108022e-06 + (-1.60216044e-05 * x120); const FLT x128 = x123 + (x120 * x127); const FLT x129 = x124 + (x120 * x128); const FLT x130 = x125 + (x120 * x129); const FLT x131 = (x120 * x130) + x126; const FLT x132 = sin(x103); const FLT x133 = x113 * x132; const FLT x134 = x133 * x131; const FLT x135 = x117 + (-1 * x134); const FLT x136 = 1. / x135; const FLT x137 = x120 * x120; const FLT x138 = x137 * x136; const FLT x139 = x125 * x138; const FLT x140 = x110 + (x113 * x139); const FLT x141 = 1. / sqrt(1 + (-1 * (x140 * x140))); const FLT x142 = 1. / x18; const FLT x143 = 2 * x6; const FLT x144 = x14 * x143; const FLT x145 = x142 * x144; const FLT x146 = x8 * x145; const FLT x147 = 2 * x4; const FLT x148 = x14 * x147; const FLT x149 = x142 * x148; const FLT x150 = x11 * x149; const FLT x151 = -1 * x150; const FLT x152 = x151 + x146; const FLT x153 = 2 * x105; const FLT x154 = 2 * x142; const FLT x155 = x16 * x154; const FLT x156 = x15 * x154; const FLT x157 = -1 + x156 + x155; const FLT x158 = 2 * x101; const FLT x159 = x11 * x145; const FLT x160 = -1 * x159; const FLT x161 = x8 * x149; const FLT x162 = -1 * x161; const FLT x163 = x162 + x160; const FLT x164 = 2 * x96; const FLT x165 = (x164 * x163) + (x157 * x158); const FLT x166 = 1.0/2.0 * x105; const FLT x167 = x118 * (1. / (x115 * sqrt(x115))) * x166; const FLT x168 = (x119 * x152) + (-1 * x167 * (x165 + (x152 * x153))); const FLT x169 = 1. / (x117 * x117); const FLT x170 = 1. / sqrt(1 + (-1 * x114 * (1. / x115) * x169)); const FLT x171 = x126 * x170 * x136; const FLT x172 = 2 * x113; const FLT x173 = x171 * x172; const FLT x174 = x123 * x170; const FLT x175 = x121 * x170; const FLT x176 = x122 * x170; const FLT x177 = x168 * x176; const FLT x178 = (x120 * (x177 + (-1 * x168 * x175))) + (x168 * x174); const FLT x179 = x124 * x170; const FLT x180 = (x168 * x179) + (x120 * x178); const FLT x181 = x113 * x138; const FLT x182 = x170 * x130; const FLT x183 = x129 * x170; const FLT x184 = x127 * x170; const FLT x185 = 2.40324066e-05 * x120; const FLT x186 = x170 * x185; const FLT x187 = x128 * x170; const FLT x188 = x125 * x170; const FLT x189 = 1. / x107; const FLT x190 = x104 * x104; const FLT x191 = 1. / sqrt(1 + (-1 * x114 * x189 * x190)); const FLT x192 = x104 * (1. / (x107 * sqrt(x107))) * x166; const FLT x193 = x108 * x104; const FLT x194 = (x193 * x152) + (-1 * x165 * x192); const FLT x195 = 1. / x101; const FLT x196 = x96 * (1. / x106); const FLT x197 = x106 * x189; const FLT x198 = ((x196 * x157) + (-1 * x163 * x195)) * x197; const FLT x199 = (-1 * x198) + (x191 * x194); const FLT x200 = x0 * cos(x111); const FLT x201 = x131 * x132; const FLT x202 = x200 * x201; const FLT x203 = x125 * x137 * (1. / (x135 * x135)); const FLT x204 = x203 * x113; const FLT x205 = x200 * x139; const FLT x206 = x198 + (-1 * x141 * (x194 + (-1 * x205 * x199) + (x168 * x173) + (-1 * x204 * ((x202 * x199) + (-1 * x133 * ((x168 * x188) + (x168 * x182) + (x120 * x180) + (x120 * (x180 + (x168 * x183) + (x120 * (x178 + (x120 * ((-1 * x168 * x186) + x177 + (x168 * x184))) + (x168 * x187))))))))) + (x181 * x180))); const FLT x207 = (-1 * asin(x140)) + x102 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha; const FLT x208 = cos(x207) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag); const FLT x209 = -1 * x146; const FLT x210 = x209 + x151; const FLT x211 = 2 * x8; const FLT x212 = x14 * x211; const FLT x213 = x11 * x212 * x142; const FLT x214 = x4 * x145; const FLT x215 = -1 * x214; const FLT x216 = x215 + x213; const FLT x217 = (x216 * x164) + (x210 * x158); const FLT x218 = x17 * x154; const FLT x219 = -1 + x218; const FLT x220 = x219 + x156; const FLT x221 = (x220 * x193) + (-1 * x217 * x192); const FLT x222 = ((x210 * x196) + (-1 * x216 * x195)) * x197; const FLT x223 = (-1 * x222) + (x221 * x191); const FLT x224 = (x220 * x119) + (-1 * x167 * (x217 + (x220 * x153))); const FLT x225 = x224 * x176; const FLT x226 = (x120 * (x225 + (-1 * x224 * x175))) + (x224 * x174); const FLT x227 = (x224 * x179) + (x226 * x120); const FLT x228 = x222 + (-1 * x141 * (x221 + (-1 * x205 * x223) + (-1 * x204 * ((x202 * x223) + (-1 * x133 * ((x227 * x120) + (x224 * x182) + (x120 * (x227 + (x224 * x183) + (x120 * (x226 + (x120 * ((-1 * x224 * x186) + x225 + (x224 * x184))) + (x224 * x187))))) + (x224 * x188))))) + (x224 * x173) + (x227 * x181))); const FLT x229 = -1 * x213; const FLT x230 = x229 + x215; const FLT x231 = x160 + x161; const FLT x232 = x219 + x155; const FLT x233 = (x232 * x164) + (x231 * x158); const FLT x234 = (x230 * x119) + (-1 * x167 * (x233 + (x230 * x153))); const FLT x235 = x234 * x176; const FLT x236 = (x120 * (x235 + (-1 * x234 * x175))) + (x234 * x174); const FLT x237 = (x234 * x179) + (x236 * x120); const FLT x238 = (x230 * x193) + (-1 * x233 * x192); const FLT x239 = ((x231 * x196) + (-1 * x232 * x195)) * x197; const FLT x240 = (-1 * x239) + (x238 * x191); const FLT x241 = x239 + (-1 * x141 * (x238 + (-1 * x204 * ((x202 * x240) + (-1 * x133 * ((x237 * x120) + (x120 * (x237 + (x234 * x183) + (x120 * (x236 + (x120 * (x235 + (-1 * x234 * x186) + (x234 * x184))) + (x234 * x187))))) + (x234 * x182) + (x234 * x188))))) + (-1 * x205 * x240) + (x237 * x181) + (x234 * x173))); const FLT x242 = 1.0 * x4; const FLT x243 = x242 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x244 = 1.0 * x11; const FLT x245 = x244 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x246 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x247 = x6 * x246; const FLT x248 = x211 + x247 + x243 + x245; const FLT x249 = 1. / (x13 * x13); const FLT x250 = x9 * x249; const FLT x251 = x7 * x249; const FLT x252 = x12 * x249; const FLT x253 = x10 * x249; const FLT x254 = x20 * (1. / (x18 * sqrt(x18))); const FLT x255 = x254 * (x212 + (-1 * x253 * x248) + (-1 * x251 * x248) + (-1 * x250 * x248) + (-1 * x252 * x248) + (x14 * x245) + (x14 * x243) + (x14 * x247)); const FLT x256 = 1.0/2.0 * x90; const FLT x257 = x11 * x256; const FLT x258 = x1 * x91; const FLT x259 = (1. / (x13 * sqrt(x13))) * x19; const FLT x260 = x259 * x248; const FLT x261 = x6 * x260; const FLT x262 = 1.0/2.0 * x87; const FLT x263 = 1.0/2.0 * x8; const FLT x264 = x92 * x263; const FLT x265 = -1 * x3 * x88; const FLT x266 = x6 * x255; const FLT x267 = (-1 * x264 * x260) + (-1 * x260 * x257) + x93 + (x266 * x262) + x258 + (x262 * x261) + x265 + (-1 * x255 * x257) + (-1 * x264 * x255); const FLT x268 = x21 * x143; const FLT x269 = x95 * x21; const FLT x270 = x269 * x246; const FLT x271 = x6 * x95; const FLT x272 = x21 * x29; const FLT x273 = x272 * x246; const FLT x274 = 1.0/2.0 * x23; const FLT x275 = 1.0/2.0 * x11; const FLT x276 = x5 * x275; const FLT x277 = x263 * x260; const FLT x278 = x1 * x22; const FLT x279 = x263 * x255; const FLT x280 = -1 * x3 * x24; const FLT x281 = x278 + (-1 * x276 * x255) + (-1 * x25 * x279) + (x274 * x261) + x26 + (x274 * x266) + (-1 * x276 * x260) + x280 + (-1 * x25 * x277); const FLT x282 = x4 * x99; const FLT x283 = x4 * x274; const FLT x284 = x25 * x275; const FLT x285 = x2 * x24; const FLT x286 = -1 * x1 * x26; const FLT x287 = x285 + (x284 * x255) + (-1 * x5 * x277) + (-1 * x283 * x255) + (-1 * x260 * x283) + x22 + x286 + (x260 * x284) + (-1 * x5 * x279); const FLT x288 = x21 * x147; const FLT x289 = x4 * x98; const FLT x290 = x4 * x262; const FLT x291 = x92 * x275; const FLT x292 = x8 * x256; const FLT x293 = x2 * x88; const FLT x294 = -1 * x1 * x93; const FLT x295 = x294 + x91; const FLT x296 = x293 + (-1 * x292 * x260) + (-1 * x290 * x255) + (-1 * x290 * x260) + (x291 * x255) + (x291 * x260) + x295 + (-1 * x292 * x255); const FLT x297 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x298 = x98 * x21; const FLT x299 = (x297 * x298) + (-1 * x297 * x100); const FLT x300 = (x282 * x255) + (-1 * x296 * x288) + x299 + (-1 * x260 * x289) + (-1 * x271 * x260) + (x268 * x267) + (x29 * x261) + x270 + (x260 * x282) + (-1 * x271 * x255) + (-1 * x268 * x281) + (-1 * x273) + (x29 * x266) + (x287 * x288) + (-1 * x289 * x255); const FLT x301 = x95 * x11; const FLT x302 = x4 * x94; const FLT x303 = x29 * x11; const FLT x304 = x4 * x27; const FLT x305 = x94 * x21; const FLT x306 = x297 * x305; const FLT x307 = x21 * x27; const FLT x308 = x297 * x307; const FLT x309 = 2 * x11; const FLT x310 = x21 * x309; const FLT x311 = 1.0/2.0 * x25; const FLT x312 = x3 * x26; const FLT x313 = x8 * x274; const FLT x314 = 1.0/2.0 * x5; const FLT x315 = x4 * x314; const FLT x316 = -1 * x2 * x22; const FLT x317 = (-1 * x255 * x313) + x24 + x312 + x316 + (-1 * x261 * x311) + (x255 * x315) + (-1 * x260 * x313) + (x260 * x315) + (-1 * x266 * x311); const FLT x318 = 1.0/2.0 * x92; const FLT x319 = x3 * x93; const FLT x320 = x87 * x263; const FLT x321 = x4 * x256; const FLT x322 = -1 * x2 * x91; const FLT x323 = x88 + x322; const FLT x324 = (-1 * x261 * x318) + x323 + x319 + (-1 * x260 * x320) + (-1 * x266 * x318) + (x255 * x321) + (x260 * x321) + (-1 * x255 * x320); const FLT x325 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x326 = (x272 * x325) + (-1 * x269 * x325); const FLT x327 = x326 + (-1 * x288 * x317) + (-1 * x260 * x303) + (-1 * x308) + (x260 * x304) + (x255 * x301) + (-1 * x255 * x302) + (x260 * x301) + (-1 * x255 * x303) + (x281 * x310) + (-1 * x260 * x302) + x306 + (-1 * x267 * x310) + (x288 * x324) + (x255 * x304); const FLT x328 = (x327 * x164) + (x300 * x158); const FLT x329 = x98 * x11; const FLT x330 = x298 * x325; const FLT x331 = x99 * x11; const FLT x332 = x6 * x27; const FLT x333 = x325 * x100; const FLT x334 = (x246 * x307) + (-1 * x246 * x305); const FLT x335 = (-1 * x287 * x310) + (x260 * x329) + (-1 * x268 * x324) + (x255 * x329) + (x94 * x261) + x334 + (-1 * x27 * x261) + (-1 * x260 * x331) + (x268 * x317) + (-1 * x330) + (-1 * x255 * x331) + (-1 * x255 * x332) + (x296 * x310) + (x94 * x266) + x333; const FLT x336 = (x335 * x193) + (-1 * x328 * x192); const FLT x337 = ((x300 * x196) + (-1 * x327 * x195)) * x197; const FLT x338 = (-1 * x337) + (x336 * x191); const FLT x339 = (x335 * x119) + (-1 * x167 * (x328 + (x335 * x153))); const FLT x340 = x339 * x176; const FLT x341 = (x120 * (x340 + (-1 * x339 * x175))) + (x339 * x174); const FLT x342 = (x339 * x179) + (x341 * x120); const FLT x343 = x337 + (-1 * x141 * (x336 + (-1 * x204 * ((x202 * x338) + (-1 * x133 * ((x342 * x120) + (x339 * x182) + (x120 * (x342 + (x339 * x183) + (x120 * (x341 + (x120 * (x340 + (-1 * x339 * x186) + (x339 * x184))) + (x339 * x187))))) + (x339 * x188))))) + (x339 * x173) + (-1 * x205 * x338) + (x342 * x181))); const FLT x344 = x6 * x297; const FLT x345 = x242 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x346 = x8 * x325; const FLT x347 = (-1 * x346) + (-1 * x345) + x344 + x309; const FLT x348 = x259 * x347; const FLT x349 = x274 * x348; const FLT x350 = x275 * x348; const FLT x351 = ((-1 * x14 * x345) + (x14 * x309) + (-1 * x252 * x347) + (-1 * x14 * x346) + (-1 * x250 * x347) + (-1 * x253 * x347) + (x14 * x344) + (-1 * x251 * x347)) * x254; const FLT x352 = x263 * x351; const FLT x353 = x263 * x348; const FLT x354 = (-1 * x278) + x280; const FLT x355 = x354 + (-1 * x26) + (x284 * x351) + (-1 * x4 * x349) + (-1 * x5 * x352) + (-1 * x5 * x353) + (x25 * x350) + (-1 * x283 * x351); const FLT x356 = x271 * x259; const FLT x357 = x6 * x29; const FLT x358 = x6 * x351; const FLT x359 = (-1 * x285) + x286; const FLT x360 = (x274 * x358) + x22 + (-1 * x5 * x350) + (-1 * x25 * x352) + (-1 * x25 * x353) + x359 + (x6 * x349) + (-1 * x276 * x351); const FLT x361 = x99 * x351; const FLT x362 = -1 * x293; const FLT x363 = 1.0/2.0 * x348; const FLT x364 = x6 * x363; const FLT x365 = x6 * x262; const FLT x366 = (x365 * x351) + x295 + (-1 * x92 * x353) + (-1 * x257 * x351) + (x87 * x364) + x362 + (-1 * x257 * x348) + (-1 * x264 * x351); const FLT x367 = x4 * x363; const FLT x368 = x265 + (-1 * x258); const FLT x369 = (-1 * x93) + (x291 * x351) + (-1 * x290 * x351) + (x92 * x350) + x368 + (-1 * x87 * x367) + (-1 * x292 * x348) + (-1 * x292 * x351); const FLT x370 = (x246 * x100) + (-1 * x298 * x246); const FLT x371 = (-1 * x272 * x297) + (x297 * x269); const FLT x372 = (-1 * x288 * x369) + (x351 * x357) + (-1 * x289 * x348) + (-1 * x289 * x351) + (x357 * x348) + (-1 * x268 * x360) + x371 + (x268 * x366) + x370 + (x288 * x355) + (-1 * x356 * x347) + (-1 * x271 * x351) + (x4 * x361) + (x282 * x348); const FLT x373 = 2 * x21; const FLT x374 = x95 * x373; const FLT x375 = x29 * x373; const FLT x376 = x2 * x26; const FLT x377 = x3 * x22; const FLT x378 = x1 * x24; const FLT x379 = (-1 * x358 * x311) + (-1 * x351 * x313) + (-1 * x25 * x364) + (-1 * x378) + x376 + (-1 * x8 * x349) + (x5 * x367) + (x351 * x315) + x377; const FLT x380 = x2 * x93; const FLT x381 = x1 * x88; const FLT x382 = x6 * x318; const FLT x383 = x3 * x91; const FLT x384 = (-1 * x382 * x351) + (x351 * x321) + (x321 * x348) + x383 + x380 + (-1 * x381) + (-1 * x351 * x320) + (-1 * x382 * x348) + (-1 * x87 * x353); const FLT x385 = x259 * x301; const FLT x386 = x334 + (-1 * x351 * x302) + (x385 * x347) + (-1 * x302 * x348) + x375 + (x351 * x304) + (-1 * x374) + (-1 * x366 * x310) + (-1 * x303 * x348) + (x360 * x310) + (-1 * x288 * x379) + (-1 * x351 * x303) + (x351 * x301) + (x288 * x384) + (x304 * x348); const FLT x387 = (x386 * x164) + (x372 * x158); const FLT x388 = x6 * x94; const FLT x389 = x99 * x373; const FLT x390 = x98 * x373; const FLT x391 = (-1 * x351 * x332) + (-1 * x268 * x384) + (-1 * x306) + (x329 * x348) + (x388 * x351) + (x388 * x348) + (-1 * x348 * x332) + (x268 * x379) + (-1 * x355 * x310) + (x351 * x329) + (-1 * x11 * x361) + (-1 * x390) + x308 + (-1 * x348 * x331) + (x369 * x310) + x389; const FLT x392 = (x391 * x193) + (-1 * x387 * x192); const FLT x393 = ((x372 * x196) + (-1 * x386 * x195)) * x197; const FLT x394 = (-1 * x393) + (x392 * x191); const FLT x395 = (x391 * x119) + (-1 * x167 * (x387 + (x391 * x153))); const FLT x396 = x395 * x176; const FLT x397 = (x120 * (x396 + (-1 * x395 * x175))) + (x395 * x174); const FLT x398 = (x395 * x179) + (x397 * x120); const FLT x399 = x393 + (-1 * x141 * (x392 + (-1 * x204 * ((x202 * x394) + (-1 * x133 * ((x398 * x120) + (x395 * x188) + (x120 * (x398 + (x395 * x183) + (x120 * (x397 + (x120 * ((-1 * x395 * x186) + x396 + (x395 * x184))) + (x395 * x187))))) + (x395 * x182))))) + (x398 * x181) + (-1 * x205 * x394) + (x395 * x173))); const FLT x400 = x244 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x401 = x6 * x325; const FLT x402 = x8 * x297; const FLT x403 = (-1 * x402) + (-1 * x401) + x147 + x400; const FLT x404 = x403 * x259; const FLT x405 = x254 * ((-1 * x14 * x402) + x148 + (-1 * x403 * x251) + (x14 * x400) + (-1 * x403 * x250) + (-1 * x403 * x253) + (-1 * x403 * x252) + (-1 * x14 * x401)); const FLT x406 = x4 * x405; const FLT x407 = x405 * x263; const FLT x408 = x404 * x263; const FLT x409 = (-1 * x312) + x316; const FLT x410 = x409 + (x405 * x284) + (-1 * x406 * x274) + (x404 * x284) + (-1 * x5 * x407) + (-1 * x5 * x408) + (-1 * x404 * x283) + x24; const FLT x411 = -1 * x319; const FLT x412 = (x405 * x291) + x411 + (x404 * x291) + x323 + (-1 * x405 * x292) + (-1 * x406 * x262) + (-1 * x404 * x290) + (-1 * x404 * x292); const FLT x413 = x6 * x404; const FLT x414 = x6 * x405; const FLT x415 = (x414 * x274) + x378 + (-1 * x376) + (-1 * x404 * x276) + (-1 * x25 * x408) + x377 + (-1 * x25 * x407) + (x413 * x274) + (-1 * x405 * x276); const FLT x416 = (x414 * x262) + x381 + (-1 * x404 * x264) + (-1 * x405 * x257) + (-1 * x405 * x264) + (-1 * x380) + x383 + (-1 * x404 * x257) + (x404 * x365); const FLT x417 = x326 + (x405 * x282) + (x416 * x268) + x390 + (-1 * x405 * x271) + (x410 * x288) + (-1 * x404 * x271) + (x404 * x282) + (-1 * x389) + (-1 * x404 * x289) + (x404 * x357) + (-1 * x405 * x289) + (-1 * x415 * x268) + (-1 * x412 * x288) + (x29 * x414); const FLT x418 = x94 * x373; const FLT x419 = x359 + (-1 * x413 * x311) + (-1 * x405 * x313) + (-1 * x404 * x313) + (-1 * x22) + (x406 * x314) + (-1 * x414 * x311) + (x404 * x315); const FLT x420 = x27 * x373; const FLT x421 = (x405 * x321) + (-1 * x91) + (-1 * x404 * x382) + (-1 * x404 * x320) + (-1 * x405 * x320) + (x404 * x321) + (-1 * x414 * x318) + x294 + x362; const FLT x422 = (x421 * x288) + (x405 * x301) + (x404 * x301) + (-1 * x420) + (x404 * x304) + (-1 * x405 * x302) + (-1 * x416 * x310) + x418 + (-1 * x404 * x303) + (-1 * x270) + (x405 * x304) + (-1 * x404 * x302) + (-1 * x419 * x288) + x273 + (-1 * x405 * x303) + (x415 * x310); const FLT x423 = (x422 * x164) + (x417 * x158); const FLT x424 = (-1 * x307 * x325) + (x305 * x325); const FLT x425 = x424 + x370 + (x404 * x329) + (x419 * x268) + (x412 * x310) + (-1 * x410 * x310) + (-1 * x404 * x331) + (-1 * x404 * x332) + (x404 * x388) + (-1 * x421 * x268) + (-1 * x405 * x331) + (x94 * x414) + (x405 * x329) + (-1 * x27 * x414); const FLT x426 = (x425 * x193) + (-1 * x423 * x192); const FLT x427 = ((x417 * x196) + (-1 * x422 * x195)) * x197; const FLT x428 = (-1 * x427) + (x426 * x191); const FLT x429 = (x425 * x119) + (-1 * x167 * (x423 + (x425 * x153))); const FLT x430 = x429 * x176; const FLT x431 = (x120 * (x430 + (-1 * x429 * x175))) + (x429 * x174); const FLT x432 = (x429 * x179) + (x431 * x120); const FLT x433 = x427 + (-1 * x141 * (x426 + (x429 * x173) + (-1 * x428 * x205) + (-1 * x204 * ((x428 * x202) + (-1 * x133 * ((x432 * x120) + (x429 * x182) + (x429 * x188) + (x120 * (x432 + (x429 * x183) + (x120 * (x431 + (x120 * ((-1 * x429 * x186) + x430 + (x429 * x184))) + (x429 * x187))))))))) + (x432 * x181))); const FLT x434 = x244 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x435 = x4 * x325; const FLT x436 = x8 * x246; const FLT x437 = (-1 * x436) + x143 + (-1 * x434) + x435; const FLT x438 = x437 * x259; const FLT x439 = x254 * ((-1 * x14 * x436) + (-1 * x437 * x251) + (-1 * x437 * x252) + (-1 * x437 * x250) + (-1 * x14 * x434) + (x14 * x435) + x144 + (-1 * x437 * x253)); const FLT x440 = x92 * x439; const FLT x441 = x6 * x439; const FLT x442 = (x441 * x262) + (-1 * x88) + (-1 * x440 * x263) + (-1 * x439 * x257) + (-1 * x438 * x257) + x322 + (x438 * x365) + x411 + (-1 * x438 * x264); const FLT x443 = x439 * x263; const FLT x444 = x438 * x263; const FLT x445 = x6 * x438; const FLT x446 = (-1 * x24) + (-1 * x438 * x276) + (x445 * x274) + (-1 * x25 * x443) + x409 + (x441 * x274) + (-1 * x25 * x444) + (-1 * x439 * x276); const FLT x447 = x98 * x439; const FLT x448 = x376 + (-1 * x377) + (x439 * x284) + x378 + (-1 * x5 * x443) + (-1 * x439 * x283) + (-1 * x5 * x444) + (x438 * x284) + (-1 * x438 * x283); const FLT x449 = (x440 * x275) + (-1 * x383) + x380 + (-1 * x439 * x290) + (x438 * x291) + (-1 * x438 * x290) + x381 + (-1 * x439 * x292) + (-1 * x438 * x292); const FLT x450 = (-1 * x449 * x288) + (x439 * x282) + (-1 * x375) + x374 + (-1 * x439 * x271) + x330 + (-1 * x4 * x447) + (x442 * x268) + (-1 * x437 * x356) + (x29 * x441) + (x448 * x288) + (x438 * x282) + (-1 * x333) + (-1 * x446 * x268) + (x438 * x357) + (-1 * x438 * x289); const FLT x451 = x368 + (-1 * x441 * x318) + (-1 * x438 * x382) + (x439 * x321) + (-1 * x439 * x320) + x93 + (x438 * x321) + (-1 * x438 * x320); const FLT x452 = x354 + (-1 * x439 * x313) + (x438 * x315) + x26 + (-1 * x445 * x311) + (x439 * x315) + (-1 * x441 * x311) + (-1 * x438 * x313); const FLT x453 = x424 + (x439 * x301) + x371 + (x451 * x288) + (-1 * x442 * x310) + (-1 * x438 * x303) + (x446 * x310) + (x438 * x304) + (-1 * x438 * x302) + (x437 * x385) + (-1 * x452 * x288) + (-1 * x439 * x302) + (-1 * x439 * x303) + (x439 * x304); const FLT x454 = (x453 * x164) + (x450 * x158); const FLT x455 = x299 + x420 + (x438 * x388) + (-1 * x439 * x331) + (-1 * x418) + (-1 * x27 * x441) + (-1 * x438 * x332) + (x438 * x329) + (-1 * x438 * x331) + (x452 * x268) + (x449 * x310) + (x11 * x447) + (x94 * x441) + (-1 * x451 * x268) + (-1 * x448 * x310); const FLT x456 = (x455 * x193) + (-1 * x454 * x192); const FLT x457 = ((x450 * x196) + (-1 * x453 * x195)) * x197; const FLT x458 = (-1 * x457) + (x456 * x191); const FLT x459 = (x455 * x119) + (-1 * x167 * (x454 + (x455 * x153))); const FLT x460 = x459 * x176; const FLT x461 = (x120 * (x460 + (-1 * x459 * x175))) + (x459 * x174); const FLT x462 = (x459 * x179) + (x461 * x120); const FLT x463 = x457 + (-1 * x141 * ((-1 * x204 * ((x458 * x202) + (-1 * x133 * ((x462 * x120) + (x459 * x188) + (x459 * x182) + (x120 * (x462 + (x459 * x183) + (x120 * (x461 + (x120 * ((-1 * x459 * x186) + x460 + (x459 * x184))) + (x459 * x187))))))))) + (-1 * x458 * x205) + x456 + (x462 * x181) + (x459 * x173))); const FLT x464 = x31 * x142; const FLT x465 = -1 * x16 * x464; const FLT x466 = -1 * x15 * x464; const FLT x467 = x466 + x32 + x465; const FLT x468 = x14 * x464; const FLT x469 = x4 * x468; const FLT x470 = x8 * x469; const FLT x471 = x6 * x11 * x468; const FLT x472 = x471 + x470; const FLT x473 = (x472 * x164) + (x467 * x158); const FLT x474 = x11 * x469; const FLT x475 = x8 * x468; const FLT x476 = x6 * x475; const FLT x477 = (-1 * x476) + x474; const FLT x478 = (x477 * x193) + (-1 * x473 * x192); const FLT x479 = ((x467 * x196) + (-1 * x472 * x195)) * x197; const FLT x480 = (-1 * x479) + (x478 * x191); const FLT x481 = (x477 * x119) + (-1 * x167 * (x473 + (x477 * x153))); const FLT x482 = x481 * x176; const FLT x483 = (x120 * (x482 + (-1 * x481 * x175))) + (x481 * x174); const FLT x484 = (x481 * x179) + (x483 * x120); const FLT x485 = x479 + (-1 * x141 * ((-1 * x204 * ((x480 * x202) + (-1 * x133 * ((x484 * x120) + (x481 * x182) + (x481 * x188) + (x120 * (x484 + (x481 * x183) + (x120 * (x483 + (x120 * ((-1 * x481 * x186) + x482 + (x481 * x184))) + (x481 * x187))))))))) + x478 + (x484 * x181) + (-1 * x480 * x205) + (x481 * x173))); const FLT x486 = x474 + x476; const FLT x487 = x6 * x469; const FLT x488 = x11 * x475; const FLT x489 = (-1 * x488) + x487; const FLT x490 = (x489 * x164) + (x486 * x158); const FLT x491 = (-1 * x17 * x464) + x32; const FLT x492 = x491 + x466; const FLT x493 = (x492 * x193) + (-1 * x490 * x192); const FLT x494 = ((x486 * x196) + (-1 * x489 * x195)) * x197; const FLT x495 = (-1 * x494) + (x493 * x191); const FLT x496 = (x492 * x119) + (-1 * x167 * (x490 + (x492 * x153))); const FLT x497 = x496 * x176; const FLT x498 = (x120 * (x497 + (-1 * x496 * x175))) + (x496 * x174); const FLT x499 = (x496 * x179) + (x498 * x120); const FLT x500 = x494 + (-1 * x141 * ((-1 * x204 * ((x495 * x202) + (-1 * x133 * ((x499 * x120) + (x120 * (x499 + (x496 * x183) + (x120 * (x498 + (x120 * ((-1 * x496 * x186) + x497 + (x496 * x184))) + (x496 * x187))))) + (x496 * x188) + (x496 * x182))))) + x493 + (x499 * x181) + (-1 * x495 * x205) + (x496 * x173))); const FLT x501 = x487 + x488; const FLT x502 = (-1 * x470) + x471; const FLT x503 = x491 + x465; const FLT x504 = (x503 * x164) + (x502 * x158); const FLT x505 = (x501 * x119) + (-1 * x167 * (x504 + (x501 * x153))); const FLT x506 = (x501 * x193) + (-1 * x504 * x192); const FLT x507 = ((x502 * x196) + (-1 * x503 * x195)) * x197; const FLT x508 = (-1 * x507) + (x506 * x191); const FLT x509 = x505 * x176; const FLT x510 = (x120 * (x509 + (-1 * x505 * x175))) + (x505 * x174); const FLT x511 = (x505 * x179) + (x510 * x120); const FLT x512 = x507 + (-1 * x141 * (x506 + (x511 * x181) + (x505 * x173) + (-1 * x204 * ((x508 * x202) + (-1 * x133 * ((x511 * x120) + (x120 * (x511 + (x505 * x183) + (x120 * ((x120 * (x509 + (-1 * x505 * x186) + (x505 * x184))) + x510 + (x505 * x187))))) + (x505 * x182) + (x505 * x188))))) + (-1 * x508 * x205))); const FLT x513 = -1 * x155; const FLT x514 = 1 + (-1 * x156); const FLT x515 = x514 + x513; const FLT x516 = x159 + x161; const FLT x517 = (x516 * x164) + (x515 * x158); const FLT x518 = x209 + x150; const FLT x519 = (x518 * x193) + (-1 * x517 * x192); const FLT x520 = ((x515 * x196) + (-1 * x516 * x195)) * x197; const FLT x521 = (-1 * x520) + (x519 * x191); const FLT x522 = (x518 * x119) + (-1 * x167 * (x517 + (x518 * x153))); const FLT x523 = x522 * x176; const FLT x524 = (x120 * (x523 + (-1 * x522 * x175))) + (x522 * x174); const FLT x525 = (x522 * x179) + (x524 * x120); const FLT x526 = x520 + (-1 * x141 * ((x525 * x181) + x519 + (-1 * x204 * ((x521 * x202) + (-1 * x133 * ((x525 * x120) + (x522 * x188) + (x522 * x182) + (x120 * (x525 + (x522 * x183) + (x120 * (x524 + (x120 * ((-1 * x522 * x186) + x523 + (x522 * x184))) + (x522 * x187))))))))) + (-1 * x521 * x205) + (x522 * x173))); const FLT x527 = x150 + x146; const FLT x528 = x229 + x214; const FLT x529 = (x528 * x164) + (x527 * x158); const FLT x530 = -1 * x218; const FLT x531 = x514 + x530; const FLT x532 = (x531 * x193) + (-1 * x529 * x192); const FLT x533 = ((x527 * x196) + (-1 * x528 * x195)) * x197; const FLT x534 = (-1 * x533) + (x532 * x191); const FLT x535 = (x531 * x119) + (-1 * x167 * (x529 + (x531 * x153))); const FLT x536 = x535 * x176; const FLT x537 = (x120 * (x536 + (-1 * x535 * x175))) + (x535 * x174); const FLT x538 = (x535 * x179) + (x537 * x120); const FLT x539 = x533 + (-1 * x141 * ((x538 * x181) + (-1 * x534 * x205) + x532 + (x535 * x173) + (-1 * x204 * ((x534 * x202) + (-1 * x133 * ((x538 * x120) + (x535 * x188) + (x535 * x182) + (x120 * ((x535 * x183) + x538 + (x120 * (x537 + (x120 * ((-1 * x535 * x186) + (x535 * x184) + x536)) + (x535 * x187))))))))))); const FLT x540 = x162 + x159; const FLT x541 = 1 + x530 + x513; const FLT x542 = (x541 * x164) + (x540 * x158); const FLT x543 = x214 + x213; const FLT x544 = (x543 * x193) + (-1 * x542 * x192); const FLT x545 = ((x540 * x196) + (-1 * x541 * x195)) * x197; const FLT x546 = (-1 * x545) + (x544 * x191); const FLT x547 = (x543 * x119) + (-1 * x167 * (x542 + (x543 * x153))); const FLT x548 = x547 * x176; const FLT x549 = (x120 * (x548 + (-1 * x547 * x175))) + (x547 * x174); const FLT x550 = (x547 * x179) + (x549 * x120); const FLT x551 = x545 + (-1 * x141 * (x544 + (-1 * x204 * ((x546 * x202) + (-1 * x133 * ((x547 * x188) + (x550 * x120) + (x120 * (x550 + (x547 * x183) + (x120 * (x549 + (x120 * ((x547 * x184) + (-1 * x547 * x186) + x548)) + (x547 * x187))))) + (x547 * x182))))) + (x550 * x181) + (-1 * x546 * x205) + (x547 * x173))); const FLT x552 = 1.0 * x41; const FLT x553 = 1.0 * x39; const FLT x554 = 1.0 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x555 = (x40 * x554) + (x552 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x553 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x37); const FLT x556 = x65 * x47; const FLT x557 = 1.0/2.0 * x40; const FLT x558 = 1. / (x42 * sqrt(x42)); const FLT x559 = x62 * x558; const FLT x560 = x559 * x557; const FLT x561 = x556 * x560; const FLT x562 = x71 * x558; const FLT x563 = x555 * x562; const FLT x564 = 1.0/2.0 * x563; const FLT x565 = 1.0/2.0 * x37; const FLT x566 = x65 * x565; const FLT x567 = x33 * x566; const FLT x568 = x559 * x555; const FLT x569 = x66 * x33; const FLT x570 = x66 * x47; const FLT x571 = 0.5 * x570; const FLT x572 = x571 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x573 = 1.0/2.0 * x41; const FLT x574 = x69 * x573; const FLT x575 = 0.5 * x70; const FLT x576 = -1 * x575 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x577 = 0.5 * x72; const FLT x578 = x577 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x579 = x578 + x576; const FLT x580 = (-1 * x39 * x564) + (-1 * x567 * x568) + x579 + (-1 * x555 * x561) + (x574 * x568) + x569 + x572; const FLT x581 = 2 * x83; const FLT x582 = 1.0/2.0 * x39; const FLT x583 = x582 * x556; const FLT x584 = x65 * x33; const FLT x585 = x584 * x555; const FLT x586 = x585 * x559; const FLT x587 = x69 * x565; const FLT x588 = 0.5 * x569; const FLT x589 = x588 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x590 = -1 * x571 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x591 = x577 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x592 = x591 + x590; const FLT x593 = (-1 * x573 * x586) + (x583 * x568) + x592 + x70 + x589 + (-1 * x557 * x563) + (-1 * x587 * x568); const FLT x594 = x588 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x595 = -1 * x594; const FLT x596 = x575 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x597 = -1 * x596; const FLT x598 = x573 * x556; const FLT x599 = x69 * x560; const FLT x600 = x571 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x601 = (-1 * x600) + x72; const FLT x602 = (x599 * x555) + x597 + (x598 * x568) + x601 + x595 + (-1 * x37 * x564) + (x582 * x586); const FLT x603 = x47 * x566; const FLT x604 = x69 * x582; const FLT x605 = x577 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x606 = x575 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x607 = -1 * x588 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x608 = x607 + x606; const FLT x609 = x608 + x605 + x570 + (-1 * x603 * x568) + (-1 * x41 * x564) + (x585 * x560) + (-1 * x604 * x568); const FLT x610 = (x609 * sensor_pt[1]) + (-1 * x593 * sensor_pt[0]) + (x602 * sensor_pt[2]); const FLT x611 = 2 * x85; const FLT x612 = (x580 * sensor_pt[0]) + (-1 * x609 * sensor_pt[2]) + (x602 * sensor_pt[1]); const FLT x613 = 2 * x75; const FLT x614 = 2 * x86; const FLT x615 = (-1 * x612 * x613) + (-1 * x580 * x581) + (x614 * x593) + (x610 * x611); const FLT x616 = 2 * x89; const FLT x617 = (x593 * sensor_pt[2]) + (x602 * sensor_pt[0]) + (-1 * x580 * sensor_pt[1]); const FLT x618 = 2 * x82; const FLT x619 = (x609 * x581) + (-1 * x616 * x593) + (x612 * x618) + (-1 * x611 * x617); const FLT x620 = x8 * x21; const FLT x621 = (x613 * x617) + (x616 * x580) + (-1 * x614 * x609) + (-1 * x610 * x618); const FLT x622 = x21 * x621; const FLT x623 = (-1 * x11 * x622) + (x28 * x615) + (x619 * x620); const FLT x624 = (-1 * x28 * x619) + (x615 * x620) + (x6 * x622); const FLT x625 = x621 + (x623 * x310) + (-1 * x624 * x268); const FLT x626 = (-1 * x97 * x615) + (x30 * x619) + (x8 * x622); const FLT x627 = x615 + (-1 * x623 * x288) + (x626 * x268); const FLT x628 = x619 + (x624 * x288) + (-1 * x626 * x310); const FLT x629 = (x628 * x164) + (x627 * x158); const FLT x630 = (x625 * x119) + (-1 * x167 * (x629 + (x625 * x153))); const FLT x631 = x630 * x176; const FLT x632 = (x120 * (x631 + (-1 * x630 * x175))) + (x630 * x174); const FLT x633 = (x630 * x179) + (x632 * x120); const FLT x634 = (x625 * x193) + (-1 * x629 * x192); const FLT x635 = ((x627 * x196) + (-1 * x628 * x195)) * x197; const FLT x636 = (-1 * x635) + (x634 * x191); const FLT x637 = x635 + (-1 * x141 * (x634 + (-1 * x204 * ((x636 * x202) + (-1 * x133 * ((x633 * x120) + (x120 * (x633 + (x630 * x183) + (x120 * ((x120 * ((-1 * x630 * x186) + x631 + (x630 * x184))) + x632 + (x630 * x187))))) + (x630 * x182) + (x630 * x188))))) + (x630 * x173) + (x633 * x181) + (-1 * x636 * x205))); const FLT x638 = -1 * x572; const FLT x639 = 1.0 * x37; const FLT x640 = 1.0 * x40; const FLT x641 = (-1 * x640 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x41) + (-1 * x639 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x39 * x554); const FLT x642 = x641 * x559; const FLT x643 = x69 * x642; const FLT x644 = x573 * x584; const FLT x645 = x641 * x562; const FLT x646 = (-1 * x578) + x576; const FLT x647 = x646 + x569 + (-1 * x645 * x557) + x638 + (-1 * x642 * x644) + (x642 * x583) + (-1 * x643 * x565); const FLT x648 = -1 * x589; const FLT x649 = -1 * x70; const FLT x650 = (-1 * x645 * x582) + (-1 * x641 * x561) + x592 + (-1 * x642 * x567) + x649 + x648 + (x643 * x573); const FLT x651 = -1 * x570; const FLT x652 = x582 * x584; const FLT x653 = -1 * x605; const FLT x654 = x608 + x653 + (x641 * x599) + x651 + (x642 * x598) + (-1 * x645 * x565) + (x642 * x652); const FLT x655 = (x647 * sensor_pt[2]) + (-1 * x650 * sensor_pt[1]) + (x654 * sensor_pt[0]); const FLT x656 = x584 * x560; const FLT x657 = (x641 * x656) + x594 + (-1 * x645 * x573) + x596 + x601 + (-1 * x603 * x642) + (-1 * x643 * x582); const FLT x658 = (x654 * sensor_pt[1]) + (x650 * sensor_pt[0]) + (-1 * x657 * sensor_pt[2]); const FLT x659 = (x618 * x658) + (x657 * x581) + (-1 * x616 * x647) + (-1 * x611 * x655); const FLT x660 = x21 * x659; const FLT x661 = (x657 * sensor_pt[1]) + (-1 * x647 * sensor_pt[0]) + (x654 * sensor_pt[2]); const FLT x662 = (x616 * x650) + (-1 * x614 * x657) + (x613 * x655) + (-1 * x618 * x661); const FLT x663 = x21 * x662; const FLT x664 = (-1 * x613 * x658) + (-1 * x650 * x581) + (x614 * x647) + (x611 * x661); const FLT x665 = (-1 * x97 * x664) + (x11 * x660) + (x8 * x663); const FLT x666 = (-1 * x11 * x663) + (x28 * x664) + (x8 * x660); const FLT x667 = x664 + (x665 * x268) + (-1 * x666 * x288); const FLT x668 = (-1 * x4 * x660) + (x6 * x663) + (x620 * x664); const FLT x669 = x659 + (x668 * x288) + (-1 * x665 * x310); const FLT x670 = (x669 * x164) + (x667 * x158); const FLT x671 = x662 + (-1 * x668 * x268) + (x666 * x310); const FLT x672 = (x671 * x193) + (-1 * x670 * x192); const FLT x673 = ((x667 * x196) + (-1 * x669 * x195)) * x197; const FLT x674 = (-1 * x673) + (x672 * x191); const FLT x675 = (x671 * x119) + (-1 * x167 * (x670 + (x671 * x153))); const FLT x676 = x675 * x176; const FLT x677 = (x120 * (x676 + (-1 * x675 * x175))) + (x675 * x174); const FLT x678 = (x675 * x179) + (x677 * x120); const FLT x679 = x673 + (-1 * x141 * ((-1 * x204 * ((x674 * x202) + (-1 * x133 * ((x678 * x120) + (x675 * x182) + (x120 * ((x675 * x183) + x678 + (x120 * (x677 + (x120 * ((-1 * x675 * x186) + x676 + (x675 * x184))) + (x675 * x187))))) + (x675 * x188))))) + (x675 * x173) + (-1 * x674 * x205) + x672 + (x678 * x181))); const FLT x680 = (x552 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x40) + (-1 * x37 * x554) + (-1 * x553 * (*error_model).Object.Pose.AxisAngleRot[0]); const FLT x681 = x680 * x559; const FLT x682 = x69 * x681; const FLT x683 = x680 * x562; const FLT x684 = x72 + x600; const FLT x685 = x684 + (-1 * x644 * x681) + x594 + (x681 * x583) + (-1 * x683 * x557) + (-1 * x682 * x565) + x597; const FLT x686 = -1 * x569; const FLT x687 = x579 + (-1 * x604 * x681) + x638 + x686 + (-1 * x603 * x681) + (-1 * x683 * x573) + (x680 * x656); const FLT x688 = (-1 * x606) + x607; const FLT x689 = x688 + x570 + x653 + (-1 * x681 * x567) + (-1 * x683 * x582) + (x682 * x573) + (-1 * x680 * x561); const FLT x690 = (-1 * x591) + x590; const FLT x691 = x690 + x649 + (x681 * x598) + x589 + (-1 * x683 * x565) + (x681 * x652) + (x680 * x599); const FLT x692 = 2 * ((x685 * sensor_pt[2]) + (-1 * x689 * sensor_pt[1]) + (x691 * sensor_pt[0])); const FLT x693 = (x689 * sensor_pt[0]) + (-1 * x687 * sensor_pt[2]) + (x691 * sensor_pt[1]); const FLT x694 = (-1 * x85 * x692) + (x693 * x618) + (-1 * x616 * x685) + (x687 * x581); const FLT x695 = (x687 * sensor_pt[1]) + (-1 * x685 * sensor_pt[0]) + (x691 * sensor_pt[2]); const FLT x696 = (-1 * x695 * x618) + (x616 * x689) + (-1 * x614 * x687) + (x75 * x692); const FLT x697 = x21 * x696; const FLT x698 = (-1 * x693 * x613) + (x614 * x685) + (-1 * x689 * x581) + (x695 * x611); const FLT x699 = x21 * ((-1 * x97 * x698) + (x30 * x694) + (x8 * x697)); const FLT x700 = (-1 * x11 * x697) + (x28 * x698) + (x694 * x620); const FLT x701 = x698 + (x699 * x143) + (-1 * x700 * x288); const FLT x702 = (-1 * x28 * x694) + (x6 * x697) + (x698 * x620); const FLT x703 = (x702 * x288) + x694 + (-1 * x699 * x309); const FLT x704 = (x703 * x164) + (x701 * x158); const FLT x705 = x696 + (x700 * x310) + (-1 * x702 * x268); const FLT x706 = (x705 * x193) + (-1 * x704 * x192); const FLT x707 = ((x701 * x196) + (-1 * x703 * x195)) * x197; const FLT x708 = (-1 * x707) + (x706 * x191); const FLT x709 = (x705 * x119) + (-1 * x167 * (x704 + (x705 * x153))); const FLT x710 = x709 * x176; const FLT x711 = (x120 * (x710 + (-1 * x709 * x175))) + (x709 * x174); const FLT x712 = (x709 * x179) + (x711 * x120); const FLT x713 = x707 + (-1 * x141 * (x706 + (x712 * x181) + (-1 * x204 * ((x708 * x202) + (-1 * x133 * ((x712 * x120) + (x709 * x188) + (x709 * x182) + (x120 * (x712 + (x709 * x183) + (x120 * (x711 + (x120 * ((-1 * x709 * x186) + x710 + (x709 * x184))) + (x709 * x187))))))))) + (-1 * x708 * x205) + (x709 * x173))); const FLT x714 = (x640 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x39) + (-1 * x41 * x554) + (-1 * x639 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x715 = x714 * x559; const FLT x716 = x715 * x582; const FLT x717 = x714 * x562; const FLT x718 = x688 + (-1 * x717 * x557) + x605 + (-1 * x715 * x587) + x651 + (-1 * x644 * x715) + (x716 * x556); const FLT x719 = x714 * x560; const FLT x720 = x684 + x596 + (-1 * x717 * x582) + x595 + (-1 * x719 * x556) + (-1 * x715 * x567) + (x715 * x574); const FLT x721 = (x69 * x719) + x572 + x686 + x646 + (x715 * x598) + (-1 * x717 * x565) + (x716 * x584); const FLT x722 = (x718 * sensor_pt[2]) + (-1 * x720 * sensor_pt[1]) + (x721 * sensor_pt[0]); const FLT x723 = (-1 * x69 * x716) + x648 + x70 + (x719 * x584) + x690 + (-1 * x603 * x715) + (-1 * x717 * x573); const FLT x724 = (x720 * sensor_pt[0]) + (-1 * x723 * sensor_pt[2]) + (x721 * sensor_pt[1]); const FLT x725 = (x618 * x724) + (-1 * x616 * x718) + (x723 * x581) + (-1 * x611 * x722); const FLT x726 = (x723 * sensor_pt[1]) + (x721 * sensor_pt[2]) + (-1 * x718 * sensor_pt[0]); const FLT x727 = (x613 * x722) + (x616 * x720) + (-1 * x614 * x723) + (-1 * x618 * x726); const FLT x728 = (-1 * x613 * x724) + (x614 * x718) + (-1 * x720 * x581) + (x611 * x726); const FLT x729 = (-1 * x97 * x728) + (x30 * x725) + (x620 * x727); const FLT x730 = (-1 * x30 * x727) + (x28 * x728) + (x620 * x725); const FLT x731 = x728 + (x729 * x268) + (-1 * x730 * x288); const FLT x732 = (-1 * x28 * x725) + (x97 * x727) + (x620 * x728); const FLT x733 = x725 + (x732 * x288) + (-1 * x729 * x310); const FLT x734 = (x733 * x164) + (x731 * x158); const FLT x735 = x727 + (-1 * x732 * x268) + (x730 * x310); const FLT x736 = (x735 * x193) + (-1 * x734 * x192); const FLT x737 = ((x731 * x196) + (-1 * x733 * x195)) * x197; const FLT x738 = (-1 * x737) + (x736 * x191); const FLT x739 = (x735 * x119) + (-1 * x167 * (x734 + (x735 * x153))); const FLT x740 = x739 * x176; const FLT x741 = (x120 * (x740 + (-1 * x739 * x175))) + (x739 * x174); const FLT x742 = (x739 * x179) + (x741 * x120); const FLT x743 = x737 + (-1 * x141 * (x736 + (-1 * x204 * ((x738 * x202) + (-1 * x133 * ((x739 * x182) + (x742 * x120) + (x120 * (x742 + (x739 * x183) + (x120 * (x741 + (x120 * ((-1 * x739 * x186) + x740 + (x739 * x184))) + (x739 * x187))))) + (x739 * x188))))) + (-1 * x738 * x205) + (x742 * x181) + (x739 * x173))); const FLT x744 = -1 * x77; const FLT x745 = x44 * x47; const FLT x746 = x63 * x40; const FLT x747 = 0.5 * x746; const FLT x748 = x64 * x747; const FLT x749 = dt * dt * dt; const FLT x750 = 0.5 * x58 * x749; const FLT x751 = x48 * x750; const FLT x752 = x47 * x47 * x47; const FLT x753 = dt * dt * dt * dt; const FLT x754 = 2 * (1. / (x53 * x53)) * x57; const FLT x755 = x754 * x753; const FLT x756 = 1.0 * x60; const FLT x757 = (1. / (x53 * sqrt(x53))) * x56; const FLT x758 = x756 * x757; const FLT x759 = x758 * x753; const FLT x760 = x45 * x47; const FLT x761 = x51 * x755; const FLT x762 = 2 * x59; const FLT x763 = x51 * x759; const FLT x764 = x64 * x756; const FLT x765 = (x759 * x752) + (x47 * x763) + (-1 * x755 * x752) + (x760 * x759) + (-1 * x760 * x755) + (-1 * x47 * x761) + (-1 * x764 * x745) + (x762 * x745); const FLT x766 = (1. / (x61 * sqrt(x61))) * x43; const FLT x767 = x766 * x765; const FLT x768 = x60 * x766; const FLT x769 = x768 * x765; const FLT x770 = x63 * x41; const FLT x771 = x47 * x770; const FLT x772 = x749 * x757; const FLT x773 = x33 * x772; const FLT x774 = x773 * x771; const FLT x775 = x48 * x772; const FLT x776 = x33 * x750; const FLT x777 = x47 * x776; const FLT x778 = x81 * x777; const FLT x779 = x766 * x582; const FLT x780 = x765 * x556; const FLT x781 = x50 * x772; const FLT x782 = x63 * x37; const FLT x783 = x47 * x782; const FLT x784 = x50 * x47 * x750; const FLT x785 = (x78 * x784) + (-1 * x781 * x783); const FLT x786 = x785 + x778 + (x76 * x775) + (-1 * x748 * x745) + x744 + (x779 * x780) + (-1 * x73 * x751) + (-1 * x767 * x587) + (-1 * x774) + (-1 * x769 * x557) + (-1 * x644 * x767); const FLT x787 = x766 * x573; const FLT x788 = x766 * x557; const FLT x789 = 0.5 * x64; const FLT x790 = x789 * x745; const FLT x791 = (-1 * x773 * x783) + (x78 * x777); const FLT x792 = (-1 * x81 * x784) + (x771 * x781); const FLT x793 = (-1 * x775 * x746) + (-1 * x76 * x790) + (x84 * x751) + x792 + x74 + (x69 * x765 * x787) + x791 + (-1 * x769 * x582) + (-1 * x767 * x567) + (-1 * x788 * x780); const FLT x794 = x47 * x781 * x746; const FLT x795 = x69 * x788; const FLT x796 = x765 * x584; const FLT x797 = x84 * x784; const FLT x798 = -1 * x79; const FLT x799 = x47 * x773; const FLT x800 = (x76 * x799) + (-1 * x73 * x777); const FLT x801 = x800 + (-1 * x769 * x565) + x798 + (-1 * x790 * x782) + (x775 * x770) + x794 + (x767 * x598) + (-1 * x81 * x751) + (x765 * x795) + (x779 * x796) + (-1 * x797); const FLT x802 = (x786 * sensor_pt[2]) + (-1 * x793 * sensor_pt[1]) + (x801 * sensor_pt[0]); const FLT x803 = x84 * x777; const FLT x804 = x73 * x784; const FLT x805 = x69 * x779; const FLT x806 = x76 * x781; const FLT x807 = x47 * x806; const FLT x808 = x799 * x746; const FLT x809 = (-1 * x775 * x782) + (x78 * x751) + (-1 * x765 * x805) + (x788 * x796) + (-1 * x769 * x573) + x804 + x67 + (-1 * x803) + (-1 * x603 * x767) + (-1 * x770 * x790) + (-1 * x807) + x808; const FLT x810 = (x793 * sensor_pt[0]) + (-1 * x809 * sensor_pt[2]) + (x801 * sensor_pt[1]); const FLT x811 = (x618 * x810) + (-1 * x616 * x786) + (x581 * x809) + (-1 * x611 * x802); const FLT x812 = x21 * x811; const FLT x813 = (-1 * x786 * sensor_pt[0]) + (x809 * sensor_pt[1]) + (x801 * sensor_pt[2]); const FLT x814 = (-1 * x618 * x813) + (x613 * x802) + (-1 * x614 * x809) + (x616 * x793); const FLT x815 = (x614 * x786) + (-1 * x793 * x581) + (-1 * x613 * x810) + (x611 * x813); const FLT x816 = (-1 * x97 * x815) + (x11 * x812) + (x620 * x814); const FLT x817 = (-1 * x30 * x814) + (x28 * x815) + (x8 * x812); const FLT x818 = x815 + (x816 * x268) + (-1 * x817 * x288); const FLT x819 = (-1 * x4 * x812) + (x97 * x814) + (x620 * x815); const FLT x820 = (x819 * x288) + x811 + (-1 * x816 * x310); const FLT x821 = (x820 * x164) + (x818 * x158); const FLT x822 = x814 + (x817 * x310) + (-1 * x819 * x268); const FLT x823 = (x822 * x193) + (-1 * x821 * x192); const FLT x824 = ((x818 * x196) + (-1 * x820 * x195)) * x197; const FLT x825 = (-1 * x824) + (x823 * x191); const FLT x826 = (x822 * x119) + (-1 * x167 * (x821 + (x822 * x153))); const FLT x827 = x826 * x176; const FLT x828 = (x120 * (x827 + (-1 * x826 * x175))) + (x826 * x174); const FLT x829 = (x826 * x179) + (x828 * x120); const FLT x830 = x824 + (-1 * x141 * (x823 + (x829 * x181) + (-1 * x204 * ((x825 * x202) + (-1 * x133 * ((x829 * x120) + (x826 * x188) + (x826 * x182) + (x120 * (x829 + (x826 * x183) + (x120 * (x828 + (x120 * ((-1 * x826 * x186) + x827 + (x826 * x184))) + (x826 * x187))))))))) + (-1 * x825 * x205) + (x826 * x173))); const FLT x831 = x51 * x772; const FLT x832 = x33 * x781; const FLT x833 = x770 * x832; const FLT x834 = x51 * x750; const FLT x835 = x50 * x755; const FLT x836 = x50 * x759; const FLT x837 = x68 * x44; const FLT x838 = (x50 * x50 * x50) * x753; const FLT x839 = x44 * x762; const FLT x840 = (x50 * x839) + (x758 * x838) + (-1 * x754 * x838) + (x48 * x836) + (-1 * x756 * x837) + (x45 * x836) + (-1 * x48 * x835) + (-1 * x45 * x835); const FLT x841 = x766 * x840; const FLT x842 = x69 * x841; const FLT x843 = x768 * x840; const FLT x844 = x584 * x840; const FLT x845 = x50 * x776; const FLT x846 = x81 * x845; const FLT x847 = x556 * x840; const FLT x848 = (x78 * x834) + (x779 * x847) + (-1 * x833) + (-1 * x557 * x843) + x67 + (-1 * x804) + (-1 * x782 * x831) + x807 + (-1 * x565 * x842) + (-1 * x787 * x844) + (-1 * x747 * x837) + x846; const FLT x849 = 0.5 * x837; const FLT x850 = (-1 * x84 * x845) + (x746 * x832); const FLT x851 = x785 + (-1 * x770 * x849) + x850 + (-1 * x805 * x840) + x77 + (x73 * x834) + (-1 * x76 * x831) + (-1 * x573 * x843) + (-1 * x603 * x841) + (x788 * x844); const FLT x852 = -1 * x74; const FLT x853 = x63 * x831; const FLT x854 = x33 * x806; const FLT x855 = x73 * x845; const FLT x856 = x792 + (-1 * x782 * x849) + (x795 * x840) + (-1 * x855) + (x779 * x844) + (x598 * x841) + x852 + (x40 * x853) + (-1 * x84 * x834) + x854 + (-1 * x565 * x843); const FLT x857 = (x78 * x845) + (-1 * x782 * x832); const FLT x858 = (-1 * x794) + (-1 * x582 * x843) + (x573 * x842) + (-1 * x567 * x841) + x857 + (-1 * x81 * x834) + (x41 * x853) + x797 + x798 + (-1 * x76 * x849) + (-1 * x788 * x847); const FLT x859 = (-1 * x851 * sensor_pt[2]) + (x858 * sensor_pt[0]) + (x856 * sensor_pt[1]); const FLT x860 = (-1 * x858 * sensor_pt[1]) + (x848 * sensor_pt[2]) + (x856 * sensor_pt[0]); const FLT x861 = (x618 * x859) + (-1 * x611 * x860) + (-1 * x616 * x848) + (x581 * x851); const FLT x862 = (-1 * x848 * sensor_pt[0]) + (x851 * sensor_pt[1]) + (x856 * sensor_pt[2]); const FLT x863 = (x613 * x860) + (-1 * x618 * x862) + (-1 * x614 * x851) + (x616 * x858); const FLT x864 = (x614 * x848) + (-1 * x581 * x858) + (-1 * x613 * x859) + (x611 * x862); const FLT x865 = (-1 * x97 * x864) + (x30 * x861) + (x620 * x863); const FLT x866 = (x28 * x864) + (-1 * x30 * x863) + (x620 * x861); const FLT x867 = x864 + (x865 * x268) + (-1 * x866 * x288); const FLT x868 = (-1 * x28 * x861) + (x620 * x864) + (x97 * x863); const FLT x869 = x861 + (x868 * x288) + (-1 * x865 * x310); const FLT x870 = (x869 * x164) + (x867 * x158); const FLT x871 = (-1 * x868 * x268) + x863 + (x866 * x310); const FLT x872 = (x871 * x193) + (-1 * x870 * x192); const FLT x873 = ((x867 * x196) + (-1 * x869 * x195)) * x197; const FLT x874 = (-1 * x873) + (x872 * x191); const FLT x875 = (x871 * x119) + (-1 * x167 * (x870 + (x871 * x153))); const FLT x876 = x875 * x176; const FLT x877 = (x120 * (x876 + (-1 * x875 * x175))) + (x875 * x174); const FLT x878 = (x875 * x179) + (x877 * x120); const FLT x879 = x873 + (-1 * x141 * (x872 + (-1 * x204 * ((x874 * x202) + (-1 * x133 * ((x878 * x120) + (x875 * x188) + (x875 * x182) + (x120 * (x878 + (x875 * x183) + (x120 * ((x120 * ((-1 * x875 * x186) + x876 + (x875 * x184))) + x877 + (x875 * x187))))))))) + (x878 * x181) + (-1 * x874 * x205) + (x875 * x173))); const FLT x880 = x48 * x33; const FLT x881 = x44 * x33; const FLT x882 = x33 * x33 * x33; const FLT x883 = (x759 * x882) + (-1 * x755 * x882) + (-1 * x755 * x880) + (x759 * x880) + (x33 * x763) + (-1 * x33 * x761) + (-1 * x764 * x881) + (x33 * x839); const FLT x884 = x766 * x883; const FLT x885 = x45 * x772; const FLT x886 = x63 * x885; const FLT x887 = x69 * x884; const FLT x888 = x45 * x750; const FLT x889 = x789 * x881; const FLT x890 = x768 * x883; const FLT x891 = (-1 * x37 * x886) + (x573 * x887) + (-1 * x567 * x884) + x67 + x803 + x833 + (x78 * x888) + (-1 * x808) + (-1 * x76 * x889) + (-1 * x557 * x556 * x884) + (-1 * x846) + (-1 * x582 * x890); const FLT x892 = x584 * x884; const FLT x893 = x582 * x884; const FLT x894 = x791 + x855 + (x557 * x892) + (-1 * x573 * x890) + (-1 * x603 * x884) + (-1 * x854) + (x40 * x886) + (-1 * x69 * x893) + x852 + (-1 * x770 * x889) + (-1 * x84 * x888); const FLT x895 = x850 + (-1 * x565 * x890) + (x598 * x884) + (x557 * x887) + (-1 * x73 * x888) + (-1 * x778) + (x584 * x893) + (-1 * x782 * x889) + (x76 * x885) + x774 + x744; const FLT x896 = (-1 * x894 * sensor_pt[2]) + (x891 * sensor_pt[0]) + (x895 * sensor_pt[1]); const FLT x897 = x857 + x800 + (-1 * x748 * x881) + x79 + (-1 * x41 * x886) + (-1 * x557 * x890) + (-1 * x573 * x892) + (-1 * x565 * x887) + (x556 * x893) + (x81 * x888); const FLT x898 = (x895 * sensor_pt[2]) + (x894 * sensor_pt[1]) + (-1 * x897 * sensor_pt[0]); const FLT x899 = (x611 * x898) + (-1 * x581 * x891) + (x614 * x897) + (-1 * x613 * x896); const FLT x900 = (x897 * sensor_pt[2]) + (-1 * x891 * sensor_pt[1]) + (x895 * sensor_pt[0]); const FLT x901 = (-1 * x900 * x611) + (x618 * x896) + (-1 * x616 * x897) + (x581 * x894); const FLT x902 = (x900 * x613) + (-1 * x614 * x894) + (-1 * x618 * x898) + (x616 * x891); const FLT x903 = (x28 * x899) + (-1 * x30 * x902) + (x901 * x620); const FLT x904 = (-1 * x28 * x901) + (x97 * x902) + (x620 * x899); const FLT x905 = x902 + (x903 * x310) + (-1 * x904 * x268); const FLT x906 = x21 * ((-1 * x97 * x899) + (x30 * x901) + (x902 * x620)); const FLT x907 = x899 + (x906 * x143) + (-1 * x903 * x288); const FLT x908 = x901 + (x904 * x288) + (-1 * x906 * x309); const FLT x909 = (x908 * x164) + (x907 * x158); const FLT x910 = x170 * ((x905 * x119) + (-1 * x167 * (x909 + (x905 * x153)))); const FLT x911 = x126 * x172 * x136; const FLT x912 = x910 * x122; const FLT x913 = (x120 * (x912 + (-1 * x910 * x121))) + (x910 * x123); const FLT x914 = (x910 * x124) + (x913 * x120); const FLT x915 = (x905 * x193) + (-1 * x909 * x192); const FLT x916 = ((x907 * x196) + (-1 * x908 * x195)) * x197; const FLT x917 = (-1 * x916) + (x915 * x191); const FLT x918 = x916 + (-1 * x141 * ((-1 * x917 * x205) + (x911 * x910) + x915 + (-1 * x204 * ((x917 * x202) + (-1 * x133 * ((x910 * x125) + (x914 * x120) + (x910 * x130) + (x120 * (x914 + (x910 * x129) + (x120 * (x913 + (x120 * ((-1 * x910 * x185) + x912 + (x910 * x127))) + (x910 * x128))))))))) + (x914 * x181))); const FLT x919 = -1 * dt * x155; const FLT x920 = (-1 * dt * x156) + dt; const FLT x921 = x920 + x919; const FLT x922 = dt * x161; const FLT x923 = dt * x159; const FLT x924 = x923 + x922; const FLT x925 = (x924 * x164) + (x921 * x158); const FLT x926 = dt * x150; const FLT x927 = dt * x146; const FLT x928 = (-1 * x927) + x926; const FLT x929 = (x928 * x193) + (-1 * x925 * x192); const FLT x930 = ((x921 * x196) + (-1 * x924 * x195)) * x197; const FLT x931 = (-1 * x930) + (x929 * x191); const FLT x932 = (x928 * x119) + (-1 * x167 * (x925 + (x928 * x153))); const FLT x933 = x932 * x176; const FLT x934 = (x120 * (x933 + (-1 * x932 * x175))) + (x932 * x174); const FLT x935 = (x932 * x179) + (x934 * x120); const FLT x936 = x930 + (-1 * x141 * ((-1 * x204 * ((x931 * x202) + (-1 * x133 * ((x932 * x188) + (x120 * (x935 + (x932 * x183) + (x120 * (x934 + (x120 * (x933 + (-1 * x932 * x186) + (x932 * x184))) + (x932 * x187))))) + (x935 * x120) + (x932 * x182))))) + x929 + (x935 * x181) + (-1 * x931 * x205) + (x932 * x173))); const FLT x937 = -1 * dt * x218; const FLT x938 = x920 + x937; const FLT x939 = x926 + x927; const FLT x940 = dt * x214; const FLT x941 = dt * x213; const FLT x942 = (-1 * x941) + x940; const FLT x943 = (x942 * x164) + (x939 * x158); const FLT x944 = (x938 * x119) + (-1 * x167 * (x943 + (x938 * x153))); const FLT x945 = x944 * x170; const FLT x946 = x944 * x176; const FLT x947 = (x120 * (x946 + (-1 * x945 * x121))) + (x944 * x174); const FLT x948 = (x944 * x179) + (x947 * x120); const FLT x949 = (x938 * x193) + (-1 * x943 * x192); const FLT x950 = ((x939 * x196) + (-1 * x942 * x195)) * x197; const FLT x951 = x200 * ((-1 * x950) + (x949 * x191)); const FLT x952 = x950 + (-1 * x141 * (x949 + (-1 * x204 * ((x951 * x201) + (-1 * x133 * ((x944 * x188) + (x948 * x120) + (x944 * x182) + (x120 * (x948 + (x944 * x183) + (x120 * (x947 + (x120 * ((-1 * x945 * x185) + x946 + (x944 * x184))) + (x944 * x187))))))))) + (-1 * x951 * x139) + (x945 * x911) + (x948 * x181))); const FLT x953 = (-1 * x922) + x923; const FLT x954 = x937 + x919 + dt; const FLT x955 = (x954 * x164) + (x953 * x158); const FLT x956 = x940 + x941; const FLT x957 = (x956 * x193) + (-1 * x955 * x192); const FLT x958 = ((x953 * x196) + (-1 * x954 * x195)) * x197; const FLT x959 = (-1 * x958) + (x957 * x191); const FLT x960 = (x956 * x119) + (-1 * x167 * (x955 + (x956 * x153))); const FLT x961 = x960 * x170; const FLT x962 = x961 * x122; const FLT x963 = (x120 * (x962 + (-1 * x961 * x121))) + (x961 * x123); const FLT x964 = (x961 * x124) + (x963 * x120); const FLT x965 = x958 + (-1 * x141 * ((x961 * x911) + x957 + (-1 * x204 * ((x959 * x202) + (-1 * x133 * ((x964 * x120) + (x961 * x125) + (x960 * x182) + (x120 * (x964 + (x961 * x129) + (x120 * (x963 + (x120 * ((-1 * x961 * x185) + x962 + (x961 * x127))) + (x961 * x128))))))))) + (-1 * x959 * x205) + (x964 * x181))); const FLT x966 = x203 * x134; const FLT x967 = (x139 + x966) * x141; const FLT x968 = ((-1 * x112 * x139) + (-1 * x966 * x112)) * x141; const FLT x969 = x141 * (x205 + (x966 * x200)); const FLT x970 = x109 * (1 + x190); const FLT x971 = x116 * x169; const FLT x972 = x971 * x105 * x132; const FLT x973 = x972 * x176; const FLT x974 = (x120 * (x973 + (-1 * x972 * x175))) + (x972 * x174); const FLT x975 = (x972 * x179) + (x974 * x120); const FLT x976 = x970 * x191; const FLT x977 = x141 * ((x971 * x171 * x133 * x153) + (-1 * x976 * x205) + (-1 * x204 * ((-1 * x133 * ((x975 * x120) + (x972 * x182) + (x972 * x188) + (x120 * (x975 + (x972 * x183) + (x120 * (x974 + (x120 * (x973 + (-1 * x972 * x186) + (x972 * x184))) + (x972 * x187))))))) + (x976 * x202) + (-1 * x132) + (-1 * x113 * x117 * x131))) + x970 + (x975 * x181)); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x206 + (x208 * x206)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x228 + (x208 * x228)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x241 + (x208 * x241)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x343 + (x208 * x343)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x399 + (x208 * x399)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x433 + (x433 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x463 + (x463 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x485 + (x485 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x500 + (x500 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x512 + (x512 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), x526 + (x526 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x539 + (x539 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x551 + (x551 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x637 + (x637 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x679 + (x679 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x713 + (x713 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x743 + (x743 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x830 + (x830 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x879 + (x879 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x918 + (x918 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x936 + (x936 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x952 + (x952 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x965 + (x965 * x208)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), (-1 * x967 * x208) + (-1 * x967)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), sin(x207)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), x208); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), (-1 * x968 * x208) + (-1 * x968)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), (-1 * x969 * x208) + (-1 * x969)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), (-1 * x977 * x208) + (-1 * x977)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[1]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x6 = (x5 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x7 = x6 * x6; const FLT x8 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (x5 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x9 = x8 * x8; const FLT x10 = (-1 * x5 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]); const FLT x11 = x10 * x10; const FLT x12 = x11 + x9 + x7 + x4; const FLT x13 = 1. / (x12 * x12); const FLT x14 = 1.0 * x10; const FLT x15 = x14 * (*_x0).Lighthouse.Rot[0]; const FLT x16 = 1.0 * (*_x0).Lighthouse.Rot[3]; const FLT x17 = x8 * x16; const FLT x18 = 1.0 * x6; const FLT x19 = x18 * (*_x0).Lighthouse.Rot[2]; const FLT x20 = 1.0 * x3; const FLT x21 = x20 * (*_x0).Lighthouse.Rot[1]; const FLT x22 = (-1 * x21) + (-1 * x19) + x15 + x17; const FLT x23 = x22 * x13; const FLT x24 = 1. / x12; const FLT x25 = (-1 * x24 * x21) + (-1 * x9 * x23) + (-1 * x24 * x19) + (x24 * x15) + (-1 * x4 * x23) + (-1 * x7 * x23) + (x24 * x17) + (-1 * x23 * x11); const FLT x26 = 1. / sqrt(x12); const FLT x27 = x7 * x24; const FLT x28 = x9 * x24; const FLT x29 = x24 * x11; const FLT x30 = x29 + x27 + (x4 * x24) + x28; const FLT x31 = (1. / (x30 * sqrt(x30))) * x26; const FLT x32 = x6 * x31; const FLT x33 = x32 * x25; const FLT x34 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x35 = 1.0/2.0 * x34; const FLT x36 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x37 = 1.0/2.0 * x36; const FLT x38 = 1. / sqrt(x30); const FLT x39 = x38 * (1. / (x12 * sqrt(x12))); const FLT x40 = x39 * x22; const FLT x41 = x3 * x40; const FLT x42 = x8 * x31; const FLT x43 = x42 * x25; const FLT x44 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x45 = 1.0/2.0 * x44; const FLT x46 = x6 * x40; const FLT x47 = x8 * x40; const FLT x48 = x31 * x25; const FLT x49 = x3 * x48; const FLT x50 = x38 * x26; const FLT x51 = x50 * (*_x0).Lighthouse.Rot[1]; const FLT x52 = 0.5 * x51; const FLT x53 = x1 * x50; const FLT x54 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x55 = x50 * x54; const FLT x56 = (-1 * x55 * x44) + (-1 * x52 * x36) + (-1 * x53 * x34); const FLT x57 = x56 + (-1 * x49 * x37) + (x45 * x47) + (-1 * x33 * x35) + (-1 * x41 * x37) + (x43 * x45) + (-1 * x46 * x35); const FLT x58 = x6 * x50; const FLT x59 = 2 * x58; const FLT x60 = x55 * x36; const FLT x61 = x40 * x10; const FLT x62 = x52 * x44; const FLT x63 = x50 * (*_x0).Lighthouse.Rot[0]; const FLT x64 = 0.5 * x63; const FLT x65 = x64 * x34; const FLT x66 = x48 * x10; const FLT x67 = (x66 * x35) + (-1 * x47 * x37) + (-1 * x65) + (-1 * x62) + x60 + (-1 * x43 * x37) + (x61 * x35) + (-1 * x41 * x45) + (-1 * x45 * x49); const FLT x68 = x50 * x10; const FLT x69 = 2 * x68; const FLT x70 = x8 * x50; const FLT x71 = x3 * x50; const FLT x72 = (x58 * x34) + (-1 * x70 * x44) + (x71 * x36); const FLT x73 = dt * fabs(dt); const FLT x74 = 1.0/2.0 * x73; const FLT x75 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x76 = dt * dt; const FLT x77 = x75 * x75; const FLT x78 = x77 * x76; const FLT x79 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x80 = x79 * x79; const FLT x81 = x80 * x76; const FLT x82 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x83 = x82 * x82; const FLT x84 = x83 * x76; const FLT x85 = 1e-10 + x84 + x78 + x81; const FLT x86 = sqrt(x85); const FLT x87 = 0.5 * x86; const FLT x88 = sin(x87); const FLT x89 = x88 * (1. / x86); const FLT x90 = x89 * x75; const FLT x91 = x88 * x88; const FLT x92 = 1. / x85; const FLT x93 = x92 * x91; const FLT x94 = cos(x87); const FLT x95 = (x81 * x93) + (x78 * x93) + (x94 * x94) + (x84 * x93); const FLT x96 = 1. / sqrt(x95); const FLT x97 = dt * x96; const FLT x98 = x90 * x97; const FLT x99 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x100 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x101 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x102 = (-1 * x101 * (*_x0).Object.Pose.Rot[1]) + (-1 * x99 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[0] + (-1 * x100 * (*_x0).Object.Pose.Rot[2]); const FLT x103 = (x100 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x101 * (*_x0).Object.Pose.Rot[2]) + (x99 * (*_x0).Object.Pose.Rot[0]); const FLT x104 = (-1 * x99 * (*_x0).Object.Pose.Rot[1]) + (x101 * (*_x0).Object.Pose.Rot[3]) + (x100 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x105 = (*_x0).Object.Pose.Rot[1] + (-1 * x100 * (*_x0).Object.Pose.Rot[3]) + (x101 * (*_x0).Object.Pose.Rot[0]) + (x99 * (*_x0).Object.Pose.Rot[2]); const FLT x106 = (x105 * x105) + (x102 * x102) + (x103 * x103) + (x104 * x104); const FLT x107 = 1. / sqrt(x106); const FLT x108 = x102 * x107; const FLT x109 = x89 * x97; const FLT x110 = x109 * x107; const FLT x111 = x105 * x110; const FLT x112 = x96 * x94; const FLT x113 = x103 * x107; const FLT x114 = x112 * x113; const FLT x115 = x104 * x107; const FLT x116 = x109 * x115; const FLT x117 = (x79 * x116) + x114 + (x98 * x108) + (-1 * x82 * x111); const FLT x118 = x107 * x112; const FLT x119 = x102 * x118; const FLT x120 = (-1 * x98 * x113) + (-1 * x79 * x111) + x119 + (-1 * x82 * x116); const FLT x121 = x109 * x113; const FLT x122 = x105 * x118; const FLT x123 = x102 * x110; const FLT x124 = (x79 * x123) + x122 + (-1 * x98 * x115) + (x82 * x121); const FLT x125 = (-1 * x124 * sensor_pt[2]) + (x120 * sensor_pt[1]) + (x117 * sensor_pt[0]); const FLT x126 = x104 * x118; const FLT x127 = x105 * x107; const FLT x128 = (x98 * x127) + (x82 * x123) + (-1 * x79 * x121) + x126; const FLT x129 = (x120 * sensor_pt[2]) + (-1 * x128 * sensor_pt[0]) + (x124 * sensor_pt[1]); const FLT x130 = (*_x0).Object.Pose.Pos[0] + (x74 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (2 * ((x128 * x129) + (-1 * x117 * x125))) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0]; const FLT x131 = 1.0/2.0 * x130; const FLT x132 = x50 * x130; const FLT x133 = x54 * x132; const FLT x134 = (-1 * x117 * sensor_pt[1]) + (x120 * sensor_pt[0]) + (x128 * sensor_pt[2]); const FLT x135 = (*_x0).Object.Pose.Pos[1] + (x74 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x117 * x134) + (-1 * x124 * x129))); const FLT x136 = 1.0/2.0 * x135; const FLT x137 = (*_x0).Object.Pose.Pos[2] + (2 * ((x124 * x125) + (-1 * x128 * x134))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x74 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x138 = x52 * x137; const FLT x139 = 1.0/2.0 * x137; const FLT x140 = x50 * x135; const FLT x141 = 0.5 * x140; const FLT x142 = x141 * (*_x0).Lighthouse.Rot[0]; const FLT x143 = (-1 * x43 * x131) + (-1 * x142) + (x66 * x136) + (-1 * x49 * x139) + x133 + (-1 * x138) + (-1 * x47 * x131) + (x61 * x136) + (-1 * x41 * x139); const FLT x144 = (x70 * x130) + (x71 * x137) + (-1 * x10 * x140); const FLT x145 = 1.0 * x144; const FLT x146 = x63 * x145; const FLT x147 = (x58 * x135) + (x71 * x130) + (-1 * x70 * x137); const FLT x148 = x25 * x147; const FLT x149 = 1.0 * x147; const FLT x150 = x50 * (*_x0).Lighthouse.Rot[2]; const FLT x151 = 1.0 * x150; const FLT x152 = x10 * x144; const FLT x153 = (x70 * x36) + (x71 * x44) + (-1 * x68 * x34); const FLT x154 = x10 * x153; const FLT x155 = 1.0 * x153; const FLT x156 = x63 * x155; const FLT x157 = x50 * x137; const FLT x158 = (-1 * x54 * x157) + (-1 * x1 * x140) + (-1 * x52 * x130); const FLT x159 = x158 + (x43 * x139) + (-1 * x41 * x131) + (-1 * x33 * x136) + (x47 * x139) + (-1 * x46 * x136) + (-1 * x49 * x131); const FLT x160 = (-1 * x59 * x159) + (x40 * x154) + (-1 * x48 * x152) + (-1 * x156) + (x69 * x143) + (-1 * x72 * x33) + x146 + (x57 * x59) + (-1 * x67 * x69) + (x32 * x148) + (x149 * x150) + (x46 * x147) + (-1 * x40 * x152) + (-1 * x72 * x151) + (-1 * x72 * x46) + (x48 * x154); const FLT x161 = x135 + (2 * ((-1 * x58 * x147) + (x68 * x144))) + (-1 * (x34 + (2 * ((-1 * x72 * x58) + (x68 * x153))))); const FLT x162 = 2 * x161; const FLT x163 = 2 * x70; const FLT x164 = (x68 * x137) + (x3 * x140) + (-1 * x58 * x130); const FLT x165 = x6 * x164; const FLT x166 = (x71 * x34) + (x68 * x44) + (-1 * x58 * x36); const FLT x167 = x40 * x166; const FLT x168 = x64 * x137; const FLT x169 = x141 * (*_x0).Lighthouse.Rot[1]; const FLT x170 = x1 * x132; const FLT x171 = (x33 * x131) + x170 + (-1 * x49 * x136) + (x46 * x131) + (-1 * x66 * x139) + (-1 * x61 * x139) + x168 + (-1 * x169) + (-1 * x41 * x136); const FLT x172 = x64 * x44; const FLT x173 = x52 * x34; const FLT x174 = x53 * x36; const FLT x175 = 2 * (x174 + (x33 * x37) + (-1 * x173) + (-1 * x61 * x45) + (x46 * x37) + (-1 * x49 * x35) + (-1 * x41 * x35) + x172 + (-1 * x66 * x45)); const FLT x176 = 1.0 * x164; const FLT x177 = (x166 * x151) + (-1 * x176 * x150); const FLT x178 = x50 * x16; const FLT x179 = (x178 * x153) + (-1 * x178 * x144); const FLT x180 = x179 + (-1 * x48 * x165) + (x6 * x167) + (-1 * x40 * x165) + (x43 * x144) + (-1 * x47 * x153) + (-1 * x163 * x143) + (-1 * x58 * x175) + (-1 * x43 * x153) + x177 + (x47 * x144) + (x59 * x171) + (x67 * x163) + (x33 * x166); const FLT x181 = (-1 * (x36 + (2 * ((-1 * x70 * x153) + (x58 * x166))))) + x130 + (2 * ((-1 * x70 * x144) + (x58 * x164))); const FLT x182 = 2 * x181; const FLT x183 = x10 * x164; const FLT x184 = x10 * x166; const FLT x185 = x63 * x176; const FLT x186 = 1.0 * x166; const FLT x187 = x63 * x186; const FLT x188 = (-1 * x72 * x178) + (x68 * x175) + (-1 * x69 * x171) + x187 + (-1 * x10 * x167) + (x48 * x183) + (-1 * x57 * x163) + (-1 * x42 * x148) + (-1 * x48 * x184) + (x40 * x183) + (x72 * x43) + (-1 * x47 * x147) + (x178 * x147) + (x163 * x159) + (x72 * x47) + (-1 * x185); const FLT x189 = x137 + (-1 * (x44 + (2 * ((-1 * x68 * x166) + (x70 * x72))))) + (2 * ((-1 * x68 * x164) + (x70 * x147))); const FLT x190 = 2 * x189; const FLT x191 = (x188 * x190) + (x180 * x182); const FLT x192 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x193 = cos(x192); const FLT x194 = 1. / x193; const FLT x195 = x161 * x161; const FLT x196 = x181 * x181; const FLT x197 = x196 + (x189 * x189); const FLT x198 = x197 + x195; const FLT x199 = 1.0/2.0 * x161; const FLT x200 = (1. / (x198 * sqrt(x198))) * x199 * x194; const FLT x201 = 1. / sqrt(x198); const FLT x202 = x201 * x194; const FLT x203 = (x202 * x160) + (-1 * x200 * (x191 + (x160 * x162))); const FLT x204 = asin(x202 * x161); const FLT x205 = 8.0108022e-06 * x204; const FLT x206 = -8.0108022e-06 + (-1 * x205); const FLT x207 = 0.0028679863 + (x204 * x206); const FLT x208 = 1. / (x193 * x193); const FLT x209 = 1. / sqrt(1 + (-1 * x208 * (1. / x198) * x195)); const FLT x210 = x209 * x207; const FLT x211 = x209 * x205; const FLT x212 = x209 * x206; const FLT x213 = x212 * x203; const FLT x214 = (x204 * (x213 + (-1 * x211 * x203))) + (x210 * x203); const FLT x215 = 5.3685255e-06 + (x204 * x207); const FLT x216 = x215 * x209; const FLT x217 = (x216 * x203) + (x214 * x204); const FLT x218 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag; const FLT x219 = atan2(-1 * x189, x181); const FLT x220 = 1. / sqrt(x197); const FLT x221 = tan(x192); const FLT x222 = x220 * x221; const FLT x223 = x222 * x161; const FLT x224 = asin(x223) + (-1 * x219) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase); const FLT x225 = sin(x224); const FLT x226 = (*_x0).BSD0.curve + (-1 * x218 * x225) + (*error_model).BSD0.curve; const FLT x227 = 0.0076069798 + (x215 * x204); const FLT x228 = x204 * x227; const FLT x229 = -8.0108022e-06 + (-1.60216044e-05 * x204); const FLT x230 = x207 + (x204 * x229); const FLT x231 = x215 + (x230 * x204); const FLT x232 = x227 + (x231 * x204); const FLT x233 = (x232 * x204) + x228; const FLT x234 = sin(x192); const FLT x235 = x234 * x226; const FLT x236 = x233 * x235; const FLT x237 = x193 + (-1 * x236); const FLT x238 = 1. / x237; const FLT x239 = x204 * x204; const FLT x240 = x238 * x239; const FLT x241 = x226 * x240; const FLT x242 = x238 * x209 * x228; const FLT x243 = 2 * x226; const FLT x244 = x242 * x243; const FLT x245 = 1. / x197; const FLT x246 = x221 * x221; const FLT x247 = 1. / sqrt(1 + (-1 * x245 * x246 * x195)); const FLT x248 = x221 * (1. / (x197 * sqrt(x197))) * x199; const FLT x249 = (x222 * x160) + (-1 * x248 * x191); const FLT x250 = 1. / x181; const FLT x251 = x189 * (1. / x196); const FLT x252 = x245 * x196; const FLT x253 = ((x251 * x180) + (-1 * x250 * x188)) * x252; const FLT x254 = (-1 * x253) + (x247 * x249); const FLT x255 = x227 * x240; const FLT x256 = x218 * cos(x224); const FLT x257 = x256 * x255; const FLT x258 = x232 * x209; const FLT x259 = x231 * x209; const FLT x260 = x209 * x229; const FLT x261 = 2.40324066e-05 * x204; const FLT x262 = x209 * x261; const FLT x263 = x230 * x209; const FLT x264 = x209 * x227; const FLT x265 = x234 * x233; const FLT x266 = x265 * x256; const FLT x267 = (1. / (x237 * x237)) * x239 * x227; const FLT x268 = x267 * x226; const FLT x269 = x223 + (x255 * x226); const FLT x270 = 1. / sqrt(1 + (-1 * (x269 * x269))); const FLT x271 = x253 + (-1 * x270 * ((-1 * x268 * ((x266 * x254) + (-1 * x235 * ((x217 * x204) + (x203 * x258) + (x203 * x264) + (x204 * (x217 + (x203 * x259) + (x204 * (x214 + (x204 * ((-1 * x203 * x262) + x213 + (x203 * x260))) + (x203 * x263))))))))) + x249 + (-1 * x254 * x257) + (x217 * x241) + (x203 * x244))); const FLT x272 = (-1 * asin(x269)) + x219 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha; const FLT x273 = cos(x272) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag); const FLT x274 = x10 * x16; const FLT x275 = 1.0 * x8; const FLT x276 = x275 * (*_x0).Lighthouse.Rot[0]; const FLT x277 = x18 * (*_x0).Lighthouse.Rot[1]; const FLT x278 = x20 * (*_x0).Lighthouse.Rot[2]; const FLT x279 = (-1 * x278) + x277 + (-1 * x274) + x276; const FLT x280 = x39 * x279; const FLT x281 = x6 * x280; const FLT x282 = x13 * x279; const FLT x283 = x31 * ((-1 * x9 * x282) + (x24 * x276) + (-1 * x11 * x282) + (-1 * x24 * x278) + (x24 * x277) + (-1 * x24 * x274) + (-1 * x7 * x282) + (-1 * x4 * x282)); const FLT x284 = 1.0/2.0 * x283; const FLT x285 = x3 * x284; const FLT x286 = x3 * x280; const FLT x287 = x10 * x280; const FLT x288 = x10 * x284; const FLT x289 = x6 * x284; const FLT x290 = (-1 * x285 * x135) + (x281 * x131) + (-1 * x286 * x136) + x158 + (-1 * x287 * x139) + (x289 * x130) + (-1 * x288 * x137); const FLT x291 = x8 * x153; const FLT x292 = x8 * x280; const FLT x293 = x8 * x283; const FLT x294 = x53 * x44; const FLT x295 = x55 * x34; const FLT x296 = x64 * x36; const FLT x297 = x296 + (-1 * x45 * x286) + (-1 * x294) + (x34 * x288) + (-1 * x37 * x293) + (-1 * x37 * x292) + (x35 * x287) + x295 + (-1 * x44 * x285); const FLT x298 = x64 * x130; const FLT x299 = x1 * x157; const FLT x300 = x54 * x140; const FLT x301 = (x288 * x135) + x300 + (-1 * x286 * x139) + (-1 * x299) + x298 + (-1 * x285 * x137) + (-1 * x293 * x131) + (-1 * x292 * x131) + (x287 * x136); const FLT x302 = (x36 * x289) + x56 + (x37 * x281) + (-1 * x45 * x287) + (-1 * x44 * x288) + (-1 * x34 * x285) + (-1 * x35 * x286); const FLT x303 = x283 * x166; const FLT x304 = x280 * x144; const FLT x305 = (x8 * x304) + (-1 * x51 * x186) + (x293 * x144) + (-1 * x291 * x283) + (x281 * x166) + (x59 * x290) + (x51 * x176) + (-1 * x283 * x165) + x156 + (x6 * x303) + (x297 * x163) + (-1 * x59 * x302) + (-1 * x280 * x165) + (-1 * x301 * x163) + (-1 * x146) + (-1 * x291 * x280); const FLT x306 = x63 * x149; const FLT x307 = (-1 * x168) + (-1 * x285 * x130) + x169 + (x292 * x139) + (-1 * x170) + (x293 * x139) + (-1 * x281 * x136) + (-1 * x289 * x135) + (-1 * x286 * x131); const FLT x308 = 1.0 * x72; const FLT x309 = x63 * x308; const FLT x310 = x72 * x283; const FLT x311 = x280 * x147; const FLT x312 = (-1 * x36 * x285) + (-1 * x172) + (-1 * x174) + (x45 * x292) + (-1 * x35 * x281) + (-1 * x37 * x286) + (x45 * x293) + x173 + (-1 * x34 * x289); const FLT x313 = (x280 * x183) + (-1 * x312 * x163) + (-1 * x8 * x311) + (x164 * x178) + (-1 * x280 * x184) + (x69 * x302) + (x283 * x183) + (-1 * x293 * x147) + (-1 * x69 * x290) + (-1 * x10 * x303) + x306 + (-1 * x166 * x178) + (x72 * x292) + (x8 * x310) + (x307 * x163) + (-1 * x309); const FLT x314 = (x313 * x190) + (x305 * x182); const FLT x315 = x6 * x147; const FLT x316 = (x51 * x308) + (-1 * x51 * x149); const FLT x317 = x179 + x316 + (-1 * x10 * x304) + (-1 * x59 * x307) + (-1 * x6 * x310) + (x69 * x301) + (x283 * x154) + (x280 * x154) + (x6 * x311) + (x283 * x315) + (-1 * x69 * x297) + (-1 * x72 * x281) + (-1 * x283 * x152) + (x59 * x312); const FLT x318 = (x222 * x317) + (-1 * x248 * x314); const FLT x319 = ((x251 * x305) + (-1 * x250 * x313)) * x252; const FLT x320 = (-1 * x319) + (x247 * x318); const FLT x321 = (x202 * x317) + (-1 * x200 * (x314 + (x317 * x162))); const FLT x322 = x212 * x321; const FLT x323 = (x204 * (x322 + (-1 * x211 * x321))) + (x210 * x321); const FLT x324 = (x216 * x321) + (x204 * x323); const FLT x325 = x319 + (-1 * x270 * (x318 + (-1 * x268 * ((x266 * x320) + (-1 * x235 * ((x204 * x324) + (x264 * x321) + (x258 * x321) + (x204 * (x324 + (x259 * x321) + (x204 * (x323 + (x204 * ((-1 * x262 * x321) + x322 + (x260 * x321))) + (x263 * x321))))))))) + (x241 * x324) + (-1 * x257 * x320) + (x244 * x321))); const FLT x326 = x14 * (*_x0).Lighthouse.Rot[2]; const FLT x327 = x275 * (*_x0).Lighthouse.Rot[1]; const FLT x328 = x18 * (*_x0).Lighthouse.Rot[0]; const FLT x329 = x3 * x16; const FLT x330 = (-1 * x329) + x328 + x326 + (-1 * x327); const FLT x331 = x39 * x330; const FLT x332 = x13 * x330; const FLT x333 = (-1 * x24 * x329) + (-1 * x4 * x332) + (x24 * x326) + (x24 * x328) + (-1 * x11 * x332) + (-1 * x9 * x332) + (-1 * x24 * x327) + (-1 * x7 * x332); const FLT x334 = x31 * x333; const FLT x335 = x10 * x334; const FLT x336 = x6 * x331; const FLT x337 = x10 * x331; const FLT x338 = x32 * x333; const FLT x339 = x334 * x136; const FLT x340 = x331 * x136; const FLT x341 = (-1 * x298) + (-1 * x3 * x340) + (-1 * x300) + (-1 * x335 * x139) + (x336 * x131) + (-1 * x337 * x139) + (-1 * x3 * x339) + x299 + (x338 * x131); const FLT x342 = x42 * x333; const FLT x343 = x3 * x331; const FLT x344 = x3 * x334; const FLT x345 = x8 * x331; const FLT x346 = 2 * (x158 + (x10 * x340) + (-1 * x342 * x131) + (x10 * x339) + (-1 * x345 * x131) + (-1 * x343 * x139) + (-1 * x344 * x139)); const FLT x347 = x56 + (x35 * x335) + (-1 * x45 * x343) + (-1 * x37 * x345) + (-1 * x37 * x342) + (x35 * x337) + (-1 * x45 * x344); const FLT x348 = (x37 * x338) + x294 + (-1 * x35 * x344) + (-1 * x296) + (-1 * x45 * x335) + (-1 * x35 * x343) + (-1 * x295) + (-1 * x45 * x337) + (x37 * x336); const FLT x349 = (-1 * x187) + (-1 * x291 * x331) + (-1 * x342 * x153) + (x345 * x144) + (-1 * x331 * x165) + x185 + (x342 * x144) + (-1 * x70 * x346) + (x59 * x341) + (x51 * x145) + (x338 * x166) + (x336 * x166) + (x347 * x163) + (-1 * x334 * x165) + (-1 * x51 * x155) + (-1 * x59 * x348); const FLT x350 = (x342 * x139) + (-1 * x343 * x131) + x142 + (-1 * x338 * x136) + x138 + (-1 * x336 * x136) + (x345 * x139) + (-1 * x133) + (-1 * x344 * x131); const FLT x351 = (-1 * x37 * x343) + (-1 * x35 * x338) + x65 + (x45 * x342) + (-1 * x60) + (x45 * x345) + (-1 * x37 * x344) + x62 + (-1 * x35 * x336); const FLT x352 = x316 + x177 + (x334 * x183) + (-1 * x351 * x163) + (-1 * x331 * x184) + (-1 * x334 * x184) + (x72 * x345) + (-1 * x342 * x147) + (x72 * x342) + (-1 * x69 * x341) + (x350 * x163) + (x69 * x348) + (-1 * x345 * x147) + (x331 * x183); const FLT x353 = (x352 * x190) + (x349 * x182); const FLT x354 = (-1 * x331 * x152) + (x338 * x147) + (-1 * x59 * x350) + (x331 * x154) + (x315 * x331) + (x144 * x151) + (-1 * x151 * x153) + (-1 * x72 * x336) + x309 + (x334 * x154) + (-1 * x69 * x347) + (-1 * x306) + (x59 * x351) + (-1 * x72 * x338) + (-1 * x334 * x152) + (x68 * x346); const FLT x355 = (x222 * x354) + (-1 * x248 * x353); const FLT x356 = ((x251 * x349) + (-1 * x250 * x352)) * x252; const FLT x357 = (-1 * x356) + (x247 * x355); const FLT x358 = (x202 * x354) + (-1 * x200 * (x353 + (x354 * x162))); const FLT x359 = x212 * x358; const FLT x360 = (x204 * (x359 + (-1 * x211 * x358))) + (x210 * x358); const FLT x361 = (x216 * x358) + (x204 * x360); const FLT x362 = x356 + (-1 * x270 * (x355 + (-1 * x268 * ((x266 * x357) + (-1 * x235 * ((x204 * (x361 + (x259 * x358) + (x204 * (x360 + (x204 * ((-1 * x262 * x358) + x359 + (x260 * x358))) + (x263 * x358))))) + (x204 * x361) + (x258 * x358) + (x264 * x358))))) + (x241 * x361) + (-1 * x257 * x357) + (x244 * x358))); const FLT x363 = 1. / x30; const FLT x364 = 2 * x363; const FLT x365 = x24 * x364; const FLT x366 = x6 * x365; const FLT x367 = x3 * x366; const FLT x368 = x8 * x365; const FLT x369 = x10 * x368; const FLT x370 = -1 * x369; const FLT x371 = x370 + x367; const FLT x372 = x28 * x364; const FLT x373 = x27 * x364; const FLT x374 = -1 + x373; const FLT x375 = x374 + x372; const FLT x376 = x10 * x366; const FLT x377 = -1 * x376; const FLT x378 = x3 * x368; const FLT x379 = -1 * x378; const FLT x380 = x379 + x377; const FLT x381 = (x380 * x190) + (x375 * x182); const FLT x382 = (x202 * x371) + (-1 * x200 * (x381 + (x371 * x162))); const FLT x383 = x212 * x382; const FLT x384 = (x204 * (x383 + (-1 * x211 * x382))) + (x210 * x382); const FLT x385 = (x216 * x382) + (x204 * x384); const FLT x386 = (x222 * x371) + (-1 * x248 * x381); const FLT x387 = ((x251 * x375) + (-1 * x250 * x380)) * x252; const FLT x388 = (-1 * x387) + (x247 * x386); const FLT x389 = x387 + (-1 * x270 * ((-1 * x268 * ((x266 * x388) + (-1 * x235 * ((x264 * x382) + (x258 * x382) + (x204 * x385) + (x204 * (x385 + (x259 * x382) + (x204 * ((x204 * (x383 + (-1 * x262 * x382) + (x260 * x382))) + x384 + (x263 * x382))))))))) + (-1 * x257 * x388) + (x244 * x382) + x386 + (x241 * x385))); const FLT x390 = -1 * x367; const FLT x391 = x390 + x370; const FLT x392 = x3 * x10; const FLT x393 = x392 * x365; const FLT x394 = x8 * x366; const FLT x395 = -1 * x394; const FLT x396 = x395 + x393; const FLT x397 = (x396 * x190) + (x391 * x182); const FLT x398 = x29 * x364; const FLT x399 = x374 + x398; const FLT x400 = (x222 * x399) + (-1 * x248 * x397); const FLT x401 = ((x251 * x391) + (-1 * x250 * x396)) * x252; const FLT x402 = (-1 * x401) + (x400 * x247); const FLT x403 = (x202 * x399) + (-1 * x200 * (x397 + (x399 * x162))); const FLT x404 = x403 * x212; const FLT x405 = (x204 * (x404 + (-1 * x403 * x211))) + (x403 * x210); const FLT x406 = (x403 * x216) + (x405 * x204); const FLT x407 = x401 + (-1 * x270 * (x400 + (-1 * x268 * ((x402 * x266) + (-1 * x235 * ((x406 * x204) + (x204 * (x406 + (x403 * x259) + (x204 * ((x204 * ((-1 * x403 * x262) + x404 + (x403 * x260))) + x405 + (x403 * x263))))) + (x403 * x258) + (x403 * x264))))) + (x403 * x244) + (-1 * x402 * x257) + (x406 * x241))); const FLT x408 = -1 * x393; const FLT x409 = x408 + x395; const FLT x410 = x377 + x378; const FLT x411 = -1 + x372 + x398; const FLT x412 = (x411 * x190) + (x410 * x182); const FLT x413 = (x409 * x202) + (-1 * x200 * (x412 + (x409 * x162))); const FLT x414 = x413 * x212; const FLT x415 = (x204 * (x414 + (-1 * x413 * x211))) + (x413 * x210); const FLT x416 = (x413 * x216) + (x415 * x204); const FLT x417 = (x409 * x222) + (-1 * x412 * x248); const FLT x418 = ((x410 * x251) + (-1 * x411 * x250)) * x252; const FLT x419 = (-1 * x418) + (x417 * x247); const FLT x420 = x418 + (-1 * x270 * ((-1 * x268 * ((x419 * x266) + (-1 * x235 * ((x204 * (x416 + (x413 * x259) + (x204 * (x415 + (x204 * ((-1 * x413 * x262) + x414 + (x413 * x260))) + (x413 * x263))))) + (x416 * x204) + (x413 * x258) + (x413 * x264))))) + (-1 * x419 * x257) + x417 + (x416 * x241) + (x413 * x244))); const FLT x421 = x73 * x363; const FLT x422 = -1 * x28 * x421; const FLT x423 = (-1 * x27 * x421) + x74; const FLT x424 = x423 + x422; const FLT x425 = x24 * x421; const FLT x426 = x8 * x425; const FLT x427 = x3 * x426; const FLT x428 = x6 * x425; const FLT x429 = x10 * x428; const FLT x430 = x429 + x427; const FLT x431 = (x430 * x190) + (x424 * x182); const FLT x432 = x10 * x426; const FLT x433 = x3 * x428; const FLT x434 = (-1 * x433) + x432; const FLT x435 = (x434 * x222) + (-1 * x431 * x248); const FLT x436 = ((x424 * x251) + (-1 * x430 * x250)) * x252; const FLT x437 = (-1 * x436) + (x435 * x247); const FLT x438 = (x434 * x202) + (-1 * x200 * (x431 + (x434 * x162))); const FLT x439 = x438 * x212; const FLT x440 = (x204 * (x439 + (-1 * x438 * x211))) + (x438 * x210); const FLT x441 = (x438 * x216) + (x440 * x204); const FLT x442 = x436 + (-1 * x270 * (x435 + (x441 * x241) + (-1 * x268 * ((x437 * x266) + (-1 * x235 * ((x441 * x204) + (x438 * x258) + (x438 * x264) + (x204 * (x441 + (x438 * x259) + (x204 * (x440 + (x204 * ((-1 * x438 * x262) + x439 + (x438 * x260))) + (x438 * x263))))))))) + (-1 * x437 * x257) + (x438 * x244))); const FLT x443 = x432 + x433; const FLT x444 = x8 * x428; const FLT x445 = x425 * x392; const FLT x446 = (-1 * x445) + x444; const FLT x447 = (x446 * x190) + (x443 * x182); const FLT x448 = -1 * x29 * x421; const FLT x449 = x423 + x448; const FLT x450 = (x449 * x222) + (-1 * x447 * x248); const FLT x451 = ((x443 * x251) + (-1 * x446 * x250)) * x252; const FLT x452 = (-1 * x451) + (x450 * x247); const FLT x453 = (x449 * x202) + (-1 * x200 * (x447 + (x449 * x162))); const FLT x454 = x453 * x209; const FLT x455 = x453 * x212; const FLT x456 = (x204 * (x455 + (-1 * x454 * x205))) + (x453 * x210); const FLT x457 = (x453 * x216) + (x456 * x204); const FLT x458 = x451 + (-1 * x270 * (x450 + (-1 * x268 * ((x452 * x266) + (-1 * x235 * ((x204 * (x457 + (x453 * x259) + (x204 * (x456 + (x204 * (x455 + (-1 * x454 * x261) + (x453 * x260))) + (x453 * x263))))) + (x457 * x204) + (x453 * x264) + (x453 * x258))))) + (x457 * x241) + (-1 * x452 * x257) + (x454 * x238 * x228 * x243))); const FLT x459 = x444 + x445; const FLT x460 = (-1 * x427) + x429; const FLT x461 = x448 + x422 + x74; const FLT x462 = (x461 * x190) + (x460 * x182); const FLT x463 = (x459 * x202) + (-1 * x200 * (x462 + (x459 * x162))); const FLT x464 = (x459 * x222) + (-1 * x462 * x248); const FLT x465 = ((x460 * x251) + (-1 * x461 * x250)) * x252; const FLT x466 = (-1 * x465) + (x464 * x247); const FLT x467 = x463 * x212; const FLT x468 = (x204 * (x467 + (-1 * x463 * x211))) + (x463 * x210); const FLT x469 = (x463 * x216) + (x468 * x204); const FLT x470 = x465 + (-1 * x270 * ((x469 * x241) + (-1 * x268 * ((x466 * x266) + (-1 * x235 * ((x469 * x204) + (x204 * (x469 + (x463 * x259) + (x204 * (x468 + (x204 * ((-1 * x463 * x262) + x467 + (x463 * x260))) + (x463 * x263))))) + (x463 * x258) + (x463 * x264))))) + (x463 * x244) + x464 + (-1 * x466 * x257))); const FLT x471 = 1.0 * (*_x0).Object.Pose.Rot[0]; const FLT x472 = 1.0 * x102; const FLT x473 = 1.0 * x103; const FLT x474 = 1.0 * x104; const FLT x475 = (x474 * (*_x0).Object.Pose.Rot[3]) + (-1 * x473 * (*_x0).Object.Pose.Rot[2]) + (x471 * x105) + (-1 * x472 * (*_x0).Object.Pose.Rot[1]); const FLT x476 = 1. / (x106 * sqrt(x106)); const FLT x477 = x476 * x112; const FLT x478 = 1.0/2.0 * x477; const FLT x479 = x478 * x475; const FLT x480 = 1.0/2.0 * x102; const FLT x481 = x476 * x480; const FLT x482 = x98 * x475; const FLT x483 = x79 * x109; const FLT x484 = 1.0/2.0 * x476; const FLT x485 = x484 * x104; const FLT x486 = x475 * x485; const FLT x487 = x82 * x109; const FLT x488 = x475 * x487; const FLT x489 = x484 * x488; const FLT x490 = x98 * x107; const FLT x491 = 0.5 * x490; const FLT x492 = x491 * (*_x0).Object.Pose.Rot[1]; const FLT x493 = 0.5 * x118; const FLT x494 = x493 * (*_x0).Object.Pose.Rot[2]; const FLT x495 = 0.5 * x79; const FLT x496 = x495 * x110; const FLT x497 = x496 * (*_x0).Object.Pose.Rot[3]; const FLT x498 = x82 * x110; const FLT x499 = 0.5 * x498; const FLT x500 = x499 * (*_x0).Object.Pose.Rot[0]; const FLT x501 = (-1 * x500) + x497 + (-1 * x492) + (-1 * x494); const FLT x502 = (x489 * x105) + (-1 * x483 * x486) + x501 + (-1 * x479 * x103) + (-1 * x482 * x481); const FLT x503 = 2 * x125; const FLT x504 = x484 * x103; const FLT x505 = x475 * x483; const FLT x506 = x496 * (*_x0).Object.Pose.Rot[2]; const FLT x507 = x499 * (*_x0).Object.Pose.Rot[1]; const FLT x508 = x484 * x105; const FLT x509 = x491 * (*_x0).Object.Pose.Rot[0]; const FLT x510 = x493 * (*_x0).Object.Pose.Rot[3]; const FLT x511 = (-1 * x479 * x104) + x509 + (-1 * x481 * x488) + x510 + (-1 * x507) + (x504 * x505) + x506 + (-1 * x482 * x508); const FLT x512 = x477 * x480; const FLT x513 = x98 * x504; const FLT x514 = x496 * (*_x0).Object.Pose.Rot[0]; const FLT x515 = x491 * (*_x0).Object.Pose.Rot[2]; const FLT x516 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x517 = x498 * x516; const FLT x518 = x493 * (*_x0).Object.Pose.Rot[1]; const FLT x519 = (-1 * x518) + (-1 * x517) + (-1 * x514) + x515; const FLT x520 = x519 + (x488 * x485) + (x475 * x513) + (x505 * x508) + (-1 * x475 * x512); const FLT x521 = (-1 * x496 * (*_x0).Object.Pose.Rot[1]) + (x493 * (*_x0).Object.Pose.Rot[0]) + (-1 * x490 * x516) + (-1 * x499 * (*_x0).Object.Pose.Rot[2]); const FLT x522 = x521 + (-1 * x479 * x105) + (-1 * x489 * x103) + (x98 * x486) + (-1 * x481 * x505); const FLT x523 = (x522 * sensor_pt[1]) + (-1 * x511 * sensor_pt[0]) + (x520 * sensor_pt[2]); const FLT x524 = 2 * x128; const FLT x525 = (-1 * x522 * sensor_pt[2]) + (x502 * sensor_pt[0]) + (x520 * sensor_pt[1]); const FLT x526 = 2 * x117; const FLT x527 = 2 * x129; const FLT x528 = (-1 * x525 * x526) + (-1 * x502 * x503) + (x511 * x527) + (x524 * x523); const FLT x529 = 2 * x134; const FLT x530 = (x511 * sensor_pt[2]) + (-1 * x502 * sensor_pt[1]) + (x520 * sensor_pt[0]); const FLT x531 = 2 * x124; const FLT x532 = (x525 * x531) + (x522 * x503) + (-1 * x511 * x529) + (-1 * x524 * x530); const FLT x533 = (-1 * x523 * x531) + (x529 * x502) + (-1 * x522 * x527) + (x526 * x530); const FLT x534 = (-1 * x68 * x533) + (x70 * x528) + (x71 * x532); const FLT x535 = (x58 * x533) + (-1 * x70 * x532) + (x71 * x528); const FLT x536 = x533 + (x69 * x534) + (-1 * x59 * x535); const FLT x537 = (-1 * x58 * x528) + (x71 * x533) + (x68 * x532); const FLT x538 = (x59 * x537) + x528 + (-1 * x534 * x163); const FLT x539 = (x535 * x163) + x532 + (-1 * x69 * x537); const FLT x540 = (x539 * x190) + (x538 * x182); const FLT x541 = (x536 * x202) + (-1 * x200 * (x540 + (x536 * x162))); const FLT x542 = x541 * x212; const FLT x543 = (x204 * (x542 + (-1 * x541 * x211))) + (x541 * x210); const FLT x544 = (x541 * x216) + (x543 * x204); const FLT x545 = (x536 * x222) + (-1 * x540 * x248); const FLT x546 = ((x538 * x251) + (-1 * x539 * x250)) * x252; const FLT x547 = (-1 * x546) + (x545 * x247); const FLT x548 = x546 + (-1 * x270 * (x545 + (-1 * x547 * x257) + (-1 * x268 * ((x547 * x266) + (-1 * x235 * ((x544 * x204) + (x541 * x258) + (x541 * x264) + (x204 * (x544 + (x541 * x259) + (x204 * (x543 + (x204 * ((-1 * x541 * x262) + x542 + (x541 * x260))) + (x541 * x263))))))))) + (x541 * x244) + (x544 * x241))); const FLT x549 = 1.0 * x105; const FLT x550 = (x471 * x104) + (-1 * x472 * (*_x0).Object.Pose.Rot[2]) + (-1 * x549 * (*_x0).Object.Pose.Rot[3]) + (x473 * (*_x0).Object.Pose.Rot[1]); const FLT x551 = x483 * x550; const FLT x552 = x98 * x550; const FLT x553 = x487 * x550; const FLT x554 = x478 * x550; const FLT x555 = x521 + (-1 * x554 * x104) + (-1 * x481 * x553) + (x504 * x551) + (-1 * x508 * x552); const FLT x556 = x507 + (-1 * x509) + (-1 * x510) + (-1 * x506); const FLT x557 = x556 + (-1 * x504 * x553) + (-1 * x554 * x105) + (-1 * x481 * x551) + (x485 * x552); const FLT x558 = (x485 * x553) + (-1 * x512 * x550) + (x508 * x551) + x501 + (x504 * x552); const FLT x559 = (-1 * x515) + (-1 * x554 * x103) + x517 + (x508 * x553) + (-1 * x485 * x551) + x518 + (-1 * x481 * x552) + x514; const FLT x560 = (x559 * sensor_pt[0]) + (-1 * x557 * sensor_pt[2]) + (x558 * sensor_pt[1]); const FLT x561 = (-1 * x559 * sensor_pt[1]) + (x555 * sensor_pt[2]) + (x558 * sensor_pt[0]); const FLT x562 = (x503 * x557) + (-1 * x524 * x561) + (-1 * x529 * x555) + (x531 * x560); const FLT x563 = (x557 * sensor_pt[1]) + (-1 * x555 * sensor_pt[0]) + (x558 * sensor_pt[2]); const FLT x564 = (x526 * x561) + (-1 * x531 * x563) + (-1 * x527 * x557) + (x529 * x559); const FLT x565 = (x524 * x563) + (x527 * x555) + (-1 * x503 * x559) + (-1 * x526 * x560); const FLT x566 = (-1 * x58 * x565) + (x68 * x562) + (x71 * x564); const FLT x567 = (x71 * x562) + (-1 * x68 * x564) + (x70 * x565); const FLT x568 = x565 + (x59 * x566) + (-1 * x567 * x163); const FLT x569 = (-1 * x70 * x562) + (x58 * x564) + (x71 * x565); const FLT x570 = x562 + (x569 * x163) + (-1 * x69 * x566); const FLT x571 = (x570 * x190) + (x568 * x182); const FLT x572 = (x69 * x567) + x564 + (-1 * x59 * x569); const FLT x573 = (x572 * x222) + (-1 * x571 * x248); const FLT x574 = ((x568 * x251) + (-1 * x570 * x250)) * x252; const FLT x575 = (-1 * x574) + (x573 * x247); const FLT x576 = (x572 * x202) + (-1 * x200 * (x571 + (x572 * x162))); const FLT x577 = x576 * x212; const FLT x578 = (x204 * (x577 + (-1 * x576 * x211))) + (x576 * x210); const FLT x579 = (x576 * x216) + (x578 * x204); const FLT x580 = x574 + (-1 * x270 * (x573 + (-1 * x268 * ((x575 * x266) + (-1 * x235 * ((x579 * x204) + (x576 * x264) + (x576 * x258) + (x204 * (x579 + (x576 * x259) + (x204 * (x578 + (x204 * ((-1 * x576 * x262) + x577 + (x576 * x260))) + (x576 * x263))))))))) + (x579 * x241) + (-1 * x575 * x257) + (x576 * x244))); const FLT x581 = (-1 * x474 * (*_x0).Object.Pose.Rot[1]) + (x549 * (*_x0).Object.Pose.Rot[2]) + (-1 * x472 * (*_x0).Object.Pose.Rot[3]) + (x471 * x103); const FLT x582 = x98 * x581; const FLT x583 = x483 * x581; const FLT x584 = x487 * x581; const FLT x585 = x478 * x581; const FLT x586 = (-1 * x585 * x104) + x519 + (-1 * x481 * x584) + (-1 * x508 * x582) + (x504 * x583); const FLT x587 = (x508 * x584) + (-1 * x585 * x103) + x521 + (-1 * x481 * x582) + (-1 * x485 * x583); const FLT x588 = x556 + (x513 * x581) + (x485 * x584) + (-1 * x512 * x581) + (x508 * x583); const FLT x589 = (x586 * sensor_pt[2]) + (-1 * x587 * sensor_pt[1]) + (x588 * sensor_pt[0]); const FLT x590 = (-1 * x504 * x584) + x500 + (-1 * x585 * x105) + x492 + (-1 * x497) + (-1 * x481 * x583) + (x485 * x582) + x494; const FLT x591 = (x587 * sensor_pt[0]) + (-1 * x590 * sensor_pt[2]) + (x588 * sensor_pt[1]); const FLT x592 = (x590 * x503) + (x591 * x531) + (-1 * x529 * x586) + (-1 * x524 * x589); const FLT x593 = (x590 * sensor_pt[1]) + (x588 * sensor_pt[2]) + (-1 * x586 * sensor_pt[0]); const FLT x594 = (x526 * x589) + (-1 * x590 * x527) + (x529 * x587) + (-1 * x593 * x531); const FLT x595 = (x593 * x524) + (x527 * x586) + (-1 * x503 * x587) + (-1 * x591 * x526); const FLT x596 = (-1 * x58 * x595) + (x68 * x592) + (x71 * x594); const FLT x597 = x50 * x595; const FLT x598 = x50 * x592; const FLT x599 = (x8 * x597) + (-1 * x68 * x594) + (x3 * x598); const FLT x600 = (x59 * x596) + x595 + (-1 * x599 * x163); const FLT x601 = (-1 * x8 * x598) + (x58 * x594) + (x3 * x597); const FLT x602 = x592 + (x601 * x163) + (-1 * x69 * x596); const FLT x603 = (x602 * x190) + (x600 * x182); const FLT x604 = x594 + (x69 * x599) + (-1 * x59 * x601); const FLT x605 = (x604 * x222) + (-1 * x603 * x248); const FLT x606 = ((x600 * x251) + (-1 * x602 * x250)) * x252; const FLT x607 = (-1 * x606) + (x605 * x247); const FLT x608 = (x604 * x202) + (-1 * x200 * (x603 + (x604 * x162))); const FLT x609 = x608 * x212; const FLT x610 = (x204 * (x609 + (-1 * x608 * x211))) + (x608 * x210); const FLT x611 = (x608 * x216) + (x610 * x204); const FLT x612 = x606 + (-1 * x270 * ((-1 * x268 * ((x607 * x266) + (-1 * x235 * ((x611 * x204) + (x608 * x258) + (x608 * x264) + (x204 * (x611 + (x608 * x259) + (x204 * ((x204 * ((-1 * x608 * x262) + x609 + (x608 * x260))) + x610 + (x608 * x263))))))))) + (x611 * x241) + x605 + (-1 * x607 * x257) + (x608 * x244))); const FLT x613 = -1 * x373; const FLT x614 = 1 + (-1 * x372); const FLT x615 = x614 + x613; const FLT x616 = x376 + x378; const FLT x617 = (x616 * x190) + (x615 * x182); const FLT x618 = x390 + x369; const FLT x619 = (x618 * x222) + (-1 * x617 * x248); const FLT x620 = ((x615 * x251) + (-1 * x616 * x250)) * x252; const FLT x621 = (-1 * x620) + (x619 * x247); const FLT x622 = (x618 * x202) + (-1 * x200 * (x617 + (x618 * x162))); const FLT x623 = x622 * x212; const FLT x624 = (x204 * (x623 + (-1 * x622 * x211))) + (x622 * x210); const FLT x625 = (x622 * x216) + (x624 * x204); const FLT x626 = x620 + (-1 * x270 * (x619 + (x625 * x241) + (-1 * x621 * x257) + (-1 * x268 * ((x621 * x266) + (-1 * x235 * ((x622 * x264) + (x622 * x258) + (x625 * x204) + (x204 * (x625 + (x622 * x259) + (x204 * ((x204 * ((-1 * x622 * x262) + x623 + (x622 * x260))) + x624 + (x622 * x263))))))))) + (x622 * x244))); const FLT x627 = x369 + x367; const FLT x628 = x408 + x394; const FLT x629 = (x628 * x190) + (x627 * x182); const FLT x630 = -1 * x398; const FLT x631 = 1 + x613 + x630; const FLT x632 = (x631 * x222) + (-1 * x629 * x248); const FLT x633 = ((x627 * x251) + (-1 * x628 * x250)) * x252; const FLT x634 = (-1 * x633) + (x632 * x247); const FLT x635 = (x631 * x202) + (-1 * x200 * (x629 + (x631 * x162))); const FLT x636 = x635 * x212; const FLT x637 = (x204 * (x636 + (-1 * x635 * x211))) + (x635 * x210); const FLT x638 = (x635 * x216) + (x637 * x204); const FLT x639 = x633 + (-1 * x270 * (x632 + (x638 * x241) + (x635 * x244) + (-1 * x634 * x257) + (-1 * x268 * ((x634 * x266) + (-1 * x235 * ((x638 * x204) + (x635 * x264) + (x635 * x258) + (x204 * ((x635 * x259) + x638 + (x204 * ((x204 * ((-1 * x635 * x262) + (x635 * x260) + x636)) + x637 + (x635 * x263))))))))))); const FLT x640 = x379 + x376; const FLT x641 = x614 + x630; const FLT x642 = (x641 * x190) + (x640 * x182); const FLT x643 = x394 + x393; const FLT x644 = (x643 * x222) + (-1 * x642 * x248); const FLT x645 = ((x640 * x251) + (-1 * x641 * x250)) * x252; const FLT x646 = (-1 * x645) + (x644 * x247); const FLT x647 = (x643 * x202) + (-1 * x200 * (x642 + (x643 * x162))); const FLT x648 = x647 * x212; const FLT x649 = (x204 * (x648 + (-1 * x647 * x211))) + (x647 * x210); const FLT x650 = (x647 * x216) + (x649 * x204); const FLT x651 = x645 + (-1 * x270 * ((-1 * x268 * ((x646 * x266) + (-1 * x235 * ((x650 * x204) + (x647 * x264) + (x204 * (x650 + (x647 * x259) + (x204 * (x649 + (x204 * ((-1 * x647 * x262) + (x647 * x260) + x648)) + (x647 * x263))))) + (x647 * x258))))) + (x650 * x241) + (-1 * x646 * x257) + x644 + (x647 * x244))); const FLT x652 = -1 * x121; const FLT x653 = x89 * x495; const FLT x654 = x96 * x115; const FLT x655 = x76 * x654; const FLT x656 = dt * dt * dt; const FLT x657 = 0.5 * x92; const FLT x658 = x657 * x656; const FLT x659 = x80 * x658; const FLT x660 = x480 * x107; const FLT x661 = x79 * x79 * x79; const FLT x662 = dt * dt * dt * dt; const FLT x663 = (1. / (x85 * x85)) * x91; const FLT x664 = 2 * x663; const FLT x665 = x664 * x662; const FLT x666 = x77 * x662; const FLT x667 = x88 * (1. / (x85 * sqrt(x85))); const FLT x668 = 1.0 * x94; const FLT x669 = x668 * x667; const FLT x670 = x79 * x669; const FLT x671 = x83 * x662; const FLT x672 = 2 * x79; const FLT x673 = x672 * x663; const FLT x674 = x76 * x93; const FLT x675 = x669 * x662; const FLT x676 = x76 * x668; const FLT x677 = x89 * x676; const FLT x678 = (x671 * x670) + (-1 * x79 * x677) + (x670 * x666) + (x672 * x674) + (x675 * x661) + (-1 * x673 * x666) + (-1 * x661 * x665) + (-1 * x671 * x673); const FLT x679 = x82 * x89; const FLT x680 = 1. / (x95 * sqrt(x95)); const FLT x681 = dt * x680; const FLT x682 = x679 * x681; const FLT x683 = x678 * x682; const FLT x684 = 1.0/2.0 * x127; const FLT x685 = x678 * x681; const FLT x686 = x90 * x685; const FLT x687 = x94 * x680; const FLT x688 = 1.0/2.0 * x687; const FLT x689 = x678 * x688; const FLT x690 = x96 * x127; const FLT x691 = x656 * x667; const FLT x692 = x79 * x75; const FLT x693 = x692 * x691; const FLT x694 = x693 * x690; const FLT x695 = x96 * x113; const FLT x696 = x80 * x691; const FLT x697 = x692 * x658; const FLT x698 = x697 * x122; const FLT x699 = x89 * x79; const FLT x700 = x699 * x685; const FLT x701 = 1.0/2.0 * x113; const FLT x702 = x96 * x108; const FLT x703 = x82 * x691; const FLT x704 = x79 * x703; const FLT x705 = x82 * x658; const FLT x706 = x79 * x705; const FLT x707 = (x706 * x119) + (-1 * x702 * x704); const FLT x708 = x707 + (x700 * x701) + (x696 * x695) + x698 + (-1 * x689 * x115) + (-1 * x653 * x655) + (-1 * x659 * x114) + (-1 * x694) + (-1 * x683 * x660) + x652 + (-1 * x684 * x686); const FLT x709 = 1.0/2.0 * x115; const FLT x710 = x76 * x695; const FLT x711 = (-1 * x693 * x702) + (x697 * x119); const FLT x712 = x690 * x703; const FLT x713 = (-1 * x706 * x122) + (x79 * x712); const FLT x714 = x713 + x711 + (-1 * x689 * x113) + (-1 * x653 * x710) + (x659 * x126) + (-1 * x696 * x654) + (-1 * x686 * x660) + (x684 * x683) + x116 + (-1 * x709 * x700); const FLT x715 = x654 * x704; const FLT x716 = x705 * x126; const FLT x717 = x79 * x716; const FLT x718 = x76 * x702; const FLT x719 = -1 * x111; const FLT x720 = x480 * x687; const FLT x721 = x720 * x107; const FLT x722 = (x695 * x693) + (-1 * x697 * x114); const FLT x723 = (x684 * x700) + (-1 * x678 * x721) + (-1 * x659 * x122) + (x696 * x690) + x719 + x715 + (-1 * x717) + x722 + (-1 * x653 * x718) + (x683 * x709) + (x686 * x701); const FLT x724 = (x708 * sensor_pt[2]) + (-1 * x714 * sensor_pt[1]) + (x723 * sensor_pt[0]); const FLT x725 = x697 * x126; const FLT x726 = x706 * x114; const FLT x727 = x76 * x690; const FLT x728 = x695 * x704; const FLT x729 = x693 * x654; const FLT x730 = x726 + (-1 * x660 * x700) + x123 + (-1 * x653 * x727) + (-1 * x683 * x701) + (x659 * x119) + (-1 * x725) + (-1 * x696 * x702) + (-1 * x689 * x127) + (x686 * x709) + (-1 * x728) + x729; const FLT x731 = 2 * ((x714 * sensor_pt[0]) + (-1 * x730 * sensor_pt[2]) + (x723 * sensor_pt[1])); const FLT x732 = (x731 * x124) + (x730 * x503) + (-1 * x708 * x529) + (-1 * x724 * x524); const FLT x733 = (x730 * sensor_pt[1]) + (-1 * x708 * sensor_pt[0]) + (x723 * sensor_pt[2]); const FLT x734 = (-1 * x733 * x531) + (x724 * x526) + (-1 * x730 * x527) + (x714 * x529); const FLT x735 = (-1 * x731 * x117) + (x708 * x527) + (-1 * x714 * x503) + (x733 * x524); const FLT x736 = (x68 * x732) + (-1 * x58 * x735) + (x71 * x734); const FLT x737 = (x70 * x735) + (-1 * x68 * x734) + (x71 * x732); const FLT x738 = x735 + (x59 * x736) + (-1 * x737 * x163); const FLT x739 = (-1 * x70 * x732) + (x58 * x734) + (x71 * x735); const FLT x740 = x732 + (x739 * x163) + (-1 * x69 * x736); const FLT x741 = (x740 * x190) + (x738 * x182); const FLT x742 = (x69 * x737) + x734 + (-1 * x59 * x739); const FLT x743 = (x742 * x222) + (-1 * x741 * x248); const FLT x744 = ((x738 * x251) + (-1 * x740 * x250)) * x252; const FLT x745 = (-1 * x744) + (x743 * x247); const FLT x746 = (x742 * x202) + (-1 * x200 * (x741 + (x742 * x162))); const FLT x747 = x746 * x212; const FLT x748 = (x204 * (x747 + (-1 * x746 * x211))) + (x746 * x210); const FLT x749 = (x746 * x216) + (x748 * x204); const FLT x750 = x744 + (-1 * x270 * (x743 + (-1 * x268 * ((x745 * x266) + (-1 * x235 * ((x746 * x264) + (x746 * x258) + (x749 * x204) + (x204 * (x749 + (x746 * x259) + (x204 * (x748 + (x204 * ((-1 * x746 * x262) + x747 + (x746 * x260))) + (x746 * x263))))))))) + (x749 * x241) + (-1 * x745 * x257) + (x746 * x244))); const FLT x751 = x83 * x691; const FLT x752 = x96 * x751; const FLT x753 = x75 * x712; const FLT x754 = x83 * x658; const FLT x755 = x80 * x665; const FLT x756 = x80 * x675; const FLT x757 = x82 * x82 * x82; const FLT x758 = 2 * x674; const FLT x759 = (-1 * x665 * x757) + (x82 * x756) + (x675 * x757) + (-1 * x82 * x677) + (-1 * x82 * x755) + (-1 * x82 * x77 * x665) + (x82 * x758) + (x82 * x669 * x666); const FLT x760 = x759 * x107; const FLT x761 = x682 * x760; const FLT x762 = x688 * x759; const FLT x763 = x90 * x681; const FLT x764 = 1.0/2.0 * x105; const FLT x765 = x764 * x760; const FLT x766 = 0.5 * x679; const FLT x767 = x75 * x705; const FLT x768 = x767 * x122; const FLT x769 = x699 * x681; const FLT x770 = x769 * x759; const FLT x771 = (x770 * x701) + (-1 * x753) + x123 + (x754 * x119) + (-1 * x726) + (-1 * x765 * x763) + (-1 * x655 * x766) + (-1 * x752 * x108) + x728 + (-1 * x480 * x761) + (-1 * x762 * x115) + x768; const FLT x772 = x480 * x760; const FLT x773 = x763 * x759; const FLT x774 = x682 * x759; const FLT x775 = x688 * x105; const FLT x776 = x75 * x703; const FLT x777 = (-1 * x75 * x716) + (x654 * x776); const FLT x778 = x777 + x707 + (-1 * x766 * x727) + x121 + (x754 * x114) + (-1 * x760 * x775) + (-1 * x774 * x701) + (-1 * x695 * x751) + (-1 * x769 * x772) + (x773 * x709); const FLT x779 = -1 * x116; const FLT x780 = x695 * x776; const FLT x781 = x767 * x114; const FLT x782 = (-1 * x766 * x718) + (x773 * x701) + (-1 * x754 * x126) + (x769 * x765) + x779 + (-1 * x760 * x720) + (x654 * x751) + x713 + (x774 * x709) + (-1 * x781) + x780; const FLT x783 = (-1 * x776 * x702) + (x767 * x119); const FLT x784 = (-1 * x766 * x710) + (x764 * x761) + (-1 * x763 * x772) + (-1 * x762 * x113) + (-1 * x754 * x122) + x783 + (-1 * x715) + x719 + (-1 * x770 * x709) + (x752 * x127) + x717; const FLT x785 = (x784 * sensor_pt[0]) + (-1 * x778 * sensor_pt[2]) + (x782 * sensor_pt[1]); const FLT x786 = (x771 * sensor_pt[2]) + (-1 * x784 * sensor_pt[1]) + (x782 * sensor_pt[0]); const FLT x787 = (-1 * x786 * x524) + (x785 * x531) + (-1 * x771 * x529) + (x778 * x503); const FLT x788 = (x778 * sensor_pt[1]) + (-1 * x771 * sensor_pt[0]) + (x782 * sensor_pt[2]); const FLT x789 = (-1 * x788 * x531) + (x786 * x526) + (-1 * x778 * x527) + (x784 * x529); const FLT x790 = (-1 * x785 * x526) + (-1 * x784 * x503) + (x771 * x527) + (x788 * x524); const FLT x791 = (x68 * x787) + (-1 * x58 * x790) + (x71 * x789); const FLT x792 = (-1 * x68 * x789) + (x70 * x790) + (x71 * x787); const FLT x793 = x790 + (x59 * x791) + (-1 * x792 * x163); const FLT x794 = (-1 * x70 * x787) + (x71 * x790) + (x58 * x789); const FLT x795 = x787 + (x794 * x163) + (-1 * x69 * x791); const FLT x796 = (x795 * x190) + (x793 * x182); const FLT x797 = x789 + (-1 * x59 * x794) + (x69 * x792); const FLT x798 = (x797 * x222) + (-1 * x796 * x248); const FLT x799 = ((x793 * x251) + (-1 * x795 * x250)) * x252; const FLT x800 = (-1 * x799) + (x798 * x247); const FLT x801 = (x797 * x202) + (-1 * x200 * (x796 + (x797 * x162))); const FLT x802 = x801 * x212; const FLT x803 = (x204 * (x802 + (-1 * x801 * x211))) + (x801 * x210); const FLT x804 = (x801 * x216) + (x803 * x204); const FLT x805 = x799 + (-1 * x270 * (x798 + (-1 * x268 * ((x800 * x266) + (-1 * x235 * ((x804 * x204) + (x801 * x264) + (x801 * x258) + (x204 * (x804 + (x801 * x259) + (x204 * (x803 + (x204 * ((-1 * x801 * x262) + x802 + (x801 * x260))) + (x801 * x263))))))))) + (x804 * x241) + (-1 * x800 * x257) + (x801 * x244))); const FLT x806 = (x75 * x75 * x75) * x662; const FLT x807 = (x669 * x806) + (x75 * x671 * x669) + (-1 * x664 * x806) + (-1 * x75 * x755) + (x75 * x756) + (x75 * x758) + (-1 * x83 * x75 * x665) + (-1 * x90 * x676); const FLT x808 = x807 * x107; const FLT x809 = x480 * x808; const FLT x810 = x77 * x656; const FLT x811 = x667 * x810; const FLT x812 = x96 * x811; const FLT x813 = x682 * x807; const FLT x814 = x657 * x810; const FLT x815 = 0.5 * x90; const FLT x816 = x688 * x807; const FLT x817 = x769 * x807; const FLT x818 = (-1 * x709 * x817) + x123 + (-1 * x812 * x108) + x725 + (x684 * x813) + (-1 * x763 * x809) + x753 + (x814 * x119) + (-1 * x710 * x815) + (-1 * x768) + (-1 * x816 * x113) + (-1 * x729); const FLT x819 = x763 * x807; const FLT x820 = (x709 * x819) + (-1 * x769 * x809) + x779 + x781 + (-1 * x780) + (-1 * x775 * x808) + (x654 * x811) + x711 + (-1 * x701 * x813) + (-1 * x727 * x815) + (-1 * x814 * x126); const FLT x821 = x777 + (-1 * x721 * x807) + (-1 * x718 * x815) + (x709 * x813) + (x812 * x113) + (x684 * x817) + (-1 * x698) + (-1 * x814 * x114) + x694 + x652 + (x701 * x819); const FLT x822 = (-1 * x820 * sensor_pt[2]) + (x818 * sensor_pt[0]) + (x821 * sensor_pt[1]); const FLT x823 = x722 + (-1 * x812 * x127) + (-1 * x655 * x815) + (x814 * x122) + (-1 * x816 * x115) + (x701 * x817) + x783 + (-1 * x682 * x809) + x111 + (-1 * x684 * x819); const FLT x824 = (x820 * sensor_pt[1]) + (x821 * sensor_pt[2]) + (-1 * x823 * sensor_pt[0]); const FLT x825 = (x524 * x824) + (x527 * x823) + (-1 * x503 * x818) + (-1 * x526 * x822); const FLT x826 = (x823 * sensor_pt[2]) + (-1 * x818 * sensor_pt[1]) + (x821 * sensor_pt[0]); const FLT x827 = (-1 * x524 * x826) + (x531 * x822) + (-1 * x529 * x823) + (x503 * x820); const FLT x828 = (x526 * x826) + (-1 * x531 * x824) + (-1 * x527 * x820) + (x529 * x818); const FLT x829 = (-1 * x68 * x828) + (x70 * x825) + (x71 * x827); const FLT x830 = (-1 * x70 * x827) + (x58 * x828) + (x71 * x825); const FLT x831 = x828 + (x69 * x829) + (-1 * x59 * x830); const FLT x832 = (-1 * x58 * x825) + (x68 * x827) + (x71 * x828); const FLT x833 = x825 + (x59 * x832) + (-1 * x829 * x163); const FLT x834 = x827 + (x830 * x163) + (-1 * x69 * x832); const FLT x835 = (x834 * x190) + (x833 * x182); const FLT x836 = (x831 * x202) + (-1 * x200 * (x835 + (x831 * x162))); const FLT x837 = x836 * x212; const FLT x838 = (x204 * (x837 + (-1 * x836 * x211))) + (x836 * x210); const FLT x839 = (x836 * x216) + (x838 * x204); const FLT x840 = (x831 * x222) + (-1 * x835 * x248); const FLT x841 = ((x833 * x251) + (-1 * x834 * x250)) * x252; const FLT x842 = (-1 * x841) + (x840 * x247); const FLT x843 = x841 + (-1 * x270 * (x840 + (-1 * x842 * x257) + (-1 * x268 * ((x842 * x266) + (-1 * x235 * ((x839 * x204) + (x836 * x258) + (x836 * x264) + (x204 * (x839 + (x836 * x259) + (x204 * (x838 + (x204 * ((-1 * x836 * x262) + x837 + (x836 * x260))) + (x836 * x263))))))))) + (x836 * x244) + (x839 * x241))); const FLT x844 = -1 * dt * x372; const FLT x845 = -1 * dt * x373; const FLT x846 = x845 + x844 + dt; const FLT x847 = dt * x378; const FLT x848 = dt * x376; const FLT x849 = x848 + x847; const FLT x850 = (x849 * x190) + (x846 * x182); const FLT x851 = dt * x369; const FLT x852 = dt * x367; const FLT x853 = (-1 * x852) + x851; const FLT x854 = (x853 * x222) + (-1 * x850 * x248); const FLT x855 = ((x846 * x251) + (-1 * x849 * x250)) * x252; const FLT x856 = (-1 * x855) + (x854 * x247); const FLT x857 = (x853 * x202) + (-1 * x200 * (x850 + (x853 * x162))); const FLT x858 = x857 * x212; const FLT x859 = (x204 * (x858 + (-1 * x857 * x211))) + (x857 * x210); const FLT x860 = (x857 * x216) + (x859 * x204); const FLT x861 = x855 + (-1 * x270 * ((-1 * x268 * ((x856 * x266) + (-1 * x235 * ((x860 * x204) + (x204 * (x860 + (x857 * x259) + (x204 * (x859 + (x204 * ((-1 * x857 * x262) + x858 + (x857 * x260))) + (x857 * x263))))) + (x857 * x264) + (x857 * x258))))) + x854 + (x860 * x241) + (-1 * x856 * x257) + (x857 * x244))); const FLT x862 = (-1 * dt * x398) + dt; const FLT x863 = x862 + x845; const FLT x864 = x851 + x852; const FLT x865 = dt * x394; const FLT x866 = dt * x393; const FLT x867 = (-1 * x866) + x865; const FLT x868 = (x867 * x190) + (x864 * x182); const FLT x869 = (x863 * x202) + (-1 * x200 * (x868 + (x863 * x162))); const FLT x870 = x869 * x212; const FLT x871 = (x204 * (x870 + (-1 * x869 * x211))) + (x869 * x210); const FLT x872 = (x869 * x216) + (x871 * x204); const FLT x873 = (x863 * x222) + (-1 * x868 * x248); const FLT x874 = ((x864 * x251) + (-1 * x867 * x250)) * x252; const FLT x875 = x256 * ((-1 * x874) + (x873 * x247)); const FLT x876 = x874 + (-1 * x270 * (x873 + (-1 * x875 * x255) + (-1 * x268 * ((x875 * x265) + (-1 * x235 * ((x872 * x204) + (x869 * x264) + (x869 * x258) + (x204 * ((x869 * x259) + x872 + (x204 * (x871 + (x204 * ((-1 * x869 * x262) + x870 + (x869 * x260))) + (x869 * x263))))))))) + (x869 * x244) + (x872 * x241))); const FLT x877 = (-1 * x847) + x848; const FLT x878 = x862 + x844; const FLT x879 = (x878 * x190) + (x877 * x182); const FLT x880 = x865 + x866; const FLT x881 = (x880 * x222) + (-1 * x879 * x248); const FLT x882 = ((x877 * x251) + (-1 * x878 * x250)) * x252; const FLT x883 = (-1 * x882) + (x881 * x247); const FLT x884 = (x880 * x202) + (-1 * x200 * (x879 + (x880 * x162))); const FLT x885 = x884 * x212; const FLT x886 = (x204 * (x885 + (-1 * x884 * x211))) + (x884 * x210); const FLT x887 = (x884 * x216) + (x886 * x204); const FLT x888 = x882 + (-1 * x270 * (x881 + (x884 * x244) + (-1 * x883 * x257) + (-1 * x268 * ((x883 * x266) + (-1 * x235 * ((x887 * x204) + (x884 * x264) + (x884 * x258) + (x204 * (x887 + (x884 * x259) + (x204 * ((x204 * (x885 + (-1 * x884 * x262) + (x884 * x260))) + x886 + (x884 * x263))))))))) + (x887 * x241))); const FLT x889 = x236 * x267; const FLT x890 = (x255 + x889) * x270; const FLT x891 = ((-1 * x255 * x225) + (-1 * x889 * x225)) * x270; const FLT x892 = x270 * (x257 + (x889 * x256)); const FLT x893 = x220 * x161 * (1 + x246); const FLT x894 = x201 * x208; const FLT x895 = x894 * x234 * x161; const FLT x896 = x895 * x212; const FLT x897 = (x204 * (x896 + (-1 * x895 * x211))) + (x895 * x210); const FLT x898 = (x895 * x216) + (x897 * x204); const FLT x899 = x893 * x247; const FLT x900 = x270 * ((-1 * x899 * x257) + (-1 * x268 * ((-1 * x235 * ((x898 * x204) + (x895 * x264) + (x895 * x258) + (x204 * ((x895 * x259) + x898 + (x204 * ((x204 * ((-1 * x895 * x262) + x896 + (x895 * x260))) + x897 + (x895 * x263))))))) + (x899 * x266) + (-1 * x234) + (-1 * x233 * x226 * x193))) + x893 + (x894 * x235 * x242 * x162) + (x898 * x241)); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x271 + (x273 * x271)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x325 + (x273 * x325)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x362 + (x273 * x362)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x389 + (x273 * x389)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x407 + (x407 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x420 + (x420 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x442 + (x442 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x458 + (x458 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x470 + (x470 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x548 + (x548 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x580 + (x580 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x612 + (x612 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x626 + (x626 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x639 + (x639 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x651 + (x651 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x750 + (x750 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x805 + (x805 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x843 + (x843 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x861 + (x861 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x876 + (x876 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x888 + (x888 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), (-1 * x890 * x273) + (-1 * x890)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), sin(x272)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), x273); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), (-1 * x891 * x273) + (-1 * x891)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), (-1 * x892 * x273) + (-1 * x892)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), (-1 * x900 * x273) + (-1 * x900)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x3 = (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x4 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x5 = dt * dt; const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x4 * x4) * x5; const FLT x11 = 1e-10 + x7 + x10 + x9; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2]; const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x3 * x3) + (x16 * x16) + (x18 * x18) + (x17 * x17))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x3 * x23; const FLT x25 = x20 * x21; const FLT x26 = x25 * x17; const FLT x27 = x6 * x22; const FLT x28 = x27 * x18; const FLT x29 = x8 * x22; const FLT x30 = x29 * x16; const FLT x31 = x28 + (-1 * x30) + x24 + x26; const FLT x32 = (x3 * x25) + (-1 * x27 * x16) + (-1 * x29 * x18) + (-1 * x23 * x17); const FLT x33 = x3 * x27; const FLT x34 = x23 * x18; const FLT x35 = x25 * x16; const FLT x36 = x29 * x17; const FLT x37 = x36 + x35 + x33 + (-1 * x34); const FLT x38 = (-1 * x37 * sensor_pt[1]) + (x32 * sensor_pt[0]) + (x31 * sensor_pt[2]); const FLT x39 = x27 * x17; const FLT x40 = x23 * x16; const FLT x41 = x25 * x18; const FLT x42 = x3 * x29; const FLT x43 = x42 + x41 + (-1 * x39) + x40; const FLT x44 = (-1 * x43 * sensor_pt[2]) + (x32 * sensor_pt[1]) + (x37 * sensor_pt[0]); const FLT x45 = 1.0/2.0 * dt * fabs(dt); const FLT x46 = (*_x0).Object.Pose.Pos[2] + (2 * ((x43 * x44) + (-1 * x31 * x38))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x47 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x50 = (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x47 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*_x0).Lighthouse.Rot[2]); const FLT x51 = (x48 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x47 * (*_x0).Lighthouse.Rot[0]) + (-1 * x49 * (*_x0).Lighthouse.Rot[2]); const FLT x52 = x51 * x51; const FLT x53 = x50 * x50; const FLT x54 = (-1 * x47 * (*_x0).Lighthouse.Rot[1]) + (x48 * (*_x0).Lighthouse.Rot[0]) + (x49 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x55 = x54 * x54; const FLT x56 = (*_x0).Lighthouse.Rot[1] + (x49 * (*_x0).Lighthouse.Rot[0]) + (-1 * x48 * (*_x0).Lighthouse.Rot[3]) + (x47 * (*_x0).Lighthouse.Rot[2]); const FLT x57 = x56 * x56; const FLT x58 = x55 + x52 + x57 + x53; const FLT x59 = 1. / x58; const FLT x60 = (1. / sqrt((x53 * x59) + (x57 * x59) + (x52 * x59) + (x55 * x59))) * (1. / sqrt(x58)); const FLT x61 = x60 * x50; const FLT x62 = (-1 * x31 * sensor_pt[0]) + (x32 * sensor_pt[2]) + (x43 * sensor_pt[1]); const FLT x63 = (*_x0).Object.Pose.Pos[1] + (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x38 * x37) + (-1 * x62 * x43))); const FLT x64 = x60 * x56; const FLT x65 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x62 * x31) + (-1 * x44 * x37))); const FLT x66 = x60 * x54; const FLT x67 = (x65 * x66) + (x61 * x46) + (-1 * x63 * x64); const FLT x68 = x60 * x51; const FLT x69 = (x63 * x68) + (x61 * x65) + (-1 * x66 * x46); const FLT x70 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x71 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x72 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x73 = (x72 * x66) + (x70 * x61) + (-1 * x71 * x64); const FLT x74 = (-1 * x70 * x66) + (x71 * x68) + (x72 * x61); const FLT x75 = x63 + (2 * ((-1 * x68 * x69) + (x64 * x67))) + (-1 * (x71 + (2 * ((-1 * x74 * x68) + (x73 * x64))))); const FLT x76 = x75 * x75; const FLT x77 = (x70 * x64) + (x71 * x61) + (-1 * x72 * x68); const FLT x78 = (x64 * x46) + (x63 * x61) + (-1 * x68 * x65); const FLT x79 = (-1 * (x70 + (2 * ((-1 * x77 * x64) + (x74 * x66))))) + x46 + (2 * ((-1 * x78 * x64) + (x66 * x69))); const FLT x80 = x65 + (-1 * (x72 + (2 * ((-1 * x73 * x66) + (x77 * x68))))) + (2 * ((-1 * x67 * x66) + (x78 * x68))); const FLT x81 = x80 * x80; const FLT x82 = x81 + (x79 * x79); const FLT x83 = 1. / x82; const FLT x84 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt; const FLT x85 = tan(x84); const FLT x86 = 1. / sqrt(1 + (-1 * x83 * (x85 * x85) * x76)); const FLT x87 = (-1 * x28) + (-1 * x26) + x30 + (-1 * x24); const FLT x88 = 2 * x31; const FLT x89 = 1 + (x88 * x87) + (-2 * (x37 * x37)); const FLT x90 = 2 * x32; const FLT x91 = x90 * x31; const FLT x92 = 2 * x43; const FLT x93 = (x92 * x37) + (-1 * x91); const FLT x94 = x90 * x37; const FLT x95 = x94 + (-1 * x87 * x92); const FLT x96 = (-1 * x64 * x95) + (x89 * x66) + (x61 * x93); const FLT x97 = 2 * x66; const FLT x98 = (x64 * x93) + (-1 * x89 * x68) + (x61 * x95); const FLT x99 = 2 * x68; const FLT x100 = x89 + (-1 * x97 * x96) + (x99 * x98); const FLT x101 = 2 * x80; const FLT x102 = (-1 * x66 * x93) + (x68 * x95) + (x89 * x61); const FLT x103 = 2 * x64; const FLT x104 = x93 + (x97 * x102) + (-1 * x98 * x103); const FLT x105 = 2 * x79; const FLT x106 = (x105 * x104) + (x101 * x100); const FLT x107 = 1.0/2.0 * x75; const FLT x108 = (1. / (x82 * sqrt(x82))) * x85 * x107; const FLT x109 = x95 + (-1 * x99 * x102) + (x96 * x103); const FLT x110 = (1. / sqrt(x82)) * x85; const FLT x111 = (x109 * x110) + (-1 * x108 * x106); const FLT x112 = 1. / x80; const FLT x113 = (1. / x81) * x79; const FLT x114 = x81 * x83; const FLT x115 = ((x100 * x113) + (-1 * x104 * x112)) * x114; const FLT x116 = (-1 * x115) + (x86 * x111); const FLT x117 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag; const FLT x118 = atan2(-1 * x79, x80); const FLT x119 = x75 * x110; const FLT x120 = asin(x119) + (-1 * x118) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase); const FLT x121 = x117 * cos(x120); const FLT x122 = x82 + x76; const FLT x123 = cos(x84); const FLT x124 = 1. / x123; const FLT x125 = x124 * (1. / sqrt(x122)); const FLT x126 = asin(x75 * x125); const FLT x127 = 8.0108022e-06 * x126; const FLT x128 = -8.0108022e-06 + (-1 * x127); const FLT x129 = 0.0028679863 + (x128 * x126); const FLT x130 = 5.3685255e-06 + (x126 * x129); const FLT x131 = 0.0076069798 + (x126 * x130); const FLT x132 = x126 * x131; const FLT x133 = -8.0108022e-06 + (-1.60216044e-05 * x126); const FLT x134 = x129 + (x126 * x133); const FLT x135 = x130 + (x126 * x134); const FLT x136 = x131 + (x126 * x135); const FLT x137 = (x126 * x136) + x132; const FLT x138 = (*_x0).BSD0.curve + (-1 * x117 * sin(x120)) + (*error_model).BSD0.curve; const FLT x139 = sin(x84); const FLT x140 = x138 * x139; const FLT x141 = x123 + (-1 * x137 * x140); const FLT x142 = 1. / x141; const FLT x143 = x126 * x126; const FLT x144 = x131 * x142 * x143; const FLT x145 = x121 * x144; const FLT x146 = 1. / sqrt(1 + (-1 * x76 * (1. / (x123 * x123)) * (1. / x122))); const FLT x147 = 2 * x75; const FLT x148 = x107 * x124 * (1. / (x122 * sqrt(x122))); const FLT x149 = (x109 * x125) + (-1 * x148 * (x106 + (x109 * x147))); const FLT x150 = x146 * x149; const FLT x151 = x128 * x150; const FLT x152 = (x126 * (x151 + (-1 * x127 * x150))) + (x129 * x150); const FLT x153 = (x130 * x150) + (x126 * x152); const FLT x154 = x138 * x143; const FLT x155 = x142 * x154; const FLT x156 = 2 * x132 * x138 * x142; const FLT x157 = 2.40324066e-05 * x126; const FLT x158 = x131 * x146; const FLT x159 = x137 * x139; const FLT x160 = x121 * x159; const FLT x161 = x131 * (1. / (x141 * x141)) * x154; const FLT x162 = x119 + (x131 * x155); const FLT x163 = 1. / sqrt(1 + (-1 * (x162 * x162))); const FLT x164 = x115 + (-1 * x163 * (x111 + (-1 * x161 * ((x116 * x160) + (-1 * x140 * ((x149 * x158) + (x136 * x150) + (x126 * x153) + (x126 * ((x135 * x150) + x153 + (x126 * (x152 + (x126 * ((-1 * x150 * x157) + x151 + (x133 * x150))) + (x134 * x150))))))))) + (x150 * x156) + (-1 * x116 * x145) + (x153 * x155))); const FLT x165 = ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag) * cos(x118 + (*error_model).BSD0.gibpha + (-1 * asin(x162)) + (*_x0).BSD0.gibpha); const FLT x166 = (-1 * x33) + (-1 * x36) + x34 + (-1 * x35); const FLT x167 = 2 * x37; const FLT x168 = 1 + (x167 * x166) + (-2 * (x43 * x43)); const FLT x169 = (x92 * x31) + (-1 * x94); const FLT x170 = x90 * x43; const FLT x171 = x170 + (-1 * x88 * x166); const FLT x172 = (-1 * x66 * x171) + (x68 * x168) + (x61 * x169); const FLT x173 = (-1 * x64 * x168) + (x66 * x169) + (x61 * x171); const FLT x174 = x168 + (-1 * x99 * x172) + (x103 * x173); const FLT x175 = (-1 * x68 * x169) + (x64 * x171) + (x61 * x168); const FLT x176 = x169 + (-1 * x97 * x173) + (x99 * x175); const FLT x177 = x171 + (x97 * x172) + (-1 * x103 * x175); const FLT x178 = (x105 * x177) + (x101 * x176); const FLT x179 = (x125 * x174) + (-1 * x148 * (x178 + (x174 * x147))); const FLT x180 = x179 * x146; const FLT x181 = x128 * x180; const FLT x182 = (x126 * (x181 + (-1 * x127 * x180))) + (x129 * x180); const FLT x183 = (x180 * x130) + (x126 * x182); const FLT x184 = (x110 * x174) + (-1 * x108 * x178); const FLT x185 = ((x113 * x176) + (-1 * x112 * x177)) * x114; const FLT x186 = x121 * ((-1 * x185) + (x86 * x184)); const FLT x187 = x185 + (-1 * x163 * (x184 + (-1 * x186 * x144) + (-1 * x161 * ((x186 * x159) + (-1 * x140 * ((x126 * x183) + (x179 * x158) + (x180 * x136) + (x126 * (x183 + (x180 * x135) + (x126 * (x182 + (x126 * (x181 + (-1 * x180 * x157) + (x180 * x133))) + (x180 * x134))))))))) + (x183 * x155) + (x180 * x156))); const FLT x188 = (x88 * x37) + (-1 * x170); const FLT x189 = (-1 * x41) + (-1 * x42) + (-1 * x40) + x39; const FLT x190 = x91 + (-1 * x167 * x189); const FLT x191 = 1 + (x92 * x189) + (-2 * (x31 * x31)); const FLT x192 = (-1 * x66 * x191) + (x68 * x188) + (x61 * x190); const FLT x193 = (-1 * x64 * x188) + (x66 * x190) + (x61 * x191); const FLT x194 = x188 + (-1 * x99 * x192) + (x103 * x193); const FLT x195 = (-1 * x68 * x190) + (x64 * x191) + (x61 * x188); const FLT x196 = x190 + (-1 * x97 * x193) + (x99 * x195); const FLT x197 = x191 + (x97 * x192) + (-1 * x103 * x195); const FLT x198 = (x105 * x197) + (x101 * x196); const FLT x199 = (x125 * x194) + (-1 * x148 * (x198 + (x194 * x147))); const FLT x200 = x199 * x146; const FLT x201 = x200 * x128; const FLT x202 = (x126 * (x201 + (-1 * x200 * x127))) + (x200 * x129); const FLT x203 = (x200 * x130) + (x202 * x126); const FLT x204 = (x110 * x194) + (-1 * x108 * x198); const FLT x205 = ((x113 * x196) + (-1 * x112 * x197)) * x114; const FLT x206 = (-1 * x205) + (x86 * x204); const FLT x207 = x205 + (-1 * x163 * ((-1 * x206 * x145) + (x203 * x155) + x204 + (-1 * x161 * ((x206 * x160) + (-1 * x140 * ((x199 * x158) + (x200 * x136) + (x203 * x126) + (x126 * ((x200 * x135) + x203 + (x126 * (x202 + (x126 * ((-1 * x200 * x157) + (x200 * x133) + x201)) + (x200 * x134))))))))) + (x200 * x156))); cnMatrixOptionalSet(Hx, 0, 0, x164 + (x165 * x164)); cnMatrixOptionalSet(Hx, 0, 1, x187 + (x165 * x187)); cnMatrixOptionalSet(Hx, 0, 2, x207 + (x207 * x165)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt); } } static inline FLT SurviveJointKalmanErrorModel_LightMeas_y_gen2(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x3 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2]; const FLT x4 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]); const FLT x6 = x5 * x5; const FLT x7 = (*_x0).Lighthouse.Rot[0] + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]); const FLT x8 = x7 * x7; const FLT x9 = x3 * x3; const FLT x10 = (*_x0).Lighthouse.Rot[1] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (x0 * (*_x0).Lighthouse.Rot[0]) + (x2 * (*_x0).Lighthouse.Rot[2]); const FLT x11 = x10 * x10; const FLT x12 = x11 + x9 + x6 + x8; const FLT x13 = 1. / x12; const FLT x14 = (1. / sqrt(x12)) * (1. / sqrt((x13 * x11) + (x8 * x13) + (x6 * x13) + (x9 * x13))); const FLT x15 = x3 * x14; const FLT x16 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x17 = x7 * x14; const FLT x18 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x19 = x5 * x14; const FLT x20 = x14 * ((-1 * x4 * x15) + (x19 * x18) + (x17 * x16)); const FLT x21 = x14 * x10; const FLT x22 = (x4 * x21) + (x18 * x17) + (-1 * x19 * x16); const FLT x23 = 1.0/2.0 * dt * fabs(dt); const FLT x24 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x26 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x27 = (-1 * x26 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x24 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x25 * (*_x0).Object.Pose.Rot[2]); const FLT x28 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x29 = dt * dt; const FLT x30 = (x28 * x28) * x29; const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x32 = (x31 * x31) * x29; const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x34 = (x33 * x33) * x29; const FLT x35 = 1e-10 + x34 + x30 + x32; const FLT x36 = sqrt(x35); const FLT x37 = 0.5 * x36; const FLT x38 = sin(x37); const FLT x39 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x40 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x41 = (-1 * x40 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x25 * (*_x0).Object.Pose.Rot[1]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x42 = (-1 * x26 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x25 * (*_x0).Object.Pose.Rot[0]) + (x24 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x43 = (*_x0).Object.Pose.Rot[1] + (x39 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (x40 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x44 = (1. / x35) * (x38 * x38); const FLT x45 = cos(x37); const FLT x46 = (1. / sqrt((x43 * x43) + (x27 * x27) + (x41 * x41) + (x42 * x42))) * (1. / sqrt((x44 * x32) + (x45 * x45) + (x44 * x30) + (x44 * x34))); const FLT x47 = dt * x46 * (1. / x36) * x38; const FLT x48 = x47 * x28; const FLT x49 = x43 * x47; const FLT x50 = x45 * x46; const FLT x51 = x47 * x31; const FLT x52 = (x51 * x42) + (x50 * x41) + (x48 * x27) + (-1 * x49 * x33); const FLT x53 = x47 * x33; const FLT x54 = (-1 * x49 * x31) + (-1 * x41 * x48) + (x50 * x27) + (-1 * x53 * x42); const FLT x55 = (x51 * x27) + (x50 * x43) + (-1 * x42 * x48) + (x53 * x41); const FLT x56 = (-1 * x55 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x52 * sensor_pt[0]); const FLT x57 = (-1 * x51 * x41) + (x53 * x27) + (x49 * x28) + (x50 * x42); const FLT x58 = (-1 * x57 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x55 * sensor_pt[1]); const FLT x59 = (*_x0).Object.Pose.Pos[0] + (2 * ((x58 * x57) + (-1 * x52 * x56))) + (x23 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])); const FLT x60 = (-1 * x52 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x57 * sensor_pt[2]); const FLT x61 = (*_x0).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (2 * ((x56 * x55) + (-1 * x60 * x57))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (x23 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x62 = (*_x0).Object.Pose.Pos[1] + (x23 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x60 * x52) + (-1 * x58 * x55))); const FLT x63 = (x62 * x19) + (x59 * x17) + (-1 * x61 * x15); const FLT x64 = (x61 * x21) + (x62 * x17) + (-1 * x59 * x19); const FLT x65 = x61 + (-1 * (x4 + (2 * ((-1 * x22 * x21) + (x3 * x20))))) + (2 * ((-1 * x64 * x21) + (x63 * x15))); const FLT x66 = (x15 * x16) + (x4 * x17) + (-1 * x21 * x18); const FLT x67 = (x59 * x15) + (x61 * x17) + (-1 * x62 * x21); const FLT x68 = x59 + (-1 * (x16 + (2 * ((-1 * x66 * x15) + (x22 * x19))))) + (2 * ((-1 * x67 * x15) + (x64 * x19))); const FLT x69 = atan2(-1 * x65, x68); const FLT x70 = (2 * ((-1 * x63 * x19) + (x67 * x21))) + x62 + (-1 * (x18 + (2 * ((-1 * x5 * x20) + (x66 * x21))))); const FLT x71 = (x68 * x68) + (x65 * x65); const FLT x72 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt); const FLT x73 = cos(x72); const FLT x74 = asin(x70 * (1. / x73) * (1. / sqrt(x71 + (x70 * x70)))); const FLT x75 = 0.0028679863 + (x74 * (-8.0108022e-06 + (-8.0108022e-06 * x74))); const FLT x76 = 5.3685255e-06 + (x75 * x74); const FLT x77 = 0.0076069798 + (x74 * x76); const FLT x78 = -1 * (1. / sqrt(x71)) * x70 * tan(x72); const FLT x79 = (*_x0).BSD1.curve + (((*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag) * sin((-1 * asin(x78)) + (*_x0).BSD1.ogeephase + (*error_model).BSD1.ogeephase + x69)) + (*error_model).BSD1.curve; const FLT x80 = (-1 * asin(x78 + (x79 * (x74 * x74) * x77 * (1. / (x73 + (x79 * sin(x72) * ((x74 * (x77 + (x74 * (x76 + (x74 * (x75 + (x74 * (-8.0108022e-06 + (-1.60216044e-05 * x74))))))))) + (x74 * x77)))))))) + x69; return -1.5707963267949 + x80 + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * sin(x80 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha)); } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x4 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x5 = 0.5 * (*_x0).Lighthouse.Rot[2]; const FLT x6 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[0]); const FLT x7 = x6 * x6; const FLT x8 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x9 = (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x8 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[1]); const FLT x10 = x9 * x9; const FLT x11 = x3 * x3; const FLT x12 = (*_x0).Lighthouse.Rot[1] + (x8 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x2 * (*_x0).Lighthouse.Rot[2]); const FLT x13 = x12 * x12; const FLT x14 = x13 + x11 + x7 + x10; const FLT x15 = 1. / x14; const FLT x16 = x7 * x15; const FLT x17 = x15 * x11; const FLT x18 = x15 * x13; const FLT x19 = x18 + (x15 * x10) + x16 + x17; const FLT x20 = 1. / sqrt(x19); const FLT x21 = 1. / sqrt(x14); const FLT x22 = x20 * x21; const FLT x23 = x4 * x22; const FLT x24 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x25 = x24 * x22; const FLT x26 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x27 = x22 * x26; const FLT x28 = (x6 * x27) + (-1 * x3 * x23) + (x9 * x25); const FLT x29 = x3 * x22; const FLT x30 = (x23 * x12) + (x9 * x27) + (-1 * x6 * x25); const FLT x31 = x22 * x12; const FLT x32 = dt * fabs(dt); const FLT x33 = 1.0/2.0 * x32; const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x35 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x36 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x37 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x38 = (-1 * x37 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x39 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x40 = (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x37 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x41 = (-1 * x37 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x35 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2]; const FLT x42 = (*_x0).Object.Pose.Rot[1] + (x39 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x36 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x43 = (x42 * x42) + (x40 * x40) + (x38 * x38) + (x41 * x41); const FLT x44 = 1. / sqrt(x43); const FLT x45 = dt * dt; const FLT x46 = x34 * x34; const FLT x47 = x45 * x46; const FLT x48 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x49 = x48 * x48; const FLT x50 = x45 * x49; const FLT x51 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x52 = x51 * x51; const FLT x53 = x52 * x45; const FLT x54 = 1e-10 + x47 + x53 + x50; const FLT x55 = sqrt(x54); const FLT x56 = 0.5 * x55; const FLT x57 = sin(x56); const FLT x58 = x57 * x57; const FLT x59 = 1. / x54; const FLT x60 = x58 * x59; const FLT x61 = cos(x56); const FLT x62 = (x60 * x50) + (x61 * x61) + (x60 * x47) + (x60 * x53); const FLT x63 = 1. / sqrt(x62); const FLT x64 = x57 * (1. / x55); const FLT x65 = x63 * x64; const FLT x66 = dt * x65; const FLT x67 = x66 * x44; const FLT x68 = x67 * x38; const FLT x69 = x67 * x42; const FLT x70 = x63 * x61; const FLT x71 = x70 * x44; const FLT x72 = x71 * x40; const FLT x73 = x41 * x44; const FLT x74 = x73 * x66; const FLT x75 = (x74 * x48) + x72 + (x68 * x34) + (-1 * x69 * x51); const FLT x76 = x40 * x44; const FLT x77 = x76 * x66; const FLT x78 = x71 * x38; const FLT x79 = x78 + (-1 * x69 * x48) + (-1 * x77 * x34) + (-1 * x74 * x51); const FLT x80 = x71 * x42; const FLT x81 = (x68 * x48) + x80 + (-1 * x74 * x34) + (x77 * x51); const FLT x82 = (-1 * x81 * sensor_pt[2]) + (x79 * sensor_pt[1]) + (x75 * sensor_pt[0]); const FLT x83 = x71 * x41; const FLT x84 = (-1 * x77 * x48) + (x69 * x34) + (x68 * x51) + x83; const FLT x85 = (-1 * x84 * sensor_pt[0]) + (x79 * sensor_pt[2]) + (x81 * sensor_pt[1]); const FLT x86 = (*_x0).Object.Pose.Pos[0] + (x33 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x84 * x85) + (-1 * x82 * x75))); const FLT x87 = x86 * x22; const FLT x88 = (-1 * x75 * sensor_pt[1]) + (x79 * sensor_pt[0]) + (x84 * sensor_pt[2]); const FLT x89 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x81 * x82) + (-1 * x88 * x84))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x33 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x90 = x89 * x22; const FLT x91 = (*_x0).Object.Pose.Pos[1] + (x33 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x88 * x75) + (-1 * x81 * x85))); const FLT x92 = x91 * x22; const FLT x93 = (x6 * x92) + (x9 * x87) + (-1 * x3 * x90); const FLT x94 = (x9 * x92) + (x90 * x12) + (-1 * x6 * x87); const FLT x95 = x89 + (-1 * (x4 + (2 * ((-1 * x30 * x31) + (x28 * x29))))) + (2 * ((-1 * x94 * x31) + (x93 * x29))); const FLT x96 = x6 * x22; const FLT x97 = (x9 * x23) + (x3 * x25) + (-1 * x27 * x12); const FLT x98 = (x3 * x87) + (x9 * x90) + (-1 * x92 * x12); const FLT x99 = x98 * x22; const FLT x100 = x86 + (-1 * (x24 + (2 * ((-1 * x97 * x29) + (x96 * x30))))) + (2 * ((-1 * x3 * x99) + (x96 * x94))); const FLT x101 = atan2(-1 * x95, x100); const FLT x102 = (2 * ((-1 * x93 * x96) + (x99 * x12))) + x91 + (-1 * (x26 + (2 * ((-1 * x96 * x28) + (x97 * x31))))); const FLT x103 = x100 * x100; const FLT x104 = x103 + (x95 * x95); const FLT x105 = 1. / sqrt(x104); const FLT x106 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt); const FLT x107 = tan(x106); const FLT x108 = x105 * x107; const FLT x109 = -1 * x102 * x108; const FLT x110 = asin(x109) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * x101); const FLT x111 = sin(x110); const FLT x112 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag; const FLT x113 = (*_x0).BSD1.curve + (-1 * x111 * x112) + (*error_model).BSD1.curve; const FLT x114 = x102 * x102; const FLT x115 = x104 + x114; const FLT x116 = 1. / sqrt(x115); const FLT x117 = cos(x106); const FLT x118 = 1. / x117; const FLT x119 = x118 * x116; const FLT x120 = asin(x102 * x119); const FLT x121 = 8.0108022e-06 * x120; const FLT x122 = -8.0108022e-06 + (-1 * x121); const FLT x123 = 0.0028679863 + (x120 * x122); const FLT x124 = 5.3685255e-06 + (x120 * x123); const FLT x125 = 0.0076069798 + (x120 * x124); const FLT x126 = x120 * x120; const FLT x127 = sin(x106); const FLT x128 = x120 * x125; const FLT x129 = -8.0108022e-06 + (-1.60216044e-05 * x120); const FLT x130 = x123 + (x120 * x129); const FLT x131 = x124 + (x120 * x130); const FLT x132 = x125 + (x120 * x131); const FLT x133 = (x120 * x132) + x128; const FLT x134 = x113 * x133; const FLT x135 = x127 * x134; const FLT x136 = x117 + x135; const FLT x137 = 1. / x136; const FLT x138 = x126 * x137; const FLT x139 = x125 * x138; const FLT x140 = x109 + (x113 * x139); const FLT x141 = 1. / sqrt(1 + (-1 * (x140 * x140))); const FLT x142 = 1. / x19; const FLT x143 = 2 * x9; const FLT x144 = x15 * x143; const FLT x145 = x6 * x142 * x144; const FLT x146 = 2 * x12; const FLT x147 = x15 * x146; const FLT x148 = x3 * x142; const FLT x149 = x148 * x147; const FLT x150 = -1 * x149; const FLT x151 = x150 + x145; const FLT x152 = 2 * x102; const FLT x153 = 2 * x142; const FLT x154 = x16 * x153; const FLT x155 = x17 * x153; const FLT x156 = -1 + x155; const FLT x157 = x156 + x154; const FLT x158 = 2 * x100; const FLT x159 = 2 * x6; const FLT x160 = x15 * x159; const FLT x161 = x12 * x142; const FLT x162 = x161 * x160; const FLT x163 = -1 * x162; const FLT x164 = x144 * x148; const FLT x165 = -1 * x164; const FLT x166 = x165 + x163; const FLT x167 = 2 * x95; const FLT x168 = (x167 * x166) + (x157 * x158); const FLT x169 = 1.0/2.0 * x102; const FLT x170 = x118 * (1. / (x115 * sqrt(x115))) * x169; const FLT x171 = (x119 * x151) + (-1 * x170 * (x168 + (x151 * x152))); const FLT x172 = 1. / (x117 * x117); const FLT x173 = 1. / sqrt(1 + (-1 * x114 * (1. / x115) * x172)); const FLT x174 = x121 * x173; const FLT x175 = x122 * x173; const FLT x176 = x171 * x175; const FLT x177 = x123 * x173; const FLT x178 = (x171 * x177) + (x120 * (x176 + (-1 * x171 * x174))); const FLT x179 = x124 * x173; const FLT x180 = (x171 * x179) + (x120 * x178); const FLT x181 = x113 * x138; const FLT x182 = 1. / x104; const FLT x183 = x107 * x107; const FLT x184 = 1. / sqrt(1 + (-1 * x114 * x183 * x182)); const FLT x185 = (1. / (x104 * sqrt(x104))) * x107 * x169; const FLT x186 = (-1 * x108 * x151) + (x168 * x185); const FLT x187 = 1. / x100; const FLT x188 = x95 * (1. / x103); const FLT x189 = x103 * x182; const FLT x190 = ((x188 * x157) + (-1 * x166 * x187)) * x189; const FLT x191 = (-1 * x190) + (x186 * x184); const FLT x192 = x112 * cos(x110); const FLT x193 = x127 * x133; const FLT x194 = x192 * x193; const FLT x195 = x173 * x131; const FLT x196 = x129 * x173; const FLT x197 = 2.40324066e-05 * x120; const FLT x198 = x173 * x197; const FLT x199 = x173 * x130; const FLT x200 = x125 * x173; const FLT x201 = x173 * x132; const FLT x202 = x113 * x127; const FLT x203 = x126 * x125 * (1. / (x136 * x136)); const FLT x204 = x203 * x113; const FLT x205 = x128 * x173 * x137; const FLT x206 = 2 * x113; const FLT x207 = x205 * x206; const FLT x208 = x192 * x139; const FLT x209 = x190 + (-1 * x141 * ((-1 * x208 * x191) + (x207 * x171) + (x181 * x180) + x186 + (-1 * x204 * ((x202 * ((x120 * x180) + (x201 * x171) + (x120 * ((x171 * x195) + x180 + (x120 * ((x120 * ((-1 * x171 * x198) + x176 + (x171 * x196))) + x178 + (x171 * x199))))) + (x200 * x171))) + (-1 * x191 * x194))))); const FLT x210 = (-1 * asin(x140)) + (*error_model).BSD1.gibpha + x101 + (*_x0).BSD1.gibpha; const FLT x211 = cos(x210) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag); const FLT x212 = x18 * x153; const FLT x213 = -1 + x212 + x154; const FLT x214 = -1 * x145; const FLT x215 = x214 + x150; const FLT x216 = x161 * x144; const FLT x217 = x160 * x148; const FLT x218 = -1 * x217; const FLT x219 = x218 + x216; const FLT x220 = (x219 * x167) + (x215 * x158); const FLT x221 = (x213 * x119) + (-1 * x170 * (x220 + (x213 * x152))); const FLT x222 = (-1 * x213 * x108) + (x220 * x185); const FLT x223 = ((x215 * x188) + (-1 * x219 * x187)) * x189; const FLT x224 = (-1 * x223) + (x222 * x184); const FLT x225 = x221 * x175; const FLT x226 = (x221 * x177) + (x120 * (x225 + (-1 * x221 * x174))); const FLT x227 = (x221 * x179) + (x226 * x120); const FLT x228 = x223 + (-1 * x141 * (x222 + (x227 * x181) + (-1 * x208 * x224) + (x207 * x221) + (-1 * x204 * ((x202 * ((x227 * x120) + (x200 * x221) + (x201 * x221) + (x120 * (x227 + (x221 * x195) + (x120 * (x226 + (x120 * ((-1 * x221 * x198) + x225 + (x221 * x196))) + (x221 * x199))))))) + (-1 * x224 * x194))))); const FLT x229 = x163 + x164; const FLT x230 = x156 + x212; const FLT x231 = (x230 * x167) + (x229 * x158); const FLT x232 = -1 * x216; const FLT x233 = x232 + x218; const FLT x234 = (-1 * x233 * x108) + (x231 * x185); const FLT x235 = ((x229 * x188) + (-1 * x230 * x187)) * x189; const FLT x236 = (-1 * x235) + (x234 * x184); const FLT x237 = (x233 * x119) + (-1 * x170 * (x231 + (x233 * x152))); const FLT x238 = x237 * x175; const FLT x239 = (x237 * x177) + (x120 * (x238 + (-1 * x237 * x174))); const FLT x240 = (x237 * x179) + (x239 * x120); const FLT x241 = x235 + (-1 * x141 * ((x237 * x207) + (-1 * x236 * x208) + (-1 * x204 * ((x202 * ((x237 * x201) + (x240 * x120) + (x237 * x200) + (x120 * (x240 + (x237 * x195) + (x120 * (x239 + (x120 * ((-1 * x237 * x198) + x238 + (x237 * x196))) + (x237 * x199))))))) + (-1 * x236 * x194))) + x234 + (x240 * x181))); const FLT x242 = 1.0 * x3; const FLT x243 = x242 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x244 = 1.0 * x12; const FLT x245 = x244 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x246 = 1.0 * x6; const FLT x247 = x246 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x248 = x143 + x247 + x243 + x245; const FLT x249 = 1. / (x14 * x14); const FLT x250 = x10 * x249; const FLT x251 = x7 * x249; const FLT x252 = x13 * x249; const FLT x253 = x11 * x249; const FLT x254 = x144 + (x15 * x243) + (-1 * x253 * x248) + (x15 * x245) + (-1 * x252 * x248) + (-1 * x251 * x248) + (-1 * x250 * x248) + (x15 * x247); const FLT x255 = x21 * (1. / (x19 * sqrt(x19))); const FLT x256 = x254 * x255; const FLT x257 = x97 * x12; const FLT x258 = 1.0 * x22; const FLT x259 = x97 * x258; const FLT x260 = x259 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x261 = x6 * x28; const FLT x262 = x20 * (1. / (x14 * sqrt(x14))); const FLT x263 = x262 * x248; const FLT x264 = x3 * x86; const FLT x265 = 1.0/2.0 * x263; const FLT x266 = 1.0/2.0 * x256; const FLT x267 = x86 * x266; const FLT x268 = 1.0/2.0 * x91; const FLT x269 = x268 * x263; const FLT x270 = x9 * x266; const FLT x271 = x91 * x266; const FLT x272 = 1.0/2.0 * x89; const FLT x273 = x9 * x272; const FLT x274 = x1 * x87; const FLT x275 = -1 * x8 * x92; const FLT x276 = x275 + x90; const FLT x277 = x274 + (-1 * x273 * x263) + (-1 * x3 * x267) + x276 + (-1 * x264 * x265) + (x12 * x269) + (-1 * x89 * x270) + (x12 * x271); const FLT x278 = x22 * x146; const FLT x279 = x98 * x12; const FLT x280 = x6 * x93; const FLT x281 = 1.0 * x99; const FLT x282 = x281 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x283 = x6 * x265; const FLT x284 = x2 * x27; const FLT x285 = x9 * x265; const FLT x286 = x4 * x3; const FLT x287 = x6 * x26; const FLT x288 = -1 * x1 * x23; const FLT x289 = (-1 * x24 * x270) + x288 + x284 + (x265 * x286) + (x266 * x286) + x25 + (-1 * x26 * x283) + (-1 * x24 * x285) + (-1 * x266 * x287); const FLT x290 = x22 * x159; const FLT x291 = x6 * x268; const FLT x292 = x2 * x92; const FLT x293 = x3 * x272; const FLT x294 = x89 * x266; const FLT x295 = -1 * x1 * x90; const FLT x296 = x87 + x295; const FLT x297 = x296 + (x3 * x294) + (x293 * x263) + (-1 * x86 * x270) + (-1 * x6 * x271) + x292 + (-1 * x291 * x263) + (-1 * x86 * x285); const FLT x298 = x3 * x24; const FLT x299 = x26 * x12; const FLT x300 = x4 * x9; const FLT x301 = 1.0/2.0 * x300; const FLT x302 = x1 * x25; const FLT x303 = -1 * x8 * x27; const FLT x304 = x303 + x23; const FLT x305 = (-1 * x298 * x265) + (x299 * x265) + (x299 * x266) + x302 + (-1 * x266 * x300) + x304 + (-1 * x298 * x266) + (-1 * x263 * x301); const FLT x306 = x93 * x258; const FLT x307 = x28 * x258; const FLT x308 = (x307 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x306 * (*error_model).Lighthouse.AxisAngleRot[2]); const FLT x309 = x308 + (-1 * x297 * x290) + (-1 * x279 * x263) + (-1 * x263 * x261) + (-1 * x260) + (x278 * x277) + (x256 * x257) + (-1 * x278 * x305) + (x263 * x257) + (-1 * x261 * x256) + (x290 * x289) + (x280 * x256) + x282 + (-1 * x279 * x256) + (x263 * x280); const FLT x310 = x8 * x90; const FLT x311 = -1 * x2 * x87; const FLT x312 = x12 * x272; const FLT x313 = x310 + (-1 * x263 * x312) + (-1 * x12 * x294) + (x6 * x267) + (-1 * x9 * x269) + x92 + (x86 * x283) + (-1 * x91 * x270) + x311; const FLT x314 = x94 * x258; const FLT x315 = x314 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x316 = x94 * x263; const FLT x317 = x6 * x30; const FLT x318 = x94 * x255; const FLT x319 = x254 * x318; const FLT x320 = x30 * x258; const FLT x321 = x320 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x322 = x4 * x12; const FLT x323 = x9 * x26; const FLT x324 = x6 * x24; const FLT x325 = x8 * x23; const FLT x326 = -1 * x2 * x25; const FLT x327 = x326 + x27; const FLT x328 = (-1 * x26 * x270) + x325 + (-1 * x266 * x322) + (-1 * x265 * x322) + x327 + (x24 * x283) + (-1 * x265 * x323) + (x266 * x324); const FLT x329 = x3 * x98; const FLT x330 = 2 * x3; const FLT x331 = x22 * x330; const FLT x332 = x3 * x97; const FLT x333 = x258 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x334 = (-1 * x281 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x97 * x333); const FLT x335 = x334 + (x256 * x329) + (-1 * x277 * x331) + (-1 * x256 * x332) + (-1 * x6 * x316) + (x305 * x331) + (-1 * x263 * x332) + (x290 * x313) + (x263 * x317) + (x256 * x317) + (-1 * x290 * x328) + x315 + (-1 * x321) + (-1 * x6 * x319) + (x263 * x329); const FLT x336 = x3 * x93; const FLT x337 = x30 * x12; const FLT x338 = x3 * x28; const FLT x339 = x93 * x333; const FLT x340 = x28 * x333; const FLT x341 = (x320 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x314 * (*error_model).Lighthouse.AxisAngleRot[0]); const FLT x342 = (-1 * x289 * x331) + x341 + (-1 * x256 * x336) + (-1 * x256 * x337) + (x263 * x338) + (-1 * x263 * x337) + (x12 * x316) + (x278 * x328) + (-1 * x263 * x336) + x339 + (x297 * x331) + (-1 * x340) + (x256 * x338) + (-1 * x278 * x313) + (x12 * x319); const FLT x343 = (x342 * x167) + (x335 * x158); const FLT x344 = (x309 * x119) + (-1 * x170 * (x343 + (x309 * x152))); const FLT x345 = (-1 * x309 * x108) + (x343 * x185); const FLT x346 = ((x335 * x188) + (-1 * x342 * x187)) * x189; const FLT x347 = (-1 * x346) + (x345 * x184); const FLT x348 = x344 * x175; const FLT x349 = (x344 * x177) + (x120 * (x348 + (-1 * x344 * x174))); const FLT x350 = (x344 * x179) + (x349 * x120); const FLT x351 = x346 + (-1 * x141 * (x345 + (-1 * x208 * x347) + (x207 * x344) + (x350 * x181) + (-1 * x204 * ((x202 * ((x350 * x120) + (x200 * x344) + (x201 * x344) + (x120 * (x350 + (x344 * x195) + (x120 * (x349 + (x120 * ((-1 * x344 * x198) + x348 + (x344 * x196))) + (x344 * x199))))))) + (-1 * x347 * x194))))); const FLT x352 = x246 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x353 = x242 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x354 = 1.0 * x9; const FLT x355 = x354 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x356 = (-1 * x353) + (-1 * x355) + x352 + x146; const FLT x357 = x262 * x356; const FLT x358 = 1.0/2.0 * x357; const FLT x359 = x12 * x357; const FLT x360 = 1.0/2.0 * x26; const FLT x361 = (-1 * x15 * x353) + x147 + (-1 * x252 * x356) + (-1 * x250 * x356) + (-1 * x15 * x355) + (-1 * x253 * x356) + (-1 * x251 * x356) + (x15 * x352); const FLT x362 = x255 * x361; const FLT x363 = 1.0/2.0 * x362; const FLT x364 = -1 * x325; const FLT x365 = x12 * x363; const FLT x366 = (-1 * x298 * x363) + (-1 * x358 * x300) + x326 + (-1 * x27) + (-1 * x298 * x358) + (x360 * x359) + (x26 * x365) + (-1 * x363 * x300) + x364; const FLT x367 = x6 * x94; const FLT x368 = x361 * x318; const FLT x369 = -1 * x302; const FLT x370 = 1.0/2.0 * x4; const FLT x371 = x9 * x358; const FLT x372 = x6 * x363; const FLT x373 = x9 * x363; const FLT x374 = (-1 * x26 * x373) + (-1 * x370 * x359) + (-1 * x4 * x365) + x369 + x304 + (x358 * x324) + (x24 * x372) + (-1 * x26 * x371); const FLT x375 = x6 * x86; const FLT x376 = x9 * x268; const FLT x377 = -1 * x274; const FLT x378 = x276 + (x86 * x372) + (-1 * x272 * x359) + (-1 * x89 * x365) + (-1 * x376 * x357) + x377 + (x375 * x358) + (-1 * x91 * x373); const FLT x379 = x86 * x363; const FLT x380 = (-1 * x310) + x311; const FLT x381 = x380 + (x91 * x365) + (-1 * x92) + (x268 * x359) + (-1 * x3 * x379) + (-1 * x264 * x358) + (-1 * x89 * x373) + (-1 * x89 * x371); const FLT x382 = (x94 * x333) + (-1 * x30 * x333); const FLT x383 = (x281 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x259 * (*error_model).Lighthouse.AxisAngleRot[2]); const FLT x384 = x382 + (-1 * x381 * x331) + (-1 * x362 * x332) + x383 + (-1 * x6 * x368) + (x362 * x329) + (x362 * x317) + (-1 * x367 * x357) + (x366 * x331) + (-1 * x357 * x332) + (x357 * x317) + (x290 * x378) + (x357 * x329) + (-1 * x290 * x374); const FLT x385 = 2 * x22; const FLT x386 = x94 * x385; const FLT x387 = x30 * x385; const FLT x388 = x1 * x27; const FLT x389 = x2 * x23; const FLT x390 = x8 * x25; const FLT x391 = (-1 * x24 * x373) + (-1 * x26 * x372) + (-1 * x287 * x358) + (-1 * x390) + (-1 * x24 * x371) + x389 + x388 + (x286 * x363) + (x286 * x358); const FLT x392 = x1 * x92; const FLT x393 = x3 * x89; const FLT x394 = x8 * x87; const FLT x395 = x2 * x90; const FLT x396 = (-1 * x91 * x372) + x395 + (x393 * x363) + (-1 * x9 * x379) + (x393 * x358) + (-1 * x394) + (-1 * x291 * x357) + x392 + (-1 * x86 * x371); const FLT x397 = x308 + (x396 * x331) + (-1 * x386) + (-1 * x278 * x378) + (-1 * x357 * x336) + (-1 * x357 * x337) + (-1 * x362 * x336) + x387 + (x278 * x374) + (x94 * x359) + (-1 * x362 * x337) + (-1 * x391 * x331) + (x362 * x338) + (x12 * x368) + (x357 * x338); const FLT x398 = (x397 * x167) + (x384 * x158); const FLT x399 = 2 * x99; const FLT x400 = x97 * x385; const FLT x401 = (-1 * x261 * x362) + (-1 * x339) + (x97 * x359) + (-1 * x400) + (-1 * x290 * x396) + (-1 * x261 * x357) + (x278 * x381) + (x280 * x362) + (-1 * x98 * x359) + (x290 * x391) + (-1 * x279 * x362) + (x280 * x357) + x340 + x399 + (x257 * x362) + (-1 * x278 * x366); const FLT x402 = (-1 * x401 * x108) + (x398 * x185); const FLT x403 = ((x384 * x188) + (-1 * x397 * x187)) * x189; const FLT x404 = (-1 * x403) + (x402 * x184); const FLT x405 = (x401 * x119) + (-1 * x170 * (x398 + (x401 * x152))); const FLT x406 = x405 * x175; const FLT x407 = (x405 * x177) + (x120 * (x406 + (-1 * x405 * x174))); const FLT x408 = (x405 * x179) + (x407 * x120); const FLT x409 = x403 + (-1 * x141 * ((-1 * x404 * x208) + (x405 * x207) + (-1 * x204 * ((x202 * ((x408 * x120) + (x405 * x200) + (x405 * x201) + (x120 * (x408 + (x405 * x195) + (x120 * (x407 + (x120 * ((-1 * x405 * x198) + x406 + (x405 * x196))) + (x405 * x199))))))) + (-1 * x404 * x194))) + x402 + (x408 * x181))); const FLT x410 = x244 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x411 = x246 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x412 = x354 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x413 = (-1 * x412) + (-1 * x411) + x330 + x410; const FLT x414 = (-1 * x15 * x412) + (-1 * x413 * x253) + (-1 * x413 * x251) + (-1 * x413 * x252) + (x15 * x410) + (x15 * x330) + (-1 * x15 * x411) + (-1 * x413 * x250); const FLT x415 = x414 * x255; const FLT x416 = 1.0/2.0 * x415; const FLT x417 = x413 * x262; const FLT x418 = x12 * x268; const FLT x419 = x9 * x417; const FLT x420 = 1.0/2.0 * x417; const FLT x421 = -1 * x292; const FLT x422 = x296 + x421 + (x417 * x418) + (-1 * x416 * x264) + (-1 * x420 * x264) + (x415 * x418) + (-1 * x415 * x273) + (-1 * x419 * x272); const FLT x423 = x86 * x416; const FLT x424 = 1.0/2.0 * x419; const FLT x425 = (-1 * x90) + (-1 * x417 * x291) + (-1 * x86 * x424) + (-1 * x9 * x423) + (-1 * x415 * x291) + (x417 * x293) + x275 + (x415 * x293) + x377; const FLT x426 = x24 * x416; const FLT x427 = x6 * x420; const FLT x428 = x369 + (x420 * x286) + (-1 * x24 * x424) + x303 + (x416 * x286) + (-1 * x26 * x427) + (-1 * x9 * x426) + (-1 * x416 * x287) + (-1 * x23); const FLT x429 = (-1 * x284) + x288; const FLT x430 = x429 + (x416 * x299) + (x420 * x299) + (-1 * x416 * x298) + (-1 * x415 * x301) + x25 + (-1 * x420 * x298) + (-1 * x417 * x301); const FLT x431 = x98 * x417; const FLT x432 = (-1 * x307 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x306 * (*error_model).Lighthouse.AxisAngleRot[0]); const FLT x433 = x432 + (x417 * x280) + (-1 * x415 * x261) + (x422 * x278) + (-1 * x417 * x261) + (-1 * x425 * x290) + (x415 * x257) + (x417 * x257) + (x415 * x280) + x383 + (-1 * x415 * x279) + (-1 * x430 * x278) + (x428 * x290) + (-1 * x12 * x431); const FLT x434 = x414 * x318; const FLT x435 = (x6 * x426) + x390 + (-1 * x26 * x424) + (-1 * x420 * x322) + (-1 * x416 * x322) + (-1 * x388) + x389 + (-1 * x416 * x323) + (x24 * x427); const FLT x436 = (-1 * x419 * x268) + (-1 * x392) + (-1 * x415 * x312) + x395 + (-1 * x417 * x312) + (x6 * x423) + (x420 * x375) + x394 + (-1 * x415 * x376); const FLT x437 = x341 + (x415 * x329) + (x3 * x431) + (x436 * x290) + (-1 * x417 * x332) + (x430 * x331) + (-1 * x6 * x434) + x400 + (-1 * x399) + (x417 * x317) + (-1 * x422 * x331) + (-1 * x417 * x367) + (-1 * x415 * x332) + (-1 * x435 * x290) + (x415 * x317); const FLT x438 = x93 * x385; const FLT x439 = x28 * x385; const FLT x440 = x94 * x12; const FLT x441 = (x417 * x440) + (x417 * x338) + (-1 * x436 * x278) + (-1 * x428 * x331) + (x425 * x331) + (x12 * x434) + (-1 * x417 * x336) + (-1 * x417 * x337) + (-1 * x439) + (-1 * x415 * x336) + x321 + (-1 * x315) + x438 + (x415 * x338) + (x435 * x278) + (-1 * x415 * x337); const FLT x442 = (x441 * x167) + (x437 * x158); const FLT x443 = (x433 * x119) + (-1 * x170 * (x442 + (x433 * x152))); const FLT x444 = x443 * x175; const FLT x445 = (x443 * x177) + (x120 * (x444 + (-1 * x443 * x174))); const FLT x446 = (x443 * x179) + (x445 * x120); const FLT x447 = (-1 * x433 * x108) + (x442 * x185); const FLT x448 = ((x437 * x188) + (-1 * x441 * x187)) * x189; const FLT x449 = (-1 * x448) + (x447 * x184); const FLT x450 = x448 + (-1 * x141 * (x447 + (-1 * x204 * ((x202 * ((x446 * x120) + (x443 * x200) + (x443 * x201) + (x120 * (x446 + (x443 * x195) + (x120 * (x445 + (x120 * ((-1 * x443 * x198) + x444 + (x443 * x196))) + (x443 * x199))))))) + (-1 * x449 * x194))) + (x443 * x207) + (-1 * x449 * x208) + (x446 * x181))); const FLT x451 = x244 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x452 = x242 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x453 = x354 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x454 = (-1 * x453) + x159 + (-1 * x451) + x452; const FLT x455 = x454 * x262; const FLT x456 = x255 * ((-1 * x15 * x453) + (-1 * x454 * x251) + (-1 * x454 * x252) + (-1 * x454 * x250) + (-1 * x15 * x451) + x160 + (x15 * x452) + (-1 * x454 * x253)); const FLT x457 = x12 * x456; const FLT x458 = 1.0/2.0 * x455; const FLT x459 = x9 * x456; const FLT x460 = x6 * x456; const FLT x461 = 1.0/2.0 * x86; const FLT x462 = (x461 * x460) + x295 + (-1 * x457 * x272) + (-1 * x455 * x312) + (-1 * x455 * x376) + (-1 * x87) + (x458 * x375) + (-1 * x459 * x268) + x421; const FLT x463 = x6 * x455; const FLT x464 = 1.0/2.0 * x24; const FLT x465 = (-1 * x25) + (x464 * x460) + (-1 * x458 * x322) + (-1 * x459 * x360) + (-1 * x458 * x323) + (-1 * x457 * x370) + x429 + (x464 * x463); const FLT x466 = x3 * x456; const FLT x467 = x388 + (-1 * x389) + (x457 * x360) + (-1 * x456 * x301) + (-1 * x464 * x466) + (x458 * x299) + (-1 * x455 * x301) + x390 + (-1 * x458 * x298); const FLT x468 = x98 * x455; const FLT x469 = (-1 * x395) + (-1 * x461 * x466) + (x457 * x268) + x392 + (x455 * x418) + x394 + (-1 * x459 * x272) + (-1 * x458 * x264) + (-1 * x455 * x273); const FLT x470 = (-1 * x469 * x331) + (x98 * x466) + (x30 * x460) + (-1 * x387) + x386 + (x462 * x290) + (x3 * x468) + (x455 * x317) + (-1 * x282) + (x467 * x331) + x260 + (-1 * x94 * x460) + (-1 * x455 * x367) + (-1 * x465 * x290) + (-1 * x97 * x466) + (-1 * x455 * x332); const FLT x471 = (x455 * x293) + (-1 * x460 * x268) + (-1 * x455 * x291) + x380 + (-1 * x459 * x461) + (x466 * x272) + x92 + (-1 * x9 * x86 * x458); const FLT x472 = (-1 * x459 * x464) + x327 + (x458 * x286) + (-1 * x9 * x455 * x464) + (-1 * x460 * x360) + (x466 * x370) + (-1 * x463 * x360) + x364; const FLT x473 = x432 + (x94 * x457) + (x465 * x278) + (x471 * x331) + (-1 * x462 * x278) + (-1 * x455 * x337) + (-1 * x455 * x336) + (x455 * x338) + (-1 * x472 * x331) + x382 + (-1 * x93 * x466) + (x455 * x440) + (-1 * x30 * x457) + (x456 * x338); const FLT x474 = (x473 * x167) + (x470 * x158); const FLT x475 = x334 + x439 + (-1 * x438) + (-1 * x12 * x468) + (x97 * x457) + (-1 * x28 * x460) + (-1 * x471 * x290) + (-1 * x98 * x457) + (x469 * x278) + (-1 * x467 * x278) + (x455 * x257) + (-1 * x455 * x261) + (x472 * x290) + (x455 * x280) + (x93 * x460); const FLT x476 = (-1 * x475 * x108) + (x474 * x185); const FLT x477 = ((x470 * x188) + (-1 * x473 * x187)) * x189; const FLT x478 = x192 * ((-1 * x477) + (x476 * x184)); const FLT x479 = (x475 * x119) + (-1 * x170 * (x474 + (x475 * x152))); const FLT x480 = x479 * x175; const FLT x481 = (x479 * x177) + (x120 * (x480 + (-1 * x479 * x174))); const FLT x482 = (x479 * x179) + (x481 * x120); const FLT x483 = x477 + (-1 * x141 * ((x482 * x181) + x476 + (-1 * x478 * x139) + (-1 * x204 * ((x202 * ((x482 * x120) + (x479 * x200) + (x479 * x201) + (x120 * (x482 + (x479 * x195) + (x120 * (x481 + (x120 * ((-1 * x479 * x198) + (x479 * x196) + x480)) + (x479 * x199))))))) + (-1 * x478 * x193))) + (x479 * x207))); const FLT x484 = x32 * x142; const FLT x485 = x15 * x484; const FLT x486 = x3 * x12 * x485; const FLT x487 = x6 * x485; const FLT x488 = x9 * x487; const FLT x489 = (-1 * x488) + x486; const FLT x490 = -1 * x17 * x484; const FLT x491 = -1 * x16 * x484; const FLT x492 = x491 + x33 + x490; const FLT x493 = x9 * x485; const FLT x494 = x3 * x493; const FLT x495 = x12 * x487; const FLT x496 = x495 + x494; const FLT x497 = (x496 * x167) + (x492 * x158); const FLT x498 = (x489 * x119) + (-1 * x170 * (x497 + (x489 * x152))); const FLT x499 = (-1 * x489 * x108) + (x497 * x185); const FLT x500 = ((x492 * x188) + (-1 * x496 * x187)) * x189; const FLT x501 = (-1 * x500) + (x499 * x184); const FLT x502 = x498 * x175; const FLT x503 = (x498 * x177) + (x120 * (x502 + (-1 * x498 * x174))); const FLT x504 = (x498 * x179) + (x503 * x120); const FLT x505 = x500 + (-1 * x141 * ((-1 * x501 * x208) + (x504 * x181) + x499 + (x498 * x207) + (-1 * x204 * ((x202 * ((x504 * x120) + (x498 * x201) + (x120 * (x504 + (x498 * x195) + (x120 * (x503 + (x120 * ((-1 * x498 * x198) + x502 + (x498 * x196))) + (x498 * x199))))) + (x498 * x200))) + (-1 * x501 * x194))))); const FLT x506 = (-1 * x18 * x484) + x33; const FLT x507 = x506 + x491; const FLT x508 = x486 + x488; const FLT x509 = x3 * x487; const FLT x510 = x12 * x493; const FLT x511 = (-1 * x510) + x509; const FLT x512 = (x511 * x167) + (x508 * x158); const FLT x513 = (x507 * x119) + (-1 * x170 * (x512 + (x507 * x152))); const FLT x514 = x513 * x175; const FLT x515 = (x513 * x177) + (x120 * (x514 + (-1 * x513 * x174))); const FLT x516 = (x513 * x179) + (x515 * x120); const FLT x517 = (-1 * x507 * x108) + (x512 * x185); const FLT x518 = ((x508 * x188) + (-1 * x511 * x187)) * x189; const FLT x519 = (-1 * x518) + (x517 * x184); const FLT x520 = x518 + (-1 * x141 * ((-1 * x519 * x208) + (-1 * x204 * ((x202 * ((x516 * x120) + (x513 * x200) + (x120 * (x516 + (x513 * x195) + (x120 * (x515 + (x120 * ((-1 * x513 * x198) + x514 + (x513 * x196))) + (x513 * x199))))) + (x513 * x201))) + (-1 * x519 * x194))) + x517 + (x516 * x181) + (x513 * x207))); const FLT x521 = x509 + x510; const FLT x522 = (-1 * x494) + x495; const FLT x523 = x506 + x490; const FLT x524 = (x523 * x167) + (x522 * x158); const FLT x525 = (x521 * x119) + (-1 * x170 * (x524 + (x521 * x152))); const FLT x526 = x525 * x175; const FLT x527 = (x525 * x177) + (x120 * (x526 + (-1 * x525 * x174))); const FLT x528 = (x525 * x179) + (x527 * x120); const FLT x529 = (-1 * x521 * x108) + (x524 * x185); const FLT x530 = ((x522 * x188) + (-1 * x523 * x187)) * x189; const FLT x531 = (-1 * x530) + (x529 * x184); const FLT x532 = x530 + (-1 * x141 * (x529 + (-1 * x531 * x208) + (-1 * x204 * ((x202 * ((x120 * (x528 + (x525 * x195) + (x120 * (x527 + (x120 * ((-1 * x525 * x198) + x526 + (x525 * x196))) + (x525 * x199))))) + (x528 * x120) + (x525 * x201) + (x525 * x200))) + (-1 * x531 * x194))) + (x528 * x181) + (x525 * x207))); const FLT x533 = x214 + x149; const FLT x534 = -1 * x154; const FLT x535 = -1 * x155; const FLT x536 = 1 + x535 + x534; const FLT x537 = x162 + x164; const FLT x538 = (x537 * x167) + (x536 * x158); const FLT x539 = (x533 * x119) + (-1 * x170 * (x538 + (x533 * x152))); const FLT x540 = x539 * x175; const FLT x541 = (x539 * x177) + (x120 * (x540 + (-1 * x539 * x174))); const FLT x542 = (x539 * x179) + (x541 * x120); const FLT x543 = (-1 * x533 * x108) + (x538 * x185); const FLT x544 = ((x536 * x188) + (-1 * x537 * x187)) * x189; const FLT x545 = (-1 * x544) + (x543 * x184); const FLT x546 = x544 + (-1 * x141 * (x543 + (x539 * x207) + (x542 * x181) + (-1 * x545 * x208) + (-1 * x204 * ((x202 * ((x542 * x120) + (x539 * x200) + (x539 * x201) + (x120 * (x542 + (x539 * x195) + (x120 * (x541 + (x120 * ((-1 * x539 * x198) + x540 + (x539 * x196))) + (x539 * x199))))))) + (-1 * x545 * x194))))); const FLT x547 = x149 + x145; const FLT x548 = x232 + x217; const FLT x549 = (x548 * x167) + (x547 * x158); const FLT x550 = 1 + (-1 * x212); const FLT x551 = x550 + x534; const FLT x552 = (-1 * x551 * x108) + (x549 * x185); const FLT x553 = ((x547 * x188) + (-1 * x548 * x187)) * x189; const FLT x554 = (-1 * x553) + (x552 * x184); const FLT x555 = (x551 * x119) + (-1 * x170 * (x549 + (x551 * x152))); const FLT x556 = x555 * x175; const FLT x557 = (x555 * x177) + (x120 * (x556 + (-1 * x555 * x174))); const FLT x558 = (x555 * x179) + (x557 * x120); const FLT x559 = x553 + (-1 * x141 * (x552 + (-1 * x554 * x208) + (-1 * x204 * ((x202 * ((x555 * x200) + (x558 * x120) + (x555 * x201) + (x120 * (x558 + (x555 * x195) + (x120 * (x557 + (x120 * ((-1 * x555 * x198) + x556 + (x555 * x196))) + (x555 * x199))))))) + (-1 * x554 * x194))) + (x555 * x207) + (x558 * x181))); const FLT x560 = x217 + x216; const FLT x561 = x165 + x162; const FLT x562 = x550 + x535; const FLT x563 = (x562 * x167) + (x561 * x158); const FLT x564 = (x560 * x119) + (-1 * x170 * (x563 + (x560 * x152))); const FLT x565 = x564 * x175; const FLT x566 = (x564 * x177) + (x120 * (x565 + (-1 * x564 * x174))); const FLT x567 = (x564 * x179) + (x566 * x120); const FLT x568 = (-1 * x560 * x108) + (x563 * x185); const FLT x569 = ((x561 * x188) + (-1 * x562 * x187)) * x189; const FLT x570 = (-1 * x569) + (x568 * x184); const FLT x571 = x569 + (-1 * x141 * (x568 + (-1 * x570 * x208) + (-1 * x204 * ((x202 * ((x567 * x120) + (x564 * x200) + (x564 * x201) + (x120 * (x567 + (x564 * x195) + (x120 * (x566 + (x120 * ((-1 * x564 * x198) + x565 + (x564 * x196))) + (x564 * x199))))))) + (-1 * x570 * x194))) + (x567 * x181) + (x564 * x207))); const FLT x572 = 1.0/2.0 * x41; const FLT x573 = 1.0 * x42; const FLT x574 = 1.0 * x40; const FLT x575 = 1.0 * x41; const FLT x576 = (x575 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x574 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x573 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x38); const FLT x577 = 1. / (x43 * sqrt(x43)); const FLT x578 = x66 * x577; const FLT x579 = x576 * x578; const FLT x580 = x48 * x579; const FLT x581 = 1.0/2.0 * x40; const FLT x582 = x70 * x577; const FLT x583 = x576 * x582; const FLT x584 = 1.0/2.0 * x38; const FLT x585 = x34 * x584; const FLT x586 = x67 * x48; const FLT x587 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x588 = x586 * x587; const FLT x589 = 1.0/2.0 * x42; const FLT x590 = x51 * x579; const FLT x591 = 0.5 * x71; const FLT x592 = x591 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x593 = x67 * x34; const FLT x594 = x67 * x51; const FLT x595 = 0.5 * x594; const FLT x596 = -1 * x595 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x597 = x596 + x593; const FLT x598 = x597 + (-1 * x581 * x583) + (x590 * x589) + (-1 * x572 * x580) + x592 + (-1 * x579 * x585) + x588; const FLT x599 = 2 * x82; const FLT x600 = x34 * x579; const FLT x601 = 1.0/2.0 * x583; const FLT x602 = x591 * (*error_model).Object.Pose.AxisAngleRot[1]; const FLT x603 = 0.5 * x586; const FLT x604 = -1 * x603 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x605 = 0.5 * x593; const FLT x606 = x605 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x607 = x606 + x604; const FLT x608 = x607 + x602 + (-1 * x600 * x589) + (x580 * x581) + x594 + (-1 * x590 * x584) + (-1 * x41 * x601); const FLT x609 = x594 * x587; const FLT x610 = -1 * x609; const FLT x611 = x605 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x612 = -1 * x611; const FLT x613 = x603 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x614 = (-1 * x613) + x71; const FLT x615 = x614 + (x590 * x572) + x612 + x610 + (x600 * x581) + (-1 * x38 * x601) + (x580 * x589); const FLT x616 = x595 * (*error_model).Object.Pose.AxisAngleRot[2]; const FLT x617 = -1 * x593 * x587; const FLT x618 = x591 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x619 = x618 + x617; const FLT x620 = x616 + (-1 * x590 * x581) + (-1 * x580 * x584) + x619 + x586 + (x600 * x572) + (-1 * x589 * x583); const FLT x621 = (x620 * sensor_pt[1]) + (-1 * x608 * sensor_pt[0]) + (x615 * sensor_pt[2]); const FLT x622 = 2 * x84; const FLT x623 = (x598 * sensor_pt[0]) + (-1 * x620 * sensor_pt[2]) + (x615 * sensor_pt[1]); const FLT x624 = 2 * x75; const FLT x625 = 2 * x85; const FLT x626 = (x625 * x608) + (-1 * x624 * x623) + (-1 * x599 * x598) + (x622 * x621); const FLT x627 = 2 * x88; const FLT x628 = (x608 * sensor_pt[2]) + (x615 * sensor_pt[0]) + (-1 * x598 * sensor_pt[1]); const FLT x629 = 2 * x81; const FLT x630 = (x623 * x629) + (x620 * x599) + (-1 * x627 * x608) + (-1 * x622 * x628); const FLT x631 = x9 * x22; const FLT x632 = (x627 * x598) + (x624 * x628) + (-1 * x625 * x620) + (-1 * x621 * x629); const FLT x633 = x22 * x632; const FLT x634 = (-1 * x12 * x633) + (x29 * x626) + (x630 * x631); const FLT x635 = (x31 * x630) + (-1 * x96 * x626) + (x9 * x633); const FLT x636 = x626 + (-1 * x634 * x331) + (x635 * x290); const FLT x637 = (-1 * x29 * x630) + (x626 * x631) + (x96 * x632); const FLT x638 = x630 + (x637 * x331) + (-1 * x635 * x278); const FLT x639 = (x638 * x167) + (x636 * x158); const FLT x640 = (x634 * x278) + x632 + (-1 * x637 * x290); const FLT x641 = (-1 * x640 * x108) + (x639 * x185); const FLT x642 = ((x636 * x188) + (-1 * x638 * x187)) * x189; const FLT x643 = (-1 * x642) + (x641 * x184); const FLT x644 = (x640 * x119) + (-1 * x170 * (x639 + (x640 * x152))); const FLT x645 = x644 * x175; const FLT x646 = (x644 * x177) + (x120 * (x645 + (-1 * x644 * x174))); const FLT x647 = (x644 * x179) + (x646 * x120); const FLT x648 = x642 + (-1 * x141 * (x641 + (-1 * x643 * x208) + (x647 * x181) + (-1 * x204 * ((x202 * ((x647 * x120) + (x644 * x200) + (x644 * x201) + (x120 * (x647 + (x644 * x195) + (x120 * (x646 + (x120 * ((-1 * x644 * x198) + x645 + (x644 * x196))) + (x644 * x199))))))) + (-1 * x643 * x194))) + (x644 * x207))); const FLT x649 = 1.0 * x38; const FLT x650 = (2 * x42) + (-1 * x575 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x574 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x651 = x650 * x578; const FLT x652 = x48 * x651; const FLT x653 = x51 * x651; const FLT x654 = x650 * x582; const FLT x655 = x34 * x651; const FLT x656 = x614 + (x655 * x572) + x609 + (-1 * x652 * x584) + x611 + (-1 * x653 * x581) + (-1 * x654 * x589); const FLT x657 = -1 * x588; const FLT x658 = -1 * x592; const FLT x659 = (-1 * x654 * x572) + (-1 * x653 * x584) + (x652 * x581) + x657 + (-1 * x655 * x589) + x597 + x658; const FLT x660 = -1 * x586; const FLT x661 = (-1 * x618) + x617; const FLT x662 = (x653 * x572) + (-1 * x654 * x584) + (x652 * x589) + x616 + x660 + x661 + (x655 * x581); const FLT x663 = (-1 * x659 * sensor_pt[0]) + (x656 * sensor_pt[1]) + (x662 * sensor_pt[2]); const FLT x664 = -1 * x594; const FLT x665 = (-1 * x606) + x604; const FLT x666 = (-1 * x654 * x581) + x602 + x664 + (-1 * x651 * x585) + (-1 * x652 * x572) + x665 + (x653 * x589); const FLT x667 = (x659 * sensor_pt[2]) + (-1 * x666 * sensor_pt[1]) + (x662 * sensor_pt[0]); const FLT x668 = (x627 * x666) + (-1 * x625 * x656) + (x624 * x667) + (-1 * x629 * x663); const FLT x669 = (x662 * sensor_pt[1]) + (x666 * sensor_pt[0]) + (-1 * x656 * sensor_pt[2]); const FLT x670 = (-1 * x624 * x669) + (-1 * x666 * x599) + (x625 * x659) + (x622 * x663); const FLT x671 = (x629 * x669) + (x656 * x599) + (-1 * x627 * x659) + (-1 * x622 * x667); const FLT x672 = (-1 * x29 * x671) + (x96 * x668) + (x631 * x670); const FLT x673 = (-1 * x31 * x668) + (x29 * x670) + (x631 * x671); const FLT x674 = x668 + (-1 * x672 * x290) + (x673 * x278); const FLT x675 = (-1 * x96 * x670) + (x31 * x671) + (x631 * x668); const FLT x676 = x670 + (x675 * x290) + (-1 * x673 * x331); const FLT x677 = x671 + (x672 * x331) + (-1 * x675 * x278); const FLT x678 = (x677 * x167) + (x676 * x158); const FLT x679 = (x674 * x119) + (-1 * x170 * (x678 + (x674 * x152))); const FLT x680 = x679 * x173; const FLT x681 = x679 * x175; const FLT x682 = (x679 * x177) + (x120 * (x681 + (-1 * x680 * x121))); const FLT x683 = (x679 * x179) + (x682 * x120); const FLT x684 = (-1 * x674 * x108) + (x678 * x185); const FLT x685 = ((x676 * x188) + (-1 * x677 * x187)) * x189; const FLT x686 = (-1 * x685) + (x684 * x184); const FLT x687 = x685 + (-1 * x141 * (x684 + (-1 * x686 * x208) + (-1 * x204 * ((x202 * ((x683 * x120) + (x120 * (x683 + (x679 * x195) + (x120 * (x682 + (x120 * ((-1 * x680 * x197) + x681 + (x679 * x196))) + (x679 * x199))))) + (x679 * x201) + (x679 * x200))) + (-1 * x686 * x194))) + (x683 * x181) + (x680 * x206 * x128 * x137))); const FLT x688 = (2 * x41) + (x573 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x574 * (*error_model).Object.Pose.AxisAngleRot[0]); const FLT x689 = x688 * x578; const FLT x690 = x48 * x689; const FLT x691 = x34 * x689; const FLT x692 = x51 * x689; const FLT x693 = x688 * x582; const FLT x694 = 1.0/2.0 * x693; const FLT x695 = x71 + x613; const FLT x696 = x695 + (-1 * x691 * x589) + (x690 * x581) + (-1 * x692 * x584) + x610 + x611 + (-1 * x41 * x694); const FLT x697 = (-1 * x593) + x596; const FLT x698 = x697 + (-1 * x692 * x581) + x592 + (-1 * x690 * x584) + x657 + (-1 * x693 * x589) + (x691 * x572); const FLT x699 = -1 * x616; const FLT x700 = x661 + (-1 * x689 * x585) + (-1 * x693 * x581) + (-1 * x690 * x572) + x586 + x699 + (x692 * x589); const FLT x701 = -1 * x602; const FLT x702 = x607 + x701 + (x691 * x581) + (-1 * x38 * x694) + x664 + (x690 * x589) + (x692 * x572); const FLT x703 = (x696 * sensor_pt[2]) + (-1 * x700 * sensor_pt[1]) + (x702 * sensor_pt[0]); const FLT x704 = (x700 * sensor_pt[0]) + (-1 * x698 * sensor_pt[2]) + (x702 * sensor_pt[1]); const FLT x705 = (x629 * x704) + (-1 * x622 * x703) + (-1 * x696 * x627) + (x698 * x599); const FLT x706 = (x698 * sensor_pt[1]) + (-1 * x696 * sensor_pt[0]) + (x702 * sensor_pt[2]); const FLT x707 = (x627 * x700) + (-1 * x629 * x706) + (-1 * x698 * x625) + (x624 * x703); const FLT x708 = x22 * x707; const FLT x709 = (-1 * x624 * x704) + (x696 * x625) + (-1 * x700 * x599) + (x622 * x706); const FLT x710 = (-1 * x96 * x709) + (x31 * x705) + (x9 * x708); const FLT x711 = x22 * ((-1 * x12 * x708) + (x29 * x709) + (x631 * x705)); const FLT x712 = x709 + (x710 * x290) + (-1 * x711 * x330); const FLT x713 = (x6 * x708) + (-1 * x29 * x705) + (x631 * x709); const FLT x714 = (x713 * x331) + x705 + (-1 * x710 * x278); const FLT x715 = (x714 * x167) + (x712 * x158); const FLT x716 = x707 + (x711 * x146) + (-1 * x713 * x290); const FLT x717 = (-1 * x716 * x108) + (x715 * x185); const FLT x718 = ((x712 * x188) + (-1 * x714 * x187)) * x189; const FLT x719 = (-1 * x718) + (x717 * x184); const FLT x720 = (x716 * x119) + (-1 * x170 * (x715 + (x716 * x152))); const FLT x721 = x720 * x175; const FLT x722 = (x720 * x177) + (x120 * (x721 + (-1 * x720 * x174))); const FLT x723 = (x720 * x179) + (x722 * x120); const FLT x724 = x718 + (-1 * x141 * ((-1 * x719 * x208) + (x723 * x181) + (-1 * x204 * ((x202 * ((x720 * x200) + (x723 * x120) + (x720 * x201) + (x120 * (x723 + (x720 * x195) + (x120 * (x722 + (x120 * ((-1 * x720 * x198) + x721 + (x720 * x196))) + (x720 * x199))))))) + (-1 * x719 * x194))) + x717 + (x720 * x207))); const FLT x725 = (x575 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x40) + (-1 * x573 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x726 = x725 * x578; const FLT x727 = x48 * x726; const FLT x728 = x34 * x726; const FLT x729 = x725 * x582; const FLT x730 = x51 * x726; const FLT x731 = (-1 * x730 * x581) + x594 + x701 + (-1 * x727 * x584) + x665 + (-1 * x729 * x589) + (x728 * x572); const FLT x732 = 1.0/2.0 * x729; const FLT x733 = x697 + (x730 * x572) + x658 + (x727 * x589) + x588 + (-1 * x38 * x732) + (x728 * x581); const FLT x734 = (-1 * x41 * x732) + (-1 * x730 * x584) + x699 + x660 + (-1 * x728 * x589) + x619 + (x727 * x581); const FLT x735 = (x731 * sensor_pt[1]) + (x733 * sensor_pt[2]) + (-1 * x734 * sensor_pt[0]); const FLT x736 = x695 + x609 + (-1 * x726 * x585) + (-1 * x40 * x732) + (-1 * x727 * x572) + (x730 * x589) + x612; const FLT x737 = (x734 * sensor_pt[2]) + (-1 * x736 * sensor_pt[1]) + (x733 * sensor_pt[0]); const FLT x738 = (x627 * x736) + (x624 * x737) + (-1 * x625 * x731) + (-1 * x629 * x735); const FLT x739 = (x736 * sensor_pt[0]) + (-1 * x731 * sensor_pt[2]) + (x733 * sensor_pt[1]); const FLT x740 = (x625 * x734) + (-1 * x624 * x739) + (-1 * x736 * x599) + (x622 * x735); const FLT x741 = (x629 * x739) + (x731 * x599) + (-1 * x627 * x734) + (-1 * x622 * x737); const FLT x742 = (-1 * x29 * x741) + (x96 * x738) + (x631 * x740); const FLT x743 = (-1 * x31 * x738) + (x29 * x740) + (x631 * x741); const FLT x744 = x738 + (-1 * x742 * x290) + (x743 * x278); const FLT x745 = (-1 * x96 * x740) + (x31 * x741) + (x631 * x738); const FLT x746 = x740 + (x745 * x290) + (-1 * x743 * x331); const FLT x747 = x741 + (x742 * x331) + (-1 * x745 * x278); const FLT x748 = (x747 * x167) + (x746 * x158); const FLT x749 = (x744 * x119) + (-1 * x170 * (x748 + (x744 * x152))); const FLT x750 = x749 * x175; const FLT x751 = (x749 * x177) + (x120 * (x750 + (-1 * x749 * x174))); const FLT x752 = (x749 * x179) + (x751 * x120); const FLT x753 = (-1 * x744 * x108) + (x748 * x185); const FLT x754 = ((x746 * x188) + (-1 * x747 * x187)) * x189; const FLT x755 = (-1 * x754) + (x753 * x184); const FLT x756 = x754 + (-1 * x141 * (x753 + (x749 * x207) + (-1 * x755 * x208) + (-1 * x204 * ((x202 * ((x752 * x120) + (x749 * x200) + (x749 * x201) + (x120 * (x752 + (x749 * x195) + (x120 * (x751 + (x120 * (x750 + (-1 * x749 * x198) + (x749 * x196))) + (x749 * x199))))))) + (-1 * x755 * x194))) + (x752 * x181))); const FLT x757 = dt * dt * dt; const FLT x758 = x49 * x757; const FLT x759 = 0.5 * x59; const FLT x760 = x758 * x759; const FLT x761 = x44 * x589; const FLT x762 = dt * dt * dt * dt; const FLT x763 = (x48 * x48 * x48) * x762; const FLT x764 = 2 * (1. / (x54 * x54)) * x58; const FLT x765 = 1.0 * x61; const FLT x766 = (1. / (x54 * sqrt(x54))) * x57; const FLT x767 = x766 * x765; const FLT x768 = x767 * x762; const FLT x769 = x48 * x768; const FLT x770 = x764 * x762; const FLT x771 = x48 * x770; const FLT x772 = 2 * x60; const FLT x773 = x45 * x772; const FLT x774 = x64 * x765; const FLT x775 = x45 * x48; const FLT x776 = (x767 * x763) + (x52 * x769) + (-1 * x774 * x775) + (-1 * x764 * x763) + (x46 * x769) + (-1 * x52 * x771) + (x48 * x773) + (-1 * x46 * x771); const FLT x777 = 1. / (x62 * sqrt(x62)); const FLT x778 = dt * x64 * x777; const FLT x779 = x778 * x776; const FLT x780 = x51 * x779; const FLT x781 = x44 * x38; const FLT x782 = 1.0/2.0 * x781; const FLT x783 = x34 * x779; const FLT x784 = 1.0/2.0 * x73; const FLT x785 = x48 * x779; const FLT x786 = 0.5 * x65; const FLT x787 = x775 * x786; const FLT x788 = x61 * x777; const FLT x789 = x776 * x788; const FLT x790 = 1.0/2.0 * x789; const FLT x791 = x766 * x758; const FLT x792 = x63 * x791; const FLT x793 = x766 * x757; const FLT x794 = x48 * x793; const FLT x795 = x51 * x794; const FLT x796 = x42 * x44; const FLT x797 = x63 * x796; const FLT x798 = x759 * x757; const FLT x799 = x51 * x798; const FLT x800 = x48 * x799; const FLT x801 = (-1 * x80 * x800) + (x795 * x797); const FLT x802 = x34 * x798; const FLT x803 = x48 * x802; const FLT x804 = x63 * x781; const FLT x805 = (-1 * x34 * x794 * x804) + (x78 * x803); const FLT x806 = x801 + (-1 * x76 * x790) + (-1 * x76 * x787) + x74 + (x761 * x780) + x805 + (x83 * x760) + (-1 * x782 * x783) + (-1 * x73 * x792) + (-1 * x784 * x785); const FLT x807 = -1 * x77; const FLT x808 = x34 * x797; const FLT x809 = x794 * x808; const FLT x810 = x76 * x63; const FLT x811 = x80 * x803; const FLT x812 = 1.0/2.0 * x76; const FLT x813 = (-1 * x795 * x804) + (x78 * x800); const FLT x814 = x813 + x811 + (-1 * x809) + (-1 * x789 * x784) + (x791 * x810) + (-1 * x73 * x787) + (-1 * x761 * x783) + x807 + (x785 * x812) + (-1 * x72 * x760) + (-1 * x782 * x780); const FLT x815 = x73 * x63; const FLT x816 = x795 * x815; const FLT x817 = x83 * x48; const FLT x818 = x799 * x817; const FLT x819 = -1 * x69; const FLT x820 = x72 * x802; const FLT x821 = x794 * x810; const FLT x822 = (x34 * x821) + (-1 * x48 * x820); const FLT x823 = x822 + x819 + (-1 * x781 * x787) + (x761 * x785) + (-1 * x80 * x760) + (-1 * x790 * x781) + (x792 * x796) + (x784 * x780) + (x783 * x812) + (-1 * x818) + x816; const FLT x824 = x802 * x817; const FLT x825 = x72 * x800; const FLT x826 = x34 * x784; const FLT x827 = x51 * x821; const FLT x828 = x34 * x815; const FLT x829 = x794 * x828; const FLT x830 = (-1 * x781 * x792) + x68 + (-1 * x780 * x812) + (-1 * x824) + (-1 * x782 * x785) + (x779 * x826) + (-1 * x787 * x796) + x829 + (x78 * x760) + x825 + (-1 * x761 * x789) + (-1 * x827); const FLT x831 = (x830 * sensor_pt[1]) + (-1 * x814 * sensor_pt[0]) + (x823 * sensor_pt[2]); const FLT x832 = (-1 * x830 * sensor_pt[2]) + (x806 * sensor_pt[0]) + (x823 * sensor_pt[1]); const FLT x833 = (-1 * x624 * x832) + (x625 * x814) + (-1 * x599 * x806) + (x622 * x831); const FLT x834 = (-1 * x806 * sensor_pt[1]) + (x814 * sensor_pt[2]) + (x823 * sensor_pt[0]); const FLT x835 = (x629 * x832) + (x599 * x830) + (-1 * x627 * x814) + (-1 * x622 * x834); const FLT x836 = (-1 * x629 * x831) + (x624 * x834) + (-1 * x625 * x830) + (x627 * x806); const FLT x837 = x22 * x836; const FLT x838 = (-1 * x12 * x837) + (x29 * x833) + (x631 * x835); const FLT x839 = (-1 * x29 * x835) + (x96 * x836) + (x631 * x833); const FLT x840 = x836 + (x838 * x278) + (-1 * x839 * x290); const FLT x841 = (-1 * x96 * x833) + (x31 * x835) + (x9 * x837); const FLT x842 = x833 + (x841 * x290) + (-1 * x838 * x331); const FLT x843 = x835 + (x839 * x331) + (-1 * x841 * x278); const FLT x844 = (x843 * x167) + (x842 * x158); const FLT x845 = (x840 * x119) + (-1 * x170 * (x844 + (x840 * x152))); const FLT x846 = (-1 * x840 * x108) + (x844 * x185); const FLT x847 = ((x842 * x188) + (-1 * x843 * x187)) * x189; const FLT x848 = (-1 * x847) + (x846 * x184); const FLT x849 = x845 * x175; const FLT x850 = (x845 * x177) + (x120 * (x849 + (-1 * x845 * x174))); const FLT x851 = (x845 * x179) + (x850 * x120); const FLT x852 = x847 + (-1 * x141 * ((x851 * x181) + x846 + (-1 * x848 * x208) + (x845 * x207) + (-1 * x204 * ((x202 * ((x851 * x120) + (x845 * x200) + (x845 * x201) + (x120 * (x851 + (x845 * x195) + (x120 * (x850 + (x120 * ((-1 * x845 * x198) + x849 + (x845 * x196))) + (x845 * x199))))))) + (-1 * x848 * x194))))); const FLT x853 = x52 * x798; const FLT x854 = x52 * x793; const FLT x855 = x63 * x854; const FLT x856 = x49 * x770; const FLT x857 = x49 * x768; const FLT x858 = x51 * x45; const FLT x859 = x64 * x858; const FLT x860 = x51 * x46; const FLT x861 = x51 * x51 * x51; const FLT x862 = (x772 * x858) + (-1 * x770 * x861) + (x768 * x861) + (-1 * x51 * x856) + (-1 * x765 * x859) + (x51 * x857) + (x768 * x860) + (-1 * x770 * x860); const FLT x863 = x778 * x862; const FLT x864 = x48 * x863; const FLT x865 = x34 * x863; const FLT x866 = x51 * x863; const FLT x867 = x788 * x862; const FLT x868 = 0.5 * x859; const FLT x869 = x51 * x793; const FLT x870 = x34 * x869; const FLT x871 = x51 * x802; const FLT x872 = (x78 * x871) + (-1 * x870 * x804); const FLT x873 = x872 + (-1 * x810 * x868) + (-1 * x816) + x819 + (-1 * x784 * x864) + (-1 * x782 * x865) + (-1 * x80 * x853) + (x796 * x855) + (-1 * x812 * x867) + (x761 * x866) + x818; const FLT x874 = x808 * x869; const FLT x875 = x80 * x871; const FLT x876 = x812 * x863; const FLT x877 = (x78 * x853) + (x48 * x876) + (-1 * x784 * x867) + (-1 * x874) + x68 + (-1 * x825) + (-1 * x804 * x854) + (-1 * x761 * x865) + x827 + (-1 * x782 * x866) + (-1 * x815 * x868) + x875; const FLT x878 = x870 * x810; const FLT x879 = x51 * x820; const FLT x880 = -1 * x74; const FLT x881 = x801 + (x784 * x866) + (-1 * x804 * x868) + (x34 * x876) + (x761 * x864) + (-1 * x782 * x867) + x880 + x878 + (-1 * x83 * x853) + (x73 * x855) + (-1 * x879); const FLT x882 = (-1 * x83 * x871) + (x828 * x869); const FLT x883 = x813 + x882 + (-1 * x797 * x868) + x77 + (-1 * x51 * x876) + (-1 * x810 * x854) + (-1 * x761 * x867) + (x72 * x853) + (x826 * x863) + (-1 * x782 * x864); const FLT x884 = (x883 * sensor_pt[1]) + (-1 * x877 * sensor_pt[0]) + (x881 * sensor_pt[2]); const FLT x885 = (-1 * x883 * sensor_pt[2]) + (x873 * sensor_pt[0]) + (x881 * sensor_pt[1]); const FLT x886 = (x625 * x877) + (-1 * x599 * x873) + (-1 * x624 * x885) + (x622 * x884); const FLT x887 = (x877 * sensor_pt[2]) + (-1 * x873 * sensor_pt[1]) + (x881 * sensor_pt[0]); const FLT x888 = (x624 * x887) + (-1 * x629 * x884) + (-1 * x625 * x883) + (x627 * x873); const FLT x889 = (-1 * x622 * x887) + (x629 * x885) + (-1 * x627 * x877) + (x599 * x883); const FLT x890 = (-1 * x29 * x889) + (x631 * x886) + (x96 * x888); const FLT x891 = (-1 * x31 * x888) + (x29 * x886) + (x631 * x889); const FLT x892 = x888 + (-1 * x890 * x290) + (x891 * x278); const FLT x893 = x22 * ((-1 * x96 * x886) + (x31 * x889) + (x631 * x888)); const FLT x894 = x886 + (x893 * x159) + (-1 * x891 * x331); const FLT x895 = x889 + (x890 * x331) + (-1 * x893 * x146); const FLT x896 = (x895 * x167) + (x894 * x158); const FLT x897 = (x892 * x119) + (-1 * x170 * (x896 + (x892 * x152))); const FLT x898 = x897 * x175; const FLT x899 = (x897 * x177) + (x120 * (x898 + (-1 * x897 * x174))); const FLT x900 = (x897 * x179) + (x899 * x120); const FLT x901 = (-1 * x892 * x108) + (x896 * x185); const FLT x902 = ((x894 * x188) + (-1 * x895 * x187)) * x189; const FLT x903 = (-1 * x902) + (x901 * x184); const FLT x904 = x902 + (-1 * x141 * (x901 + (-1 * x903 * x208) + (-1 * x204 * ((x202 * ((x900 * x120) + (x120 * (x900 + (x897 * x195) + (x120 * (x899 + (x120 * (x898 + (-1 * x897 * x198) + (x897 * x196))) + (x897 * x199))))) + (x897 * x201) + (x897 * x200))) + (-1 * x903 * x194))) + (x900 * x181) + (x897 * x207))); const FLT x905 = x45 * x34; const FLT x906 = x52 * x34; const FLT x907 = x34 * x34 * x34; const FLT x908 = (x907 * x768) + (-1 * x907 * x770) + (-1 * x34 * x856) + (x34 * x857) + (x906 * x768) + (-1 * x905 * x774) + (x34 * x773) + (-1 * x906 * x770); const FLT x909 = x908 * x778; const FLT x910 = x34 * x909; const FLT x911 = x46 * x793; const FLT x912 = x51 * x909; const FLT x913 = x46 * x798; const FLT x914 = x905 * x786; const FLT x915 = x908 * x788; const FLT x916 = x48 * x909; const FLT x917 = (-1 * x916 * x784) + (-1 * x910 * x782) + x824 + x874 + x68 + (x912 * x761) + (-1 * x875) + (-1 * x911 * x804) + (x78 * x913) + (-1 * x829) + (-1 * x76 * x914) + (-1 * x915 * x812); const FLT x918 = x63 * x911; const FLT x919 = x805 + x879 + (x909 * x826) + (-1 * x912 * x812) + (-1 * x916 * x782) + (-1 * x915 * x761) + (-1 * x878) + x880 + (x73 * x918) + (-1 * x914 * x796) + (-1 * x83 * x913); const FLT x920 = x882 + (-1 * x915 * x782) + (-1 * x914 * x781) + (x911 * x810) + (x910 * x812) + (x912 * x784) + x807 + (-1 * x811) + (x916 * x761) + (-1 * x72 * x913) + x809; const FLT x921 = (x917 * sensor_pt[0]) + (-1 * x919 * sensor_pt[2]) + (x920 * sensor_pt[1]); const FLT x922 = x822 + x872 + (-1 * x918 * x796) + (-1 * x915 * x784) + (-1 * x73 * x914) + x69 + (x916 * x812) + (x80 * x913) + (-1 * x912 * x782) + (-1 * x910 * x761); const FLT x923 = (x919 * sensor_pt[1]) + (x920 * sensor_pt[2]) + (-1 * x922 * sensor_pt[0]); const FLT x924 = (x923 * x622) + (x922 * x625) + (-1 * x917 * x599) + (-1 * x921 * x624); const FLT x925 = (x922 * sensor_pt[2]) + (-1 * x917 * sensor_pt[1]) + (x920 * sensor_pt[0]); const FLT x926 = (-1 * x925 * x622) + (x921 * x629) + (-1 * x922 * x627) + (x919 * x599); const FLT x927 = (x925 * x624) + (-1 * x923 * x629) + (-1 * x919 * x625) + (x917 * x627); const FLT x928 = x22 * x927; const FLT x929 = (-1 * x12 * x928) + (x29 * x924) + (x926 * x631); const FLT x930 = (x6 * x928) + (-1 * x29 * x926) + (x924 * x631); const FLT x931 = x927 + (x929 * x278) + (-1 * x930 * x290); const FLT x932 = (-1 * x96 * x924) + (x31 * x926) + (x9 * x928); const FLT x933 = x924 + (x932 * x290) + (-1 * x929 * x331); const FLT x934 = x926 + (x930 * x331) + (-1 * x932 * x278); const FLT x935 = (x934 * x167) + (x933 * x158); const FLT x936 = (x931 * x119) + (-1 * x170 * (x935 + (x931 * x152))); const FLT x937 = x936 * x175; const FLT x938 = (x936 * x177) + (x120 * (x937 + (-1 * x936 * x174))); const FLT x939 = (x936 * x179) + (x938 * x120); const FLT x940 = (-1 * x931 * x108) + (x935 * x185); const FLT x941 = ((x933 * x188) + (-1 * x934 * x187)) * x189; const FLT x942 = (-1 * x941) + (x940 * x184); const FLT x943 = x941 + (-1 * x141 * (x940 + (-1 * x204 * ((x202 * ((x939 * x120) + (x120 * (x939 + (x936 * x195) + (x120 * (x938 + (x120 * ((-1 * x936 * x198) + (x936 * x196) + x937)) + (x936 * x199))))) + (x936 * x201) + (x936 * x200))) + (-1 * x942 * x194))) + (-1 * x942 * x208) + (x936 * x207) + (x939 * x181))); const FLT x944 = -1 * dt * x155; const FLT x945 = -1 * dt * x154; const FLT x946 = x945 + x944 + dt; const FLT x947 = dt * x164; const FLT x948 = dt * x162; const FLT x949 = x948 + x947; const FLT x950 = (x949 * x167) + (x946 * x158); const FLT x951 = dt * x149; const FLT x952 = dt * x145; const FLT x953 = (-1 * x952) + x951; const FLT x954 = (-1 * x953 * x108) + (x950 * x185); const FLT x955 = ((x946 * x188) + (-1 * x949 * x187)) * x189; const FLT x956 = (-1 * x955) + (x954 * x184); const FLT x957 = (x953 * x119) + (-1 * x170 * (x950 + (x953 * x152))); const FLT x958 = x957 * x175; const FLT x959 = (x957 * x177) + (x120 * (x958 + (-1 * x957 * x174))); const FLT x960 = (x957 * x179) + (x959 * x120); const FLT x961 = x955 + (-1 * x141 * ((-1 * x956 * x208) + (x957 * x207) + x954 + (-1 * x204 * ((x202 * ((x957 * x200) + (x120 * (x960 + (x957 * x195) + (x120 * (x959 + (x120 * ((-1 * x957 * x198) + x958 + (x957 * x196))) + (x957 * x199))))) + (x960 * x120) + (x957 * x201))) + (-1 * x956 * x194))) + (x960 * x181))); const FLT x962 = (-1 * dt * x212) + dt; const FLT x963 = x962 + x945; const FLT x964 = x951 + x952; const FLT x965 = dt * x217; const FLT x966 = dt * x216; const FLT x967 = (-1 * x966) + x965; const FLT x968 = (x967 * x167) + (x964 * x158); const FLT x969 = (x963 * x119) + (-1 * x170 * (x968 + (x963 * x152))); const FLT x970 = (-1 * x963 * x108) + (x968 * x185); const FLT x971 = ((x964 * x188) + (-1 * x967 * x187)) * x189; const FLT x972 = (-1 * x971) + (x970 * x184); const FLT x973 = x969 * x175; const FLT x974 = (x969 * x177) + (x120 * (x973 + (-1 * x969 * x174))); const FLT x975 = (x969 * x179) + (x974 * x120); const FLT x976 = x971 + (-1 * x141 * (x970 + (-1 * x972 * x208) + (x975 * x181) + (x969 * x207) + (-1 * x204 * ((x202 * ((x969 * x201) + (x975 * x120) + (x120 * (x975 + (x969 * x195) + (x120 * (x974 + (x120 * ((-1 * x969 * x198) + x973 + (x969 * x196))) + (x969 * x199))))) + (x969 * x200))) + (-1 * x972 * x194))))); const FLT x977 = x965 + x966; const FLT x978 = (-1 * x947) + x948; const FLT x979 = x962 + x944; const FLT x980 = (x979 * x167) + (x978 * x158); const FLT x981 = (x977 * x119) + (-1 * x170 * (x980 + (x977 * x152))); const FLT x982 = (-1 * x977 * x108) + (x980 * x185); const FLT x983 = ((x978 * x188) + (-1 * x979 * x187)) * x189; const FLT x984 = (-1 * x983) + (x982 * x184); const FLT x985 = x981 * x175; const FLT x986 = (x981 * x177) + (x120 * (x985 + (-1 * x981 * x174))); const FLT x987 = (x981 * x179) + (x986 * x120); const FLT x988 = x983 + (-1 * x141 * (x982 + (-1 * x984 * x208) + (x987 * x181) + (x981 * x207) + (-1 * x204 * ((x202 * ((x987 * x120) + (x981 * x200) + (x120 * (x987 + (x981 * x195) + (x120 * ((x120 * ((-1 * x981 * x198) + x985 + (x981 * x196))) + x986 + (x981 * x199))))) + (x981 * x201))) + (-1 * x984 * x194))))); const FLT x989 = x203 * x135; const FLT x990 = ((-1 * x989) + x139) * x141; const FLT x991 = ((x989 * x111) + (-1 * x111 * x139)) * x141; const FLT x992 = x141 * ((-1 * x989 * x192) + x208); const FLT x993 = x102 * x105 * (1 + x183); const FLT x994 = x993 * x184; const FLT x995 = x116 * x172; const FLT x996 = x995 * x102 * x127; const FLT x997 = -1 * x996 * x175; const FLT x998 = (-1 * x996 * x177) + (x120 * (x997 + (x996 * x174))); const FLT x999 = (-1 * x996 * x179) + (x998 * x120); const FLT x1000 = x141 * (x993 + (-1 * x994 * x208) + (-1 * x995 * x202 * x205 * x152) + (x999 * x181) + (-1 * x204 * ((x202 * ((x999 * x120) + (-1 * x996 * x200) + (-1 * x996 * x201) + (x120 * (x999 + (-1 * x996 * x195) + (x120 * (x998 + (x120 * ((x996 * x198) + x997 + (-1 * x996 * x196))) + (-1 * x996 * x199))))))) + (-1 * x117 * x134) + x127 + (-1 * x994 * x194)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x209 + (x211 * x209)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x228 + (x211 * x228)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x241 + (x211 * x241)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x351 + (x211 * x351)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x409 + (x409 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x450 + (x450 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x483 + (x483 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x505 + (x505 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x520 + (x520 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x532 + (x532 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), x546 + (x546 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x559 + (x559 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x571 + (x571 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x648 + (x648 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x687 + (x687 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x724 + (x724 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x756 + (x756 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x852 + (x852 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x904 + (x904 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x943 + (x943 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x961 + (x961 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x976 + (x976 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x988 + (x988 * x211)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), (-1 * x990 * x211) + (-1 * x990)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), sin(x210)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), x211); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), (-1 * x991 * x211) + (-1 * x991)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), (-1 * x992 * x211) + (-1 * x992)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), (-1 * x211 * x1000) + (-1 * x1000)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x0 * (*_x0).Lighthouse.Rot[2]); const FLT x4 = 1.0/2.0 * x3; const FLT x5 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x6 = 0.5 * (*_x0).Lighthouse.Rot[3]; const FLT x7 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x6 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]); const FLT x8 = x7 * x7; const FLT x9 = (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x6 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x1 * (*_x0).Lighthouse.Rot[2]); const FLT x10 = 1.0 * x9; const FLT x11 = x10 * (*_x0).Lighthouse.Rot[0]; const FLT x12 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x6 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2]; const FLT x13 = 1.0 * x12; const FLT x14 = x13 * (*_x0).Lighthouse.Rot[3]; const FLT x15 = 1.0 * x3; const FLT x16 = x15 * (*_x0).Lighthouse.Rot[2]; const FLT x17 = 1.0 * x7; const FLT x18 = x17 * (*_x0).Lighthouse.Rot[1]; const FLT x19 = (-1 * x18) + (-1 * x16) + x11 + x14; const FLT x20 = x3 * x3; const FLT x21 = x12 * x12; const FLT x22 = x9 * x9; const FLT x23 = x22 + x21 + x20 + x8; const FLT x24 = 1. / (x23 * x23); const FLT x25 = x24 * x19; const FLT x26 = 1. / x23; const FLT x27 = 1. / sqrt(x23); const FLT x28 = x20 * x26; const FLT x29 = x21 * x26; const FLT x30 = x22 * x26; const FLT x31 = x30 + (x8 * x26) + x28 + x29; const FLT x32 = (1. / (x31 * sqrt(x31))) * x27; const FLT x33 = x32 * ((-1 * x26 * x18) + (-1 * x26 * x16) + (x26 * x11) + (-1 * x8 * x25) + (-1 * x25 * x20) + (-1 * x25 * x21) + (x26 * x14) + (-1 * x25 * x22)); const FLT x34 = x5 * x33; const FLT x35 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x36 = 1. / sqrt(x31); const FLT x37 = x36 * (1. / (x23 * sqrt(x23))); const FLT x38 = x37 * x19; const FLT x39 = 1.0/2.0 * x7; const FLT x40 = x38 * x39; const FLT x41 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x42 = x33 * x12; const FLT x43 = 1.0/2.0 * x42; const FLT x44 = x5 * x38; const FLT x45 = x38 * x12; const FLT x46 = 1.0/2.0 * x45; const FLT x47 = x33 * x39; const FLT x48 = x36 * x27; const FLT x49 = x6 * x48; const FLT x50 = 0.5 * x48; const FLT x51 = x50 * (*_x0).Lighthouse.Rot[2]; const FLT x52 = x48 * x35; const FLT x53 = 0.5 * x52; const FLT x54 = (-1 * x53 * (*_x0).Lighthouse.Rot[1]) + (-1 * x41 * x49) + (-1 * x5 * x51); const FLT x55 = (-1 * x47 * x35) + (x41 * x43) + (-1 * x4 * x34) + x54 + (-1 * x4 * x44) + (-1 * x40 * x35) + (x41 * x46); const FLT x56 = x3 * x48; const FLT x57 = 2 * x56; const FLT x58 = x6 * x52; const FLT x59 = 1.0/2.0 * x9; const FLT x60 = x50 * (*_x0).Lighthouse.Rot[1]; const FLT x61 = x60 * x41; const FLT x62 = x50 * (*_x0).Lighthouse.Rot[0]; const FLT x63 = x5 * x62; const FLT x64 = (-1 * x63) + (x59 * x34) + (-1 * x46 * x35) + (-1 * x61) + (-1 * x43 * x35) + x58 + (-1 * x41 * x47) + (x59 * x44) + (-1 * x40 * x41); const FLT x65 = x9 * x48; const FLT x66 = 2 * x65; const FLT x67 = x48 * x12; const FLT x68 = x7 * x48; const FLT x69 = (-1 * x67 * x41) + (x5 * x56) + (x68 * x35); const FLT x70 = x3 * x69; const FLT x71 = dt * fabs(dt); const FLT x72 = 1.0/2.0 * x71; const FLT x73 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x74 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x75 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x76 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0]; const FLT x77 = (*_x0).Object.Pose.Rot[0] + (-1 * x76 * (*_x0).Object.Pose.Rot[1]) + (-1 * x74 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x75 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x78 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x79 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x80 = (x79 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x76 * (*_x0).Object.Pose.Rot[2]) + (x78 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x81 = (x76 * (*_x0).Object.Pose.Rot[3]) + (-1 * x79 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x78 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2]; const FLT x82 = (*_x0).Object.Pose.Rot[1] + (x76 * (*_x0).Object.Pose.Rot[0]) + (-1 * x74 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x75 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x83 = (x77 * x77) + (x82 * x82) + (x80 * x80) + (x81 * x81); const FLT x84 = 1. / sqrt(x83); const FLT x85 = dt * dt; const FLT x86 = x73 * x73; const FLT x87 = x85 * x86; const FLT x88 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x89 = x88 * x88; const FLT x90 = x89 * x85; const FLT x91 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x92 = x91 * x91; const FLT x93 = x85 * x92; const FLT x94 = 1e-10 + x93 + x87 + x90; const FLT x95 = sqrt(x94); const FLT x96 = 0.5 * x95; const FLT x97 = sin(x96); const FLT x98 = x97 * x97; const FLT x99 = 1. / x94; const FLT x100 = x99 * x98; const FLT x101 = cos(x96); const FLT x102 = (x90 * x100) + (x101 * x101) + (x87 * x100) + (x93 * x100); const FLT x103 = 1. / sqrt(x102); const FLT x104 = x97 * (1. / x95); const FLT x105 = dt * x104; const FLT x106 = x103 * x105; const FLT x107 = x84 * x106; const FLT x108 = x77 * x107; const FLT x109 = x82 * x84; const FLT x110 = x103 * x109; const FLT x111 = x91 * x104; const FLT x112 = dt * x111; const FLT x113 = x101 * x103; const FLT x114 = x80 * x84; const FLT x115 = x113 * x114; const FLT x116 = x81 * x107; const FLT x117 = (x88 * x116) + x115 + (x73 * x108) + (-1 * x110 * x112); const FLT x118 = x106 * x114; const FLT x119 = x84 * x103; const FLT x120 = x81 * x119; const FLT x121 = x84 * x113; const FLT x122 = x77 * x121; const FLT x123 = x105 * x110; const FLT x124 = (-1 * x88 * x123) + (-1 * x73 * x118) + x122 + (-1 * x112 * x120); const FLT x125 = x103 * x114; const FLT x126 = x82 * x121; const FLT x127 = (x88 * x108) + x126 + (-1 * x73 * x116) + (x112 * x125); const FLT x128 = (-1 * x127 * sensor_pt[2]) + (x124 * sensor_pt[1]) + (x117 * sensor_pt[0]); const FLT x129 = x77 * x119; const FLT x130 = x81 * x121; const FLT x131 = (-1 * x88 * x118) + (x73 * x123) + (x112 * x129) + x130; const FLT x132 = (-1 * x131 * sensor_pt[0]) + (x124 * sensor_pt[2]) + (x127 * sensor_pt[1]); const FLT x133 = (*_x0).Object.Pose.Pos[0] + (x72 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x131 * x132) + (-1 * x117 * x128))); const FLT x134 = x49 * x133; const FLT x135 = (-1 * x117 * sensor_pt[1]) + (x124 * sensor_pt[0]) + (x131 * sensor_pt[2]); const FLT x136 = (*_x0).Object.Pose.Pos[1] + (x72 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + sensor_pt[1] + (2 * ((x117 * x135) + (-1 * x127 * x132))); const FLT x137 = x59 * x38; const FLT x138 = (*_x0).Object.Pose.Pos[2] + (2 * ((x128 * x127) + (-1 * x131 * x135))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x72 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x139 = x48 * x138; const FLT x140 = 0.5 * x139; const FLT x141 = x140 * (*_x0).Lighthouse.Rot[1]; const FLT x142 = x62 * x136; const FLT x143 = x59 * x33; const FLT x144 = (-1 * x47 * x138) + x134 + (-1 * x43 * x133) + (-1 * x46 * x133) + (-1 * x142) + (x137 * x136) + (-1 * x141) + (x136 * x143) + (-1 * x40 * x138); const FLT x145 = (x67 * x133) + (x68 * x138) + (-1 * x65 * x136); const FLT x146 = 1.0 * x48; const FLT x147 = x146 * x145; const FLT x148 = x147 * (*_x0).Lighthouse.Rot[0]; const FLT x149 = (x56 * x136) + (x68 * x133) + (-1 * x67 * x138); const FLT x150 = x3 * x149; const FLT x151 = x146 * x149; const FLT x152 = x69 * x146; const FLT x153 = x9 * x145; const FLT x154 = (x67 * x35) + (x68 * x41) + (-1 * x5 * x65); const FLT x155 = x9 * x154; const FLT x156 = x146 * x154; const FLT x157 = x156 * (*_x0).Lighthouse.Rot[0]; const FLT x158 = x4 * x136; const FLT x159 = x33 * x133; const FLT x160 = (-1 * x6 * x139) + (-1 * x51 * x136) + (-1 * x60 * x133); const FLT x161 = x160 + (x43 * x138) + (-1 * x40 * x133) + (-1 * x38 * x158) + (-1 * x33 * x158) + (x46 * x138) + (-1 * x39 * x159); const FLT x162 = (x38 * x155) + (-1 * x57 * x161) + (-1 * x33 * x153) + (x38 * x150) + (x57 * x55) + (x66 * x144) + x148 + (-1 * x70 * x33) + (x151 * (*_x0).Lighthouse.Rot[2]) + (x33 * x155) + (-1 * x64 * x66) + (x33 * x150) + (-1 * x152 * (*_x0).Lighthouse.Rot[2]) + (-1 * x157) + (-1 * x70 * x38) + (-1 * x38 * x153); const FLT x163 = x136 + (2 * ((-1 * x56 * x149) + (x65 * x145))) + (-1 * (x5 + (2 * ((-1 * x69 * x56) + (x65 * x154))))); const FLT x164 = 2 * x163; const FLT x165 = x12 * x145; const FLT x166 = 2 * x67; const FLT x167 = (x68 * x136) + (x65 * x138) + (-1 * x56 * x133); const FLT x168 = x38 * x167; const FLT x169 = (x5 * x68) + (x65 * x41) + (-1 * x56 * x35); const FLT x170 = x3 * x169; const FLT x171 = x140 * (*_x0).Lighthouse.Rot[0]; const FLT x172 = x4 * x133; const FLT x173 = x60 * x136; const FLT x174 = x51 * x133; const FLT x175 = (x4 * x159) + (-1 * x47 * x136) + (x38 * x172) + (-1 * x173) + (-1 * x40 * x136) + x171 + (-1 * x138 * x143) + x174 + (-1 * x137 * x138); const FLT x176 = x59 * x41; const FLT x177 = x62 * x41; const FLT x178 = x4 * x35; const FLT x179 = x5 * x60; const FLT x180 = x53 * (*_x0).Lighthouse.Rot[2]; const FLT x181 = x180 + (-1 * x5 * x40) + (x33 * x178) + (-1 * x179) + (-1 * x34 * x39) + (-1 * x38 * x176) + x177 + (x38 * x178) + (-1 * x33 * x176); const FLT x182 = x33 * x167; const FLT x183 = x167 * x146; const FLT x184 = x146 * (*_x0).Lighthouse.Rot[2]; const FLT x185 = (x169 * x184) + (-1 * x183 * (*_x0).Lighthouse.Rot[2]); const FLT x186 = (x156 * (*_x0).Lighthouse.Rot[3]) + (-1 * x147 * (*_x0).Lighthouse.Rot[3]); const FLT x187 = x186 + x185 + (-1 * x3 * x182) + (-1 * x45 * x154) + (-1 * x57 * x181) + (-1 * x3 * x168) + (x38 * x170) + (x33 * x165) + (-1 * x166 * x144) + (-1 * x42 * x154) + (x45 * x145) + (x57 * x175) + (x64 * x166) + (x33 * x170); const FLT x188 = x133 + (-1 * (x35 + (2 * ((-1 * x67 * x154) + (x56 * x169))))) + (2 * ((-1 * x67 * x145) + (x56 * x167))); const FLT x189 = 2 * x188; const FLT x190 = x12 * x149; const FLT x191 = x9 * x169; const FLT x192 = x183 * (*_x0).Lighthouse.Rot[0]; const FLT x193 = x169 * x146; const FLT x194 = x193 * (*_x0).Lighthouse.Rot[0]; const FLT x195 = (-1 * x66 * x175) + (-1 * x152 * (*_x0).Lighthouse.Rot[3]) + x194 + (-1 * x33 * x190) + (-1 * x38 * x191) + (-1 * x55 * x166) + (x69 * x45) + (x66 * x181) + (x9 * x168) + (-1 * x38 * x190) + (x161 * x166) + (x151 * (*_x0).Lighthouse.Rot[3]) + (x9 * x182) + (x69 * x42) + (-1 * x33 * x191) + (-1 * x192); const FLT x196 = (-1 * (x41 + (2 * ((-1 * x65 * x169) + (x67 * x69))))) + x138 + (2 * ((-1 * x65 * x167) + (x67 * x149))); const FLT x197 = 2 * x196; const FLT x198 = (x197 * x195) + (x187 * x189); const FLT x199 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt); const FLT x200 = cos(x199); const FLT x201 = 1. / x200; const FLT x202 = x163 * x163; const FLT x203 = x188 * x188; const FLT x204 = x203 + (x196 * x196); const FLT x205 = x204 + x202; const FLT x206 = 1.0/2.0 * x163; const FLT x207 = x201 * (1. / (x205 * sqrt(x205))) * x206; const FLT x208 = 1. / sqrt(x205); const FLT x209 = x201 * x208; const FLT x210 = (x209 * x162) + (-1 * x207 * (x198 + (x164 * x162))); const FLT x211 = 1. / (x200 * x200); const FLT x212 = 1. / sqrt(1 + (-1 * x211 * x202 * (1. / x205))); const FLT x213 = asin(x209 * x163); const FLT x214 = 8.0108022e-06 * x213; const FLT x215 = x212 * x214; const FLT x216 = -8.0108022e-06 + (-1 * x214); const FLT x217 = x212 * x216; const FLT x218 = x210 * x217; const FLT x219 = 0.0028679863 + (x213 * x216); const FLT x220 = x212 * x219; const FLT x221 = (x210 * x220) + (x213 * (x218 + (-1 * x210 * x215))); const FLT x222 = 5.3685255e-06 + (x213 * x219); const FLT x223 = x212 * x222; const FLT x224 = (x210 * x223) + (x213 * x221); const FLT x225 = atan2(-1 * x196, x188); const FLT x226 = 1. / sqrt(x204); const FLT x227 = tan(x199); const FLT x228 = x227 * x226; const FLT x229 = -1 * x228 * x163; const FLT x230 = asin(x229) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * x225); const FLT x231 = sin(x230); const FLT x232 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag; const FLT x233 = (*_x0).BSD1.curve + (-1 * x232 * x231) + (*error_model).BSD1.curve; const FLT x234 = x213 * x213; const FLT x235 = 0.0076069798 + (x213 * x222); const FLT x236 = x213 * x235; const FLT x237 = -8.0108022e-06 + (-1.60216044e-05 * x213); const FLT x238 = x219 + (x213 * x237); const FLT x239 = x222 + (x213 * x238); const FLT x240 = x235 + (x213 * x239); const FLT x241 = (x213 * x240) + x236; const FLT x242 = sin(x199); const FLT x243 = x233 * x242; const FLT x244 = x241 * x243; const FLT x245 = x200 + x244; const FLT x246 = 1. / x245; const FLT x247 = x234 * x246; const FLT x248 = x233 * x247; const FLT x249 = x212 * x236 * x246; const FLT x250 = 2 * x233; const FLT x251 = x250 * x249; const FLT x252 = 1. / x204; const FLT x253 = x227 * x227; const FLT x254 = 1. / sqrt(1 + (-1 * x202 * x252 * x253)); const FLT x255 = (1. / (x204 * sqrt(x204))) * x206 * x227; const FLT x256 = (-1 * x228 * x162) + (x255 * x198); const FLT x257 = 1. / x188; const FLT x258 = (1. / x203) * x196; const FLT x259 = x203 * x252; const FLT x260 = ((x258 * x187) + (-1 * x257 * x195)) * x259; const FLT x261 = (-1 * x260) + (x254 * x256); const FLT x262 = x232 * cos(x230); const FLT x263 = x241 * x242; const FLT x264 = x262 * x263; const FLT x265 = x212 * x240; const FLT x266 = x212 * x239; const FLT x267 = x212 * x237; const FLT x268 = 2.40324066e-05 * x213; const FLT x269 = x212 * x268; const FLT x270 = x212 * x238; const FLT x271 = x212 * x235; const FLT x272 = x234 * x235 * (1. / (x245 * x245)); const FLT x273 = x233 * x272; const FLT x274 = x235 * x247; const FLT x275 = x274 * x262; const FLT x276 = x229 + (x233 * x274); const FLT x277 = 1. / sqrt(1 + (-1 * (x276 * x276))); const FLT x278 = x260 + (-1 * x277 * (x256 + (-1 * x275 * x261) + (x224 * x248) + (-1 * x273 * ((x243 * ((x213 * x224) + (x210 * x265) + (x210 * x271) + (x213 * (x224 + (x210 * x266) + (x213 * (x221 + (x213 * ((-1 * x210 * x269) + x218 + (x210 * x267))) + (x210 * x270))))))) + (-1 * x264 * x261))) + (x210 * x251))); const FLT x279 = (-1 * asin(x276)) + x225 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha; const FLT x280 = cos(x279) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag); const FLT x281 = x10 * (*_x0).Lighthouse.Rot[3]; const FLT x282 = x13 * (*_x0).Lighthouse.Rot[0]; const FLT x283 = x15 * (*_x0).Lighthouse.Rot[1]; const FLT x284 = x17 * (*_x0).Lighthouse.Rot[2]; const FLT x285 = (-1 * x284) + x283 + (-1 * x281) + x282; const FLT x286 = x37 * x285; const FLT x287 = x24 * x285; const FLT x288 = x32 * ((x26 * x282) + (x26 * x283) + (-1 * x26 * x284) + (-1 * x22 * x287) + (-1 * x26 * x281) + (-1 * x20 * x287) + (-1 * x21 * x287) + (-1 * x8 * x287)); const FLT x289 = x4 * x286; const FLT x290 = 1.0/2.0 * x12; const FLT x291 = x290 * x286; const FLT x292 = x39 * x286; const FLT x293 = x288 * x138; const FLT x294 = x288 * x133; const FLT x295 = 2 * ((-1 * x39 * x294) + (-1 * x174) + (-1 * x289 * x136) + (x293 * x290) + x173 + (x291 * x138) + (-1 * x171) + (-1 * x288 * x158) + (-1 * x292 * x133)); const FLT x296 = x286 * x133; const FLT x297 = x62 * x133; const FLT x298 = x59 * x286; const FLT x299 = x140 * (*_x0).Lighthouse.Rot[2]; const FLT x300 = x49 * x136; const FLT x301 = (x59 * x288 * x136) + x300 + (-1 * x299) + (-1 * x294 * x290) + x297 + (-1 * x296 * x290) + (x298 * x136) + (-1 * x292 * x138) + (-1 * x39 * x293); const FLT x302 = x290 * x288; const FLT x303 = x41 * x39; const FLT x304 = x51 * x41; const FLT x305 = x5 * x49; const FLT x306 = x53 * (*_x0).Lighthouse.Rot[0]; const FLT x307 = x5 * x288; const FLT x308 = x306 + x305 + (-1 * x41 * x292) + (-1 * x35 * x302) + (x5 * x298) + (x59 * x307) + (-1 * x35 * x291) + (-1 * x288 * x303) + (-1 * x304); const FLT x309 = x39 * x288; const FLT x310 = (-1 * x35 * x309) + (-1 * x180) + x179 + (-1 * x5 * x289) + (-1 * x177) + (x41 * x291) + (-1 * x35 * x292) + (x41 * x302) + (-1 * x4 * x307); const FLT x311 = (x152 * (*_x0).Lighthouse.Rot[1]) + (-1 * x151 * (*_x0).Lighthouse.Rot[1]); const FLT x312 = x311 + (x288 * x155) + (-1 * x286 * x153) + (-1 * x66 * x308) + (-1 * x56 * x295) + (-1 * x70 * x286) + (-1 * x70 * x288) + x186 + (x286 * x155) + (x286 * x150) + (x288 * x150) + (x66 * x301) + (-1 * x288 * x153) + (x57 * x310); const FLT x313 = x160 + (x288 * x172) + (-1 * x309 * x136) + (-1 * x292 * x136) + (-1 * x298 * x138) + (x4 * x296) + (-1 * x59 * x293); const FLT x314 = x288 * x167; const FLT x315 = x12 * x154; const FLT x316 = x286 * x167; const FLT x317 = x54 + (x288 * x178) + (x286 * x178) + (-1 * x288 * x176) + (-1 * x5 * x292) + (-1 * x41 * x298) + (-1 * x39 * x307); const FLT x318 = (x286 * x165) + (x183 * (*_x0).Lighthouse.Rot[1]) + (x288 * x165) + (-1 * x288 * x315) + (x288 * x170) + (-1 * x193 * (*_x0).Lighthouse.Rot[1]) + (x308 * x166) + (x286 * x170) + (-1 * x57 * x317) + (-1 * x3 * x314) + (-1 * x301 * x166) + x157 + (x57 * x313) + (-1 * x3 * x316) + (-1 * x286 * x315) + (-1 * x148); const FLT x319 = x151 * (*_x0).Lighthouse.Rot[0]; const FLT x320 = x69 * x12; const FLT x321 = x152 * (*_x0).Lighthouse.Rot[0]; const FLT x322 = (x9 * x314) + (x9 * x316) + (-1 * x310 * x166) + (x288 * x320) + (-1 * x286 * x191) + (-1 * x193 * (*_x0).Lighthouse.Rot[3]) + (-1 * x66 * x313) + (x67 * x295) + (-1 * x288 * x190) + (-1 * x286 * x190) + (x183 * (*_x0).Lighthouse.Rot[3]) + (-1 * x288 * x191) + x319 + (x286 * x320) + (-1 * x321) + (x66 * x317); const FLT x323 = (x322 * x197) + (x318 * x189); const FLT x324 = (x209 * x312) + (-1 * x207 * (x323 + (x312 * x164))); const FLT x325 = (-1 * x228 * x312) + (x255 * x323); const FLT x326 = ((x258 * x318) + (-1 * x257 * x322)) * x259; const FLT x327 = (-1 * x326) + (x254 * x325); const FLT x328 = x217 * x324; const FLT x329 = (x220 * x324) + (x213 * (x328 + (-1 * x215 * x324))); const FLT x330 = (x223 * x324) + (x213 * x329); const FLT x331 = x326 + (-1 * x277 * ((x248 * x330) + x325 + (x251 * x324) + (-1 * x275 * x327) + (-1 * x273 * ((x243 * ((x213 * x330) + (x271 * x324) + (x265 * x324) + (x213 * (x330 + (x266 * x324) + (x213 * (x329 + (x213 * ((-1 * x269 * x324) + x328 + (x267 * x324))) + (x270 * x324))))))) + (-1 * x264 * x327))))); const FLT x332 = x10 * (*_x0).Lighthouse.Rot[2]; const FLT x333 = x13 * (*_x0).Lighthouse.Rot[1]; const FLT x334 = x15 * (*_x0).Lighthouse.Rot[0]; const FLT x335 = x17 * (*_x0).Lighthouse.Rot[3]; const FLT x336 = (-1 * x335) + x334 + x332 + (-1 * x333); const FLT x337 = x24 * x336; const FLT x338 = x32 * ((-1 * x26 * x335) + (-1 * x8 * x337) + (x26 * x332) + (-1 * x22 * x337) + (-1 * x20 * x337) + (-1 * x26 * x333) + (-1 * x21 * x337) + (x26 * x334)); const FLT x339 = x290 * x338; const FLT x340 = x37 * x336; const FLT x341 = x39 * x340; const FLT x342 = x290 * x340; const FLT x343 = x5 * x340; const FLT x344 = x5 * x338; const FLT x345 = x54 + (-1 * x41 * x341) + (-1 * x35 * x339) + (x59 * x343) + (-1 * x35 * x342) + (x59 * x344) + (-1 * x303 * x338); const FLT x346 = (-1 * x35 * x341) + (-1 * x4 * x344) + (x41 * x339) + (x41 * x342) + x63 + (-1 * x58) + x61 + (-1 * x35 * x39 * x338) + (-1 * x4 * x343); const FLT x347 = x338 * x133; const FLT x348 = x59 * x340; const FLT x349 = x338 * x138; const FLT x350 = x338 * x136; const FLT x351 = x160 + (x348 * x136) + (-1 * x290 * x347) + (x59 * x350) + (-1 * x341 * x138) + (-1 * x39 * x349) + (-1 * x342 * x133); const FLT x352 = (x290 * x349) + (-1 * x134) + (-1 * x341 * x133) + x142 + (-1 * x39 * x347) + x141 + (x342 * x138) + (-1 * x4 * x350) + (-1 * x340 * x158); const FLT x353 = (x338 * x150) + (-1 * x57 * x352) + (x340 * x150) + (x147 * (*_x0).Lighthouse.Rot[2]) + (x57 * x346) + x321 + (-1 * x184 * x154) + (-1 * x340 * x153) + (-1 * x66 * x345) + (-1 * x338 * x153) + (x338 * x155) + (x66 * x351) + (-1 * x70 * x338) + (x340 * x155) + (-1 * x70 * x340) + (-1 * x319); const FLT x354 = x340 * x167; const FLT x355 = (-1 * x297) + (-1 * x341 * x136) + (-1 * x300) + (-1 * x59 * x349) + (-1 * x39 * x350) + (x340 * x172) + (x338 * x172) + (-1 * x348 * x138) + x299; const FLT x356 = (x338 * x178) + (-1 * x5 * x341) + (-1 * x338 * x176) + (-1 * x39 * x344) + (-1 * x305) + (-1 * x306) + (-1 * x340 * x176) + x304 + (x340 * x178); const FLT x357 = x338 * x167; const FLT x358 = (x147 * (*_x0).Lighthouse.Rot[1]) + (x340 * x165) + (-1 * x351 * x166) + (x345 * x166) + (-1 * x3 * x354) + (-1 * x194) + (x338 * x165) + (x338 * x170) + x192 + (x340 * x170) + (-1 * x57 * x356) + (x57 * x355) + (-1 * x156 * (*_x0).Lighthouse.Rot[1]) + (-1 * x3 * x357) + (-1 * x315 * x340) + (-1 * x338 * x315); const FLT x359 = x185 + (-1 * x66 * x355) + (x352 * x166) + (-1 * x340 * x191) + (-1 * x346 * x166) + (x9 * x354) + (x9 * x357) + (-1 * x338 * x190) + (x338 * x320) + (x320 * x340) + x311 + (x66 * x356) + (-1 * x340 * x190) + (-1 * x338 * x191); const FLT x360 = (x359 * x197) + (x358 * x189); const FLT x361 = (x209 * x353) + (-1 * x207 * (x360 + (x353 * x164))); const FLT x362 = x217 * x361; const FLT x363 = (x220 * x361) + (x213 * (x362 + (-1 * x215 * x361))); const FLT x364 = (x223 * x361) + (x213 * x363); const FLT x365 = (-1 * x228 * x353) + (x255 * x360); const FLT x366 = ((x258 * x358) + (-1 * x257 * x359)) * x259; const FLT x367 = (-1 * x366) + (x254 * x365); const FLT x368 = x366 + (-1 * x277 * (x365 + (-1 * x275 * x367) + (-1 * x273 * ((x243 * ((x271 * x361) + (x265 * x361) + (x213 * x364) + (x213 * (x364 + (x266 * x361) + (x213 * (x363 + (x213 * (x362 + (-1 * x269 * x361) + (x267 * x361))) + (x270 * x361))))))) + (-1 * x264 * x367))) + (x248 * x364) + (x251 * x361))); const FLT x369 = 1. / x31; const FLT x370 = 2 * x369; const FLT x371 = x26 * x370; const FLT x372 = x3 * x371; const FLT x373 = x7 * x372; const FLT x374 = x12 * x371; const FLT x375 = x9 * x374; const FLT x376 = -1 * x375; const FLT x377 = x376 + x373; const FLT x378 = x29 * x370; const FLT x379 = x28 * x370; const FLT x380 = -1 + x379; const FLT x381 = x380 + x378; const FLT x382 = x9 * x372; const FLT x383 = -1 * x382; const FLT x384 = x7 * x374; const FLT x385 = -1 * x384; const FLT x386 = x385 + x383; const FLT x387 = (x386 * x197) + (x381 * x189); const FLT x388 = (x209 * x377) + (-1 * x207 * (x387 + (x377 * x164))); const FLT x389 = x217 * x388; const FLT x390 = (x220 * x388) + (x213 * (x389 + (-1 * x215 * x388))); const FLT x391 = (x223 * x388) + (x213 * x390); const FLT x392 = (-1 * x228 * x377) + (x255 * x387); const FLT x393 = ((x258 * x381) + (-1 * x257 * x386)) * x259; const FLT x394 = (-1 * x393) + (x254 * x392); const FLT x395 = x393 + (-1 * x277 * (x392 + (-1 * x275 * x394) + (x251 * x388) + (x248 * x391) + (-1 * x273 * ((x243 * ((x213 * x391) + (x265 * x388) + (x213 * (x391 + (x266 * x388) + (x213 * ((x213 * ((-1 * x269 * x388) + x389 + (x267 * x388))) + x390 + (x270 * x388))))) + (x271 * x388))) + (-1 * x264 * x394))))); const FLT x396 = x30 * x370; const FLT x397 = x380 + x396; const FLT x398 = -1 * x373; const FLT x399 = x398 + x376; const FLT x400 = x7 * x9; const FLT x401 = x400 * x371; const FLT x402 = x12 * x372; const FLT x403 = -1 * x402; const FLT x404 = x403 + x401; const FLT x405 = (x404 * x197) + (x399 * x189); const FLT x406 = (x209 * x397) + (-1 * x207 * (x405 + (x397 * x164))); const FLT x407 = (-1 * x228 * x397) + (x405 * x255); const FLT x408 = ((x258 * x399) + (-1 * x404 * x257)) * x259; const FLT x409 = (-1 * x408) + (x407 * x254); const FLT x410 = x406 * x217; const FLT x411 = (x406 * x220) + (x213 * (x410 + (-1 * x406 * x215))); const FLT x412 = (x406 * x223) + (x411 * x213); const FLT x413 = x408 + (-1 * x277 * (x407 + (-1 * x409 * x275) + (x406 * x251) + (x412 * x248) + (-1 * x273 * ((x243 * ((x412 * x213) + (x406 * x271) + (x406 * x265) + (x213 * (x412 + (x406 * x266) + (x213 * ((x213 * ((-1 * x406 * x269) + x410 + (x406 * x267))) + x411 + (x406 * x270))))))) + (-1 * x409 * x264))))); const FLT x414 = x383 + x384; const FLT x415 = -1 + x378 + x396; const FLT x416 = (x415 * x197) + (x414 * x189); const FLT x417 = -1 * x401; const FLT x418 = x417 + x403; const FLT x419 = (-1 * x418 * x228) + (x416 * x255); const FLT x420 = ((x414 * x258) + (-1 * x415 * x257)) * x259; const FLT x421 = (-1 * x420) + (x419 * x254); const FLT x422 = (x418 * x209) + (-1 * x207 * (x416 + (x418 * x164))); const FLT x423 = x422 * x217; const FLT x424 = (x422 * x220) + (x213 * (x423 + (-1 * x422 * x215))); const FLT x425 = (x422 * x223) + (x424 * x213); const FLT x426 = x420 + (-1 * x277 * (x419 + (-1 * x421 * x275) + (x422 * x251) + (-1 * x273 * ((x243 * ((x425 * x213) + (x422 * x271) + (x422 * x265) + (x213 * (x425 + (x422 * x266) + (x213 * (x424 + (x213 * ((-1 * x422 * x269) + x423 + (x422 * x267))) + (x422 * x270))))))) + (-1 * x421 * x264))) + (x425 * x248))); const FLT x427 = x71 * x369; const FLT x428 = x26 * x427; const FLT x429 = x9 * x428; const FLT x430 = x12 * x429; const FLT x431 = x3 * x428; const FLT x432 = x7 * x431; const FLT x433 = (-1 * x432) + x430; const FLT x434 = -1 * x28 * x427; const FLT x435 = (-1 * x29 * x427) + x72; const FLT x436 = x435 + x434; const FLT x437 = x7 * x12 * x428; const FLT x438 = x3 * x429; const FLT x439 = x438 + x437; const FLT x440 = (x439 * x197) + (x436 * x189); const FLT x441 = (x433 * x209) + (-1 * x207 * (x440 + (x433 * x164))); const FLT x442 = (-1 * x433 * x228) + (x440 * x255); const FLT x443 = ((x436 * x258) + (-1 * x439 * x257)) * x259; const FLT x444 = (-1 * x443) + (x442 * x254); const FLT x445 = x441 * x217; const FLT x446 = (x441 * x220) + (x213 * (x445 + (-1 * x441 * x215))); const FLT x447 = (x441 * x223) + (x446 * x213); const FLT x448 = x443 + (-1 * x277 * (x442 + (-1 * x444 * x275) + (x447 * x248) + (x441 * x251) + (-1 * x273 * ((x243 * ((x447 * x213) + (x441 * x265) + (x213 * (x447 + (x441 * x266) + (x213 * (x446 + (x213 * ((-1 * x441 * x269) + x445 + (x441 * x267))) + (x441 * x270))))) + (x441 * x271))) + (-1 * x444 * x264))))); const FLT x449 = -1 * x30 * x427; const FLT x450 = x449 + x72 + x434; const FLT x451 = x430 + x432; const FLT x452 = x12 * x431; const FLT x453 = x400 * x428; const FLT x454 = (-1 * x453) + x452; const FLT x455 = (x454 * x197) + (x451 * x189); const FLT x456 = (x450 * x209) + (-1 * x207 * (x455 + (x450 * x164))); const FLT x457 = x456 * x217; const FLT x458 = (x456 * x220) + (x213 * (x457 + (-1 * x456 * x215))); const FLT x459 = (x456 * x223) + (x458 * x213); const FLT x460 = (-1 * x450 * x228) + (x455 * x255); const FLT x461 = ((x451 * x258) + (-1 * x454 * x257)) * x259; const FLT x462 = (-1 * x461) + (x460 * x254); const FLT x463 = x461 + (-1 * x277 * (x460 + (x459 * x248) + (-1 * x462 * x275) + (-1 * x273 * ((x243 * ((x459 * x213) + (x213 * (x459 + (x456 * x266) + (x213 * (x458 + (x213 * ((-1 * x456 * x269) + x457 + (x456 * x267))) + (x456 * x270))))) + (x456 * x271) + (x456 * x265))) + (-1 * x462 * x264))) + (x456 * x251))); const FLT x464 = x452 + x453; const FLT x465 = (-1 * x437) + x438; const FLT x466 = x435 + x449; const FLT x467 = (x466 * x197) + (x465 * x189); const FLT x468 = (x464 * x209) + (-1 * x207 * (x467 + (x464 * x164))); const FLT x469 = x468 * x217; const FLT x470 = (x468 * x220) + (x213 * (x469 + (-1 * x468 * x215))); const FLT x471 = (x468 * x223) + (x470 * x213); const FLT x472 = (-1 * x464 * x228) + (x467 * x255); const FLT x473 = ((x465 * x258) + (-1 * x466 * x257)) * x259; const FLT x474 = (-1 * x473) + (x472 * x254); const FLT x475 = x473 + (-1 * x277 * (x472 + (-1 * x474 * x275) + (-1 * x273 * ((x243 * ((x213 * (x471 + (x468 * x266) + (x213 * (x470 + (x213 * ((-1 * x468 * x269) + x469 + (x468 * x267))) + (x468 * x270))))) + (x468 * x265) + (x471 * x213) + (x468 * x271))) + (-1 * x474 * x264))) + (x471 * x248) + (x468 * x251))); const FLT x476 = 1.0 * x82; const FLT x477 = 1.0 * x77; const FLT x478 = 1.0 * (*_x0).Object.Pose.Rot[2]; const FLT x479 = 1.0 * x81; const FLT x480 = (x479 * (*_x0).Object.Pose.Rot[3]) + (-1 * x80 * x478) + (x476 * (*_x0).Object.Pose.Rot[0]) + (-1 * x477 * (*_x0).Object.Pose.Rot[1]); const FLT x481 = 1. / (x83 * sqrt(x83)); const FLT x482 = x481 * x113; const FLT x483 = x482 * x480; const FLT x484 = 1.0/2.0 * x483; const FLT x485 = 1.0/2.0 * x77; const FLT x486 = x481 * x485; const FLT x487 = x88 * x106; const FLT x488 = x487 * x480; const FLT x489 = x73 * x106; const FLT x490 = x489 * x480; const FLT x491 = 1.0/2.0 * x81; const FLT x492 = x491 * x481; const FLT x493 = x103 * x112; const FLT x494 = 1.0/2.0 * x80; const FLT x495 = x494 * x493; const FLT x496 = x481 * x480; const FLT x497 = x112 * x119; const FLT x498 = x73 * x107; const FLT x499 = x88 * x107; const FLT x500 = (-1 * x79 * x499) + (-1 * x75 * x497) + (-1 * x74 * x498) + (x78 * x121); const FLT x501 = x500 + (-1 * x495 * x496) + (x490 * x492) + (-1 * x82 * x484) + (-1 * x488 * x486); const FLT x502 = 2 * x132; const FLT x503 = x481 * x488; const FLT x504 = 1.0/2.0 * x82; const FLT x505 = x493 * x504; const FLT x506 = x74 * x499; const FLT x507 = x78 * x497; const FLT x508 = x79 * x498; const FLT x509 = x75 * x121; const FLT x510 = (-1 * x509) + (-1 * x508) + x506 + (-1 * x507); const FLT x511 = x510 + (x496 * x505) + (-1 * x491 * x503) + (-1 * x80 * x484) + (-1 * x490 * x486); const FLT x512 = x494 * x489; const FLT x513 = x74 * x497; const FLT x514 = x79 * x121; const FLT x515 = x78 * x499; const FLT x516 = x75 * x498; const FLT x517 = x516 + (-1 * x515) + (-1 * x513) + (-1 * x514); const FLT x518 = x517 + (x492 * x493 * x480) + (x496 * x512) + (x503 * x504) + (-1 * x483 * x485); const FLT x519 = x493 * x486; const FLT x520 = x75 * x499; const FLT x521 = x79 * x497; const FLT x522 = x78 * x498; const FLT x523 = x74 * x121; const FLT x524 = x522 + (-1 * x480 * x519) + (-1 * x521) + x523 + (x494 * x503) + x520 + (-1 * x491 * x483) + (-1 * x490 * x481 * x504); const FLT x525 = (x524 * sensor_pt[2]) + (-1 * x511 * sensor_pt[1]) + (x518 * sensor_pt[0]); const FLT x526 = 2 * x117; const FLT x527 = 2 * x135; const FLT x528 = 2 * ((x501 * sensor_pt[1]) + (-1 * x524 * sensor_pt[0]) + (x518 * sensor_pt[2])); const FLT x529 = (-1 * x528 * x127) + (x511 * x527) + (-1 * x501 * x502) + (x525 * x526); const FLT x530 = 2 * x131; const FLT x531 = 2 * x128; const FLT x532 = (x511 * sensor_pt[0]) + (-1 * x501 * sensor_pt[2]) + (x518 * sensor_pt[1]); const FLT x533 = 2 * x127; const FLT x534 = (x533 * x532) + (-1 * x524 * x527) + (x501 * x531) + (-1 * x525 * x530); const FLT x535 = (x524 * x502) + (-1 * x526 * x532) + (-1 * x511 * x531) + (x528 * x131); const FLT x536 = (-1 * x56 * x535) + (x68 * x529) + (x65 * x534); const FLT x537 = (-1 * x65 * x529) + (x67 * x535) + (x68 * x534); const FLT x538 = (x57 * x536) + x535 + (-1 * x537 * x166); const FLT x539 = (-1 * x67 * x534) + (x56 * x529) + (x68 * x535); const FLT x540 = (x539 * x166) + x534 + (-1 * x66 * x536); const FLT x541 = (x540 * x197) + (x538 * x189); const FLT x542 = (x66 * x537) + x529 + (-1 * x57 * x539); const FLT x543 = (-1 * x542 * x228) + (x541 * x255); const FLT x544 = ((x538 * x258) + (-1 * x540 * x257)) * x259; const FLT x545 = (-1 * x544) + (x543 * x254); const FLT x546 = (x542 * x209) + (-1 * x207 * (x541 + (x542 * x164))); const FLT x547 = x546 * x217; const FLT x548 = (x546 * x220) + (x213 * (x547 + (-1 * x546 * x215))); const FLT x549 = (x546 * x223) + (x548 * x213); const FLT x550 = x544 + (-1 * x277 * ((-1 * x545 * x275) + x543 + (x546 * x251) + (-1 * x273 * ((x243 * ((x546 * x271) + (x549 * x213) + (x546 * x265) + (x213 * (x549 + (x546 * x266) + (x213 * (x548 + (x213 * ((-1 * x546 * x269) + x547 + (x546 * x267))) + (x546 * x270))))))) + (-1 * x545 * x264))) + (x549 * x248))); const FLT x551 = 1.0 * x80; const FLT x552 = (-1 * x77 * x478) + (-1 * x476 * (*_x0).Object.Pose.Rot[3]) + (x479 * (*_x0).Object.Pose.Rot[0]) + (x551 * (*_x0).Object.Pose.Rot[1]); const FLT x553 = x481 * x552; const FLT x554 = x487 * x553; const FLT x555 = x489 * x553; const FLT x556 = x486 * x552; const FLT x557 = x482 * x552; const FLT x558 = x500 + (-1 * x491 * x557) + (-1 * x493 * x556) + (x494 * x554) + (-1 * x504 * x555); const FLT x559 = x491 * x553; const FLT x560 = 1.0/2.0 * x557; const FLT x561 = (-1 * x523) + x521 + (-1 * x522) + (-1 * x520); const FLT x562 = x561 + (-1 * x82 * x560) + (-1 * x495 * x553) + (-1 * x487 * x556) + (x489 * x559); const FLT x563 = x510 + (-1 * x77 * x560) + (x493 * x559) + (x504 * x554) + (x494 * x555); const FLT x564 = x514 + (-1 * x80 * x560) + (-1 * x491 * x554) + x513 + (x505 * x553) + (-1 * x516) + (-1 * x489 * x556) + x515; const FLT x565 = (x564 * sensor_pt[0]) + (-1 * x562 * sensor_pt[2]) + (x563 * sensor_pt[1]); const FLT x566 = (x558 * sensor_pt[2]) + (-1 * x564 * sensor_pt[1]) + (x563 * sensor_pt[0]); const FLT x567 = (x531 * x562) + (-1 * x527 * x558) + (-1 * x530 * x566) + (x533 * x565); const FLT x568 = (x562 * sensor_pt[1]) + (-1 * x558 * sensor_pt[0]) + (x563 * sensor_pt[2]); const FLT x569 = (-1 * x533 * x568) + (-1 * x502 * x562) + (x526 * x566) + (x527 * x564); const FLT x570 = (x530 * x568) + (-1 * x531 * x564) + (x502 * x558) + (-1 * x526 * x565); const FLT x571 = (-1 * x56 * x570) + (x65 * x567) + (x68 * x569); const FLT x572 = (-1 * x65 * x569) + (x68 * x567) + (x67 * x570); const FLT x573 = x570 + (x57 * x571) + (-1 * x572 * x166); const FLT x574 = (-1 * x67 * x567) + (x56 * x569) + (x68 * x570); const FLT x575 = x567 + (x574 * x166) + (-1 * x66 * x571); const FLT x576 = (x575 * x197) + (x573 * x189); const FLT x577 = x569 + (x66 * x572) + (-1 * x57 * x574); const FLT x578 = (-1 * x577 * x228) + (x576 * x255); const FLT x579 = ((x573 * x258) + (-1 * x575 * x257)) * x259; const FLT x580 = (-1 * x579) + (x578 * x254); const FLT x581 = (x577 * x209) + (-1 * x207 * (x576 + (x577 * x164))); const FLT x582 = x581 * x217; const FLT x583 = (x581 * x220) + (x213 * (x582 + (-1 * x581 * x215))); const FLT x584 = (x581 * x223) + (x583 * x213); const FLT x585 = x579 + (-1 * x277 * ((x581 * x251) + x578 + (-1 * x580 * x275) + (-1 * x273 * ((x243 * ((x584 * x213) + (x581 * x265) + (x581 * x271) + (x213 * (x584 + (x581 * x266) + (x213 * ((x213 * ((-1 * x581 * x269) + x582 + (x581 * x267))) + x583 + (x581 * x270))))))) + (-1 * x580 * x264))) + (x584 * x248))); const FLT x586 = (x82 * x478) + (-1 * x479 * (*_x0).Object.Pose.Rot[1]) + (-1 * x477 * (*_x0).Object.Pose.Rot[3]) + (x551 * (*_x0).Object.Pose.Rot[0]); const FLT x587 = x487 * x586; const FLT x588 = x481 * x587; const FLT x589 = x482 * x586; const FLT x590 = 1.0/2.0 * x589; const FLT x591 = x481 * x586; const FLT x592 = x493 * x591; const FLT x593 = (x592 * x504) + (-1 * x80 * x590) + (-1 * x489 * x486 * x586) + x500 + (-1 * x491 * x588); const FLT x594 = x491 * x591; const FLT x595 = (-1 * x494 * x592) + x507 + (-1 * x506) + (-1 * x82 * x590) + x508 + (-1 * x486 * x587) + (x489 * x594) + x509; const FLT x596 = 1.0/2.0 * x588; const FLT x597 = (x493 * x594) + x561 + (x591 * x512) + (-1 * x485 * x589) + (x82 * x596); const FLT x598 = (x593 * sensor_pt[0]) + (-1 * x595 * sensor_pt[2]) + (x597 * sensor_pt[1]); const FLT x599 = x517 + (-1 * x489 * x591 * x504) + (-1 * x491 * x589) + (-1 * x519 * x586) + (x80 * x596); const FLT x600 = 2 * ((x595 * sensor_pt[1]) + (x597 * sensor_pt[2]) + (-1 * x599 * sensor_pt[0])); const FLT x601 = (x600 * x131) + (-1 * x593 * x531) + (x599 * x502) + (-1 * x598 * x526); const FLT x602 = (x599 * sensor_pt[2]) + (-1 * x593 * sensor_pt[1]) + (x597 * sensor_pt[0]); const FLT x603 = (x598 * x533) + (x595 * x531) + (-1 * x599 * x527) + (-1 * x602 * x530); const FLT x604 = (x602 * x526) + (x593 * x527) + (-1 * x595 * x502) + (-1 * x600 * x127); const FLT x605 = (-1 * x65 * x604) + (x67 * x601) + (x68 * x603); const FLT x606 = (-1 * x67 * x603) + (x56 * x604) + (x68 * x601); const FLT x607 = (x66 * x605) + x604 + (-1 * x57 * x606); const FLT x608 = (-1 * x56 * x601) + (x65 * x603) + (x68 * x604); const FLT x609 = x601 + (x57 * x608) + (-1 * x605 * x166); const FLT x610 = x603 + (x606 * x166) + (-1 * x66 * x608); const FLT x611 = (x610 * x197) + (x609 * x189); const FLT x612 = (x607 * x209) + (-1 * x207 * (x611 + (x607 * x164))); const FLT x613 = x612 * x217; const FLT x614 = (x612 * x220) + (x213 * (x613 + (-1 * x612 * x215))); const FLT x615 = (x612 * x223) + (x614 * x213); const FLT x616 = (-1 * x607 * x228) + (x611 * x255); const FLT x617 = ((x609 * x258) + (-1 * x610 * x257)) * x259; const FLT x618 = (-1 * x617) + (x616 * x254); const FLT x619 = x617 + (-1 * x277 * (x616 + (-1 * x618 * x275) + (-1 * x273 * ((x243 * ((x615 * x213) + (x612 * x271) + (x213 * (x615 + (x612 * x266) + (x213 * (x614 + (x213 * ((-1 * x612 * x269) + x613 + (x612 * x267))) + (x612 * x270))))) + (x612 * x265))) + (-1 * x618 * x264))) + (x612 * x251) + (x615 * x248))); const FLT x620 = x398 + x375; const FLT x621 = -1 * x378; const FLT x622 = 1 + (-1 * x379); const FLT x623 = x622 + x621; const FLT x624 = x382 + x384; const FLT x625 = (x624 * x197) + (x623 * x189); const FLT x626 = (x620 * x209) + (-1 * x207 * (x625 + (x620 * x164))); const FLT x627 = x626 * x217; const FLT x628 = (x626 * x220) + (x213 * (x627 + (-1 * x626 * x215))); const FLT x629 = (x626 * x223) + (x628 * x213); const FLT x630 = (-1 * x620 * x228) + (x625 * x255); const FLT x631 = ((x623 * x258) + (-1 * x624 * x257)) * x259; const FLT x632 = (-1 * x631) + (x630 * x254); const FLT x633 = x626 * x212; const FLT x634 = x631 + (-1 * x277 * ((x633 * x236 * x250 * x246) + (x629 * x248) + x630 + (-1 * x632 * x275) + (-1 * x273 * ((x243 * ((x626 * x271) + (x629 * x213) + (x626 * x265) + (x213 * (x629 + (x626 * x266) + (x213 * (x628 + (x213 * ((-1 * x633 * x268) + x627 + (x626 * x267))) + (x626 * x270))))))) + (-1 * x632 * x264))))); const FLT x635 = x375 + x373; const FLT x636 = x417 + x402; const FLT x637 = (x636 * x197) + (x635 * x189); const FLT x638 = -1 * x396; const FLT x639 = x622 + x638; const FLT x640 = (-1 * x639 * x228) + (x637 * x255); const FLT x641 = ((x635 * x258) + (-1 * x636 * x257)) * x259; const FLT x642 = (-1 * x641) + (x640 * x254); const FLT x643 = (x639 * x209) + (-1 * x207 * (x637 + (x639 * x164))); const FLT x644 = x643 * x217; const FLT x645 = (x643 * x220) + (x213 * (x644 + (-1 * x643 * x215))); const FLT x646 = (x643 * x223) + (x645 * x213); const FLT x647 = x641 + (-1 * x277 * (x640 + (x643 * x251) + (-1 * x642 * x275) + (-1 * x273 * ((x243 * ((x646 * x213) + (x643 * x271) + (x643 * x265) + (x213 * ((x643 * x266) + x646 + (x213 * (x645 + (x213 * ((-1 * x643 * x269) + x644 + (x643 * x267))) + (x643 * x270))))))) + (-1 * x642 * x264))) + (x646 * x248))); const FLT x648 = x402 + x401; const FLT x649 = x385 + x382; const FLT x650 = 1 + x638 + x621; const FLT x651 = (x650 * x197) + (x649 * x189); const FLT x652 = (x648 * x209) + (-1 * x207 * (x651 + (x648 * x164))); const FLT x653 = x652 * x217; const FLT x654 = (x652 * x220) + (x213 * (x653 + (-1 * x652 * x215))); const FLT x655 = (x652 * x223) + (x654 * x213); const FLT x656 = (-1 * x648 * x228) + (x651 * x255); const FLT x657 = ((x649 * x258) + (-1 * x650 * x257)) * x259; const FLT x658 = (-1 * x657) + (x656 * x254); const FLT x659 = x657 + (-1 * x277 * (x656 + (-1 * x658 * x275) + (-1 * x273 * ((x243 * ((x655 * x213) + (x652 * x271) + (x652 * x265) + (x213 * (x655 + (x652 * x266) + (x213 * ((x213 * ((-1 * x652 * x269) + x653 + (x652 * x267))) + x654 + (x652 * x270))))))) + (-1 * x658 * x264))) + (x655 * x248) + (x652 * x251))); const FLT x660 = dt * dt * dt; const FLT x661 = 0.5 * x99 * x660; const FLT x662 = x89 * x661; const FLT x663 = x88 * x88 * x88; const FLT x664 = dt * dt * dt * dt; const FLT x665 = 2 * x98 * (1. / (x94 * x94)); const FLT x666 = x664 * x665; const FLT x667 = 1.0 * x101; const FLT x668 = x97 * (1. / (x94 * sqrt(x94))); const FLT x669 = x668 * x667; const FLT x670 = x669 * x664; const FLT x671 = x88 * x670; const FLT x672 = x88 * x666; const FLT x673 = 2 * x100; const FLT x674 = x85 * x673; const FLT x675 = x667 * x104; const FLT x676 = x88 * x85; const FLT x677 = (-1 * x675 * x676) + (x92 * x671) + (x86 * x671) + (x670 * x663) + (-1 * x86 * x672) + (-1 * x663 * x666) + (-1 * x92 * x672) + (x88 * x674); const FLT x678 = x677 * x112; const FLT x679 = 1. / (x102 * sqrt(x102)); const FLT x680 = 1.0/2.0 * x109; const FLT x681 = x679 * x680; const FLT x682 = x84 * x485; const FLT x683 = x679 * x682; const FLT x684 = x73 * x105; const FLT x685 = x677 * x684; const FLT x686 = x88 * x105; const FLT x687 = x84 * x491; const FLT x688 = x679 * x687; const FLT x689 = x677 * x688; const FLT x690 = 0.5 * x104; const FLT x691 = x690 * x676; const FLT x692 = x679 * x101; const FLT x693 = x692 * x677; const FLT x694 = 1.0/2.0 * x114; const FLT x695 = x668 * x660; const FLT x696 = x89 * x695; const FLT x697 = x661 * x122; const FLT x698 = x88 * x73; const FLT x699 = x88 * x695; const FLT x700 = x73 * x699; const FLT x701 = (-1 * x700 * x129) + (x698 * x697); const FLT x702 = x91 * x699; const FLT x703 = x88 * x91; const FLT x704 = x661 * x126; const FLT x705 = (-1 * x704 * x703) + (x702 * x110); const FLT x706 = x701 + (-1 * x696 * x120) + x705 + (x662 * x130) + (x678 * x681) + (-1 * x694 * x693) + x116 + (-1 * x691 * x125) + (-1 * x683 * x685) + (-1 * x689 * x686); const FLT x707 = -1 * x118; const FLT x708 = x700 * x110; const FLT x709 = x698 * x704; const FLT x710 = x679 * x686; const FLT x711 = x694 * x710; const FLT x712 = (x697 * x703) + (-1 * x702 * x129); const FLT x713 = x712 + x709 + (x696 * x125) + (-1 * x708) + (-1 * x693 * x687) + (-1 * x691 * x120) + (-1 * x681 * x685) + (x677 * x711) + x707 + (-1 * x678 * x683) + (-1 * x662 * x115); const FLT x714 = x702 * x120; const FLT x715 = x694 * x679; const FLT x716 = x661 * x130; const FLT x717 = x716 * x703; const FLT x718 = x680 * x710; const FLT x719 = -1 * x123; const FLT x720 = x661 * x115; const FLT x721 = (x700 * x125) + (-1 * x698 * x720); const FLT x722 = x721 + (-1 * x693 * x682) + (x677 * x718) + x719 + (-1 * x662 * x126) + (x696 * x110) + (-1 * x691 * x129) + x714 + (x685 * x715) + (x678 * x688) + (-1 * x717); const FLT x723 = x698 * x716; const FLT x724 = x703 * x720; const FLT x725 = x702 * x125; const FLT x726 = x682 * x710; const FLT x727 = x700 * x120; const FLT x728 = (-1 * x696 * x129) + (x89 * x697) + (-1 * x691 * x110) + x724 + (-1 * x678 * x715) + x108 + (-1 * x693 * x680) + (-1 * x723) + (x689 * x684) + (-1 * x725) + (-1 * x677 * x726) + x727; const FLT x729 = (x728 * sensor_pt[1]) + (-1 * x713 * sensor_pt[0]) + (x722 * sensor_pt[2]); const FLT x730 = (x706 * sensor_pt[0]) + (-1 * x728 * sensor_pt[2]) + (x722 * sensor_pt[1]); const FLT x731 = (x713 * x502) + (-1 * x730 * x526) + (-1 * x706 * x531) + (x729 * x530); const FLT x732 = (x713 * sensor_pt[2]) + (-1 * x706 * sensor_pt[1]) + (x722 * sensor_pt[0]); const FLT x733 = (x728 * x531) + (-1 * x713 * x527) + (x730 * x533) + (-1 * x732 * x530); const FLT x734 = (x732 * x526) + (-1 * x729 * x533) + (-1 * x728 * x502) + (x706 * x527); const FLT x735 = (-1 * x65 * x734) + (x67 * x731) + (x68 * x733); const FLT x736 = (-1 * x67 * x733) + (x56 * x734) + (x68 * x731); const FLT x737 = (x66 * x735) + x734 + (-1 * x57 * x736); const FLT x738 = (-1 * x56 * x731) + (x65 * x733) + (x68 * x734); const FLT x739 = x731 + (x57 * x738) + (-1 * x735 * x166); const FLT x740 = x733 + (x736 * x166) + (-1 * x66 * x738); const FLT x741 = (x740 * x197) + (x739 * x189); const FLT x742 = (x737 * x209) + (-1 * x207 * (x741 + (x737 * x164))); const FLT x743 = (-1 * x737 * x228) + (x741 * x255); const FLT x744 = ((x739 * x258) + (-1 * x740 * x257)) * x259; const FLT x745 = (-1 * x744) + (x743 * x254); const FLT x746 = x742 * x217; const FLT x747 = (x742 * x220) + (x213 * (x746 + (-1 * x742 * x215))); const FLT x748 = (x742 * x223) + (x747 * x213); const FLT x749 = x744 + (-1 * x277 * (x743 + (-1 * x745 * x275) + (x742 * x251) + (x748 * x248) + (-1 * x273 * ((x243 * ((x748 * x213) + (x742 * x271) + (x742 * x265) + (x213 * ((x742 * x266) + x748 + (x213 * (x747 + (x213 * ((-1 * x742 * x269) + x746 + (x742 * x267))) + (x742 * x270))))))) + (-1 * x745 * x264))))); const FLT x750 = x92 * x661; const FLT x751 = x92 * x695; const FLT x752 = x89 * x666; const FLT x753 = x89 * x670; const FLT x754 = x85 * x111; const FLT x755 = x86 * x91; const FLT x756 = (x91 * x91 * x91) * x664; const FLT x757 = (x91 * x674) + (x669 * x756) + (-1 * x665 * x756) + (x91 * x753) + (-1 * x91 * x752) + (-1 * x667 * x754) + (-1 * x666 * x755) + (x670 * x755); const FLT x758 = x688 * x757; const FLT x759 = x684 * x757; const FLT x760 = x757 * x112; const FLT x761 = x692 * x757; const FLT x762 = 1.0/2.0 * x761; const FLT x763 = 0.5 * x754; const FLT x764 = x73 * x91; const FLT x765 = x695 * x764; const FLT x766 = (-1 * x765 * x129) + (x697 * x764); const FLT x767 = x766 + (x681 * x760) + (-1 * x683 * x759) + (-1 * x762 * x114) + x719 + (-1 * x750 * x126) + (x751 * x110) + (-1 * x763 * x125) + (-1 * x714) + (-1 * x686 * x758) + x717; const FLT x768 = x765 * x110; const FLT x769 = x764 * x704; const FLT x770 = (x711 * x757) + x725 + (-1 * x768) + x108 + (-1 * x683 * x760) + (-1 * x751 * x129) + (-1 * x724) + (-1 * x687 * x761) + (-1 * x681 * x759) + (x92 * x697) + (-1 * x763 * x120) + x769; const FLT x771 = -1 * x116; const FLT x772 = x84 * x762; const FLT x773 = x765 * x125; const FLT x774 = x764 * x720; const FLT x775 = x705 + (-1 * x763 * x129) + (-1 * x750 * x130) + (x758 * x112) + (x715 * x759) + x771 + (-1 * x774) + (-1 * x77 * x772) + (x751 * x120) + (x718 * x757) + x773; const FLT x776 = (-1 * x764 * x716) + (x765 * x120); const FLT x777 = x712 + (-1 * x763 * x110) + (-1 * x82 * x772) + x118 + (-1 * x726 * x757) + (x684 * x758) + (x750 * x115) + x776 + (-1 * x760 * x715) + (-1 * x751 * x125); const FLT x778 = (-1 * x770 * sensor_pt[0]) + (x777 * sensor_pt[1]) + (x775 * sensor_pt[2]); const FLT x779 = (x767 * sensor_pt[0]) + (-1 * x777 * sensor_pt[2]) + (x775 * sensor_pt[1]); const FLT x780 = (x770 * x502) + (-1 * x767 * x531) + (-1 * x779 * x526) + (x778 * x530); const FLT x781 = (x770 * sensor_pt[2]) + (-1 * x767 * sensor_pt[1]) + (x775 * sensor_pt[0]); const FLT x782 = (x781 * x526) + (-1 * x778 * x533) + (-1 * x777 * x502) + (x767 * x527); const FLT x783 = (x779 * x533) + (-1 * x781 * x530) + (-1 * x770 * x527) + (x777 * x531); const FLT x784 = (-1 * x67 * x783) + (x68 * x780) + (x56 * x782); const FLT x785 = (-1 * x65 * x782) + (x67 * x780) + (x68 * x783); const FLT x786 = (-1 * x57 * x784) + x782 + (x66 * x785); const FLT x787 = (-1 * x56 * x780) + (x65 * x783) + (x68 * x782); const FLT x788 = x780 + (x57 * x787) + (-1 * x785 * x166); const FLT x789 = x783 + (x784 * x166) + (-1 * x66 * x787); const FLT x790 = (x789 * x197) + (x788 * x189); const FLT x791 = (x786 * x209) + (-1 * x207 * (x790 + (x786 * x164))); const FLT x792 = x791 * x217; const FLT x793 = (x791 * x220) + (x213 * (x792 + (-1 * x791 * x215))); const FLT x794 = (x791 * x223) + (x793 * x213); const FLT x795 = (-1 * x786 * x228) + (x790 * x255); const FLT x796 = ((x788 * x258) + (-1 * x789 * x257)) * x259; const FLT x797 = (-1 * x796) + (x795 * x254); const FLT x798 = x796 + (-1 * x277 * ((-1 * x797 * x275) + (-1 * x273 * ((x243 * ((x794 * x213) + (x791 * x265) + (x213 * (x794 + (x791 * x266) + (x213 * (x793 + (x213 * ((-1 * x791 * x269) + x792 + (x791 * x267))) + (x791 * x270))))) + (x791 * x271))) + (-1 * x797 * x264))) + (x794 * x248) + x795 + (x791 * x251))); const FLT x799 = x85 * x73; const FLT x800 = x73 * x92; const FLT x801 = x73 * x73 * x73; const FLT x802 = (x670 * x801) + (-1 * x675 * x799) + (-1 * x666 * x801) + (x73 * x753) + (x673 * x799) + (x670 * x800) + (-1 * x73 * x752) + (-1 * x666 * x800); const FLT x803 = x684 * x802; const FLT x804 = x86 * x695; const FLT x805 = x802 * x112; const FLT x806 = x690 * x799; const FLT x807 = x692 * x802; const FLT x808 = 1.0/2.0 * x807; const FLT x809 = x688 * x802; const FLT x810 = (-1 * x686 * x809) + (-1 * x808 * x114) + x108 + (-1 * x683 * x803) + (-1 * x727) + x723 + (-1 * x769) + x768 + (x681 * x805) + (x86 * x697) + (-1 * x804 * x129) + (-1 * x806 * x125); const FLT x811 = x86 * x661; const FLT x812 = x701 + x774 + (-1 * x715 * x805) + (-1 * x808 * x109) + x771 + (x684 * x809) + (-1 * x811 * x130) + (-1 * x773) + (x804 * x120) + (-1 * x726 * x802) + (-1 * x806 * x110); const FLT x813 = x776 + (x809 * x112) + (x804 * x125) + (x718 * x802) + (-1 * x811 * x115) + x707 + (-1 * x682 * x807) + (-1 * x806 * x129) + (-1 * x709) + (x715 * x803) + x708; const FLT x814 = (x810 * sensor_pt[0]) + (-1 * x812 * sensor_pt[2]) + (x813 * sensor_pt[1]); const FLT x815 = x721 + (-1 * x687 * x807) + x766 + x123 + (-1 * x683 * x805) + (-1 * x806 * x120) + (x811 * x126) + (-1 * x804 * x110) + (x711 * x802) + (-1 * x681 * x803); const FLT x816 = (x813 * sensor_pt[2]) + (x812 * sensor_pt[1]) + (-1 * x815 * sensor_pt[0]); const FLT x817 = (x530 * x816) + (x502 * x815) + (-1 * x531 * x810) + (-1 * x526 * x814); const FLT x818 = (-1 * x810 * sensor_pt[1]) + (x815 * sensor_pt[2]) + (x813 * sensor_pt[0]); const FLT x819 = (-1 * x530 * x818) + (x533 * x814) + (-1 * x527 * x815) + (x531 * x812); const FLT x820 = (x526 * x818) + (-1 * x533 * x816) + (-1 * x502 * x812) + (x527 * x810); const FLT x821 = (x67 * x817) + (-1 * x65 * x820) + (x68 * x819); const FLT x822 = (-1 * x67 * x819) + (x56 * x820) + (x68 * x817); const FLT x823 = x820 + (x66 * x821) + (-1 * x57 * x822); const FLT x824 = (-1 * x56 * x817) + (x65 * x819) + (x68 * x820); const FLT x825 = x817 + (x57 * x824) + (-1 * x821 * x166); const FLT x826 = x819 + (x822 * x166) + (-1 * x66 * x824); const FLT x827 = (x826 * x197) + (x825 * x189); const FLT x828 = (x823 * x209) + (-1 * x207 * (x827 + (x823 * x164))); const FLT x829 = x828 * x217; const FLT x830 = (x828 * x220) + (x213 * (x829 + (-1 * x828 * x215))); const FLT x831 = (x828 * x223) + (x830 * x213); const FLT x832 = (-1 * x823 * x228) + (x827 * x255); const FLT x833 = ((x825 * x258) + (-1 * x826 * x257)) * x259; const FLT x834 = x262 * ((-1 * x833) + (x832 * x254)); const FLT x835 = x833 + (-1 * x277 * ((-1 * x834 * x274) + (x828 * x251) + x832 + (-1 * x273 * ((x243 * ((x213 * (x831 + (x828 * x266) + (x213 * (x830 + (x213 * ((-1 * x828 * x269) + (x828 * x267) + x829)) + (x828 * x270))))) + (x831 * x213) + (x828 * x265) + (x828 * x271))) + (-1 * x834 * x263))) + (x831 * x248))); const FLT x836 = -1 * dt * x378; const FLT x837 = (-1 * dt * x379) + dt; const FLT x838 = x837 + x836; const FLT x839 = dt * x384; const FLT x840 = dt * x382; const FLT x841 = x840 + x839; const FLT x842 = (x841 * x197) + (x838 * x189); const FLT x843 = dt * x375; const FLT x844 = dt * x373; const FLT x845 = (-1 * x844) + x843; const FLT x846 = (-1 * x845 * x228) + (x842 * x255); const FLT x847 = ((x838 * x258) + (-1 * x841 * x257)) * x259; const FLT x848 = (-1 * x847) + (x846 * x254); const FLT x849 = (x845 * x209) + (-1 * x207 * (x842 + (x845 * x164))); const FLT x850 = x849 * x217; const FLT x851 = (x849 * x220) + (x213 * (x850 + (-1 * x849 * x215))); const FLT x852 = (x849 * x223) + (x851 * x213); const FLT x853 = x847 + (-1 * x277 * (x846 + (-1 * x848 * x275) + (x849 * x251) + (-1 * x273 * ((x243 * ((x852 * x213) + (x849 * x271) + (x213 * (x852 + (x849 * x266) + (x213 * (x851 + (x213 * (x850 + (-1 * x849 * x269) + (x849 * x267))) + (x849 * x270))))) + (x849 * x265))) + (-1 * x848 * x264))) + (x852 * x248))); const FLT x854 = -1 * dt * x396; const FLT x855 = x837 + x854; const FLT x856 = x843 + x844; const FLT x857 = dt * x402; const FLT x858 = dt * x401; const FLT x859 = (-1 * x858) + x857; const FLT x860 = (x859 * x197) + (x856 * x189); const FLT x861 = (x855 * x209) + (-1 * x207 * (x860 + (x855 * x164))); const FLT x862 = (-1 * x855 * x228) + (x860 * x255); const FLT x863 = ((x856 * x258) + (-1 * x859 * x257)) * x259; const FLT x864 = (-1 * x863) + (x862 * x254); const FLT x865 = x861 * x217; const FLT x866 = (x861 * x220) + (x213 * (x865 + (-1 * x861 * x215))); const FLT x867 = (x861 * x223) + (x866 * x213); const FLT x868 = x863 + (-1 * x277 * ((-1 * x864 * x275) + (x861 * x251) + x862 + (x867 * x248) + (-1 * x273 * ((x243 * ((x867 * x213) + (x861 * x265) + (x213 * (x867 + (x861 * x266) + (x213 * (x866 + (x213 * ((-1 * x861 * x269) + x865 + (x861 * x267))) + (x861 * x270))))) + (x861 * x271))) + (-1 * x864 * x264))))); const FLT x869 = x857 + x858; const FLT x870 = (-1 * x839) + x840; const FLT x871 = x836 + x854 + dt; const FLT x872 = (x871 * x197) + (x870 * x189); const FLT x873 = (x869 * x209) + (-1 * x207 * (x872 + (x869 * x164))); const FLT x874 = (-1 * x869 * x228) + (x872 * x255); const FLT x875 = ((x870 * x258) + (-1 * x871 * x257)) * x259; const FLT x876 = (-1 * x875) + (x874 * x254); const FLT x877 = x873 * x217; const FLT x878 = (x873 * x220) + (x213 * (x877 + (-1 * x873 * x215))); const FLT x879 = (x873 * x223) + (x878 * x213); const FLT x880 = x875 + (-1 * x277 * ((-1 * x876 * x275) + (x873 * x251) + x874 + (x879 * x248) + (-1 * x273 * ((x243 * ((x873 * x271) + (x213 * (x879 + (x873 * x266) + (x213 * (x878 + (x213 * ((-1 * x873 * x269) + x877 + (x873 * x267))) + (x873 * x270))))) + (x879 * x213) + (x873 * x265))) + (-1 * x876 * x264))))); const FLT x881 = x272 * x244; const FLT x882 = ((-1 * x881) + x274) * x277; const FLT x883 = ((x881 * x231) + (-1 * x231 * x274)) * x277; const FLT x884 = x277 * ((-1 * x881 * x262) + x275); const FLT x885 = x226 * x163 * (1 + x253); const FLT x886 = x885 * x254; const FLT x887 = x211 * x208; const FLT x888 = x887 * x242 * x163; const FLT x889 = -1 * x888 * x217; const FLT x890 = (-1 * x888 * x220) + (x213 * (x889 + (x888 * x215))); const FLT x891 = (-1 * x888 * x223) + (x890 * x213); const FLT x892 = x277 * ((x891 * x248) + x885 + (-1 * x886 * x275) + (-1 * x887 * x243 * x249 * x164) + (-1 * x273 * ((-1 * x233 * x200 * x241) + (x243 * ((x891 * x213) + (-1 * x888 * x271) + (-1 * x888 * x265) + (x213 * (x891 + (-1 * x888 * x266) + (x213 * (x890 + (x213 * ((x888 * x269) + x889 + (-1 * x888 * x267))) + (-1 * x888 * x270))))))) + x242 + (-1 * x886 * x264)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x278 + (x278 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x331 + (x280 * x331)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x368 + (x280 * x368)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x395 + (x280 * x395)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x413 + (x413 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x426 + (x426 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x448 + (x448 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x463 + (x463 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x475 + (x475 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x550 + (x550 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x585 + (x585 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x619 + (x619 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x634 + (x634 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x647 + (x647 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x659 + (x659 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x749 + (x749 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x798 + (x798 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x835 + (x835 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x853 + (x853 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x868 + (x868 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x880 + (x880 * x280)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), (-1 * x882 * x280) + (-1 * x882)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), sin(x279)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), x280); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), (-1 * x883 * x280) + (-1 * x883)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), (-1 * x884 * x280) + (-1 * x884)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), (-1 * x892 * x280) + (-1 * x892)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt); } } // Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3]; const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[2]; const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1]; const FLT x3 = (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[1]); const FLT x4 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1]; const FLT x5 = dt * dt; const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x4 * x4) * x5; const FLT x11 = 1e-10 + x7 + x10 + x9; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[0]; const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2]; const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x3 * x3) + (x16 * x16) + (x18 * x18) + (x17 * x17))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x3 * x23; const FLT x25 = x20 * x21; const FLT x26 = x25 * x17; const FLT x27 = x6 * x22; const FLT x28 = x27 * x18; const FLT x29 = x8 * x22; const FLT x30 = x29 * x16; const FLT x31 = x28 + (-1 * x30) + x24 + x26; const FLT x32 = (x3 * x25) + (-1 * x27 * x16) + (-1 * x29 * x18) + (-1 * x23 * x17); const FLT x33 = x3 * x27; const FLT x34 = x23 * x18; const FLT x35 = x25 * x16; const FLT x36 = x29 * x17; const FLT x37 = x36 + x35 + x33 + (-1 * x34); const FLT x38 = (-1 * x37 * sensor_pt[1]) + (x32 * sensor_pt[0]) + (x31 * sensor_pt[2]); const FLT x39 = x27 * x17; const FLT x40 = x23 * x16; const FLT x41 = x25 * x18; const FLT x42 = x3 * x29; const FLT x43 = x42 + x41 + (-1 * x39) + x40; const FLT x44 = (-1 * x43 * sensor_pt[2]) + (x32 * sensor_pt[1]) + (x37 * sensor_pt[0]); const FLT x45 = 1.0/2.0 * dt * fabs(dt); const FLT x46 = (*_x0).Object.Pose.Pos[2] + (2 * ((x43 * x44) + (-1 * x31 * x38))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])); const FLT x47 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2]; const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1]; const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0]; const FLT x50 = (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x47 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*_x0).Lighthouse.Rot[2]); const FLT x51 = 0.5 * (*_x0).Lighthouse.Rot[0]; const FLT x52 = 0.5 * (*_x0).Lighthouse.Rot[1]; const FLT x53 = (x52 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x51 * (*error_model).Lighthouse.AxisAngleRot[2]) + (*_x0).Lighthouse.Rot[3] + (-1 * x49 * (*_x0).Lighthouse.Rot[2]); const FLT x54 = x53 * x53; const FLT x55 = x50 * x50; const FLT x56 = (x51 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x52 * (*error_model).Lighthouse.AxisAngleRot[2]) + (*_x0).Lighthouse.Rot[2]; const FLT x57 = x56 * x56; const FLT x58 = (*_x0).Lighthouse.Rot[1] + (-1 * x48 * (*_x0).Lighthouse.Rot[3]) + (x51 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x47 * (*_x0).Lighthouse.Rot[2]); const FLT x59 = x58 * x58; const FLT x60 = x59 + x57 + x54 + x55; const FLT x61 = 1. / x60; const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x55) + (x61 * x54) + (x61 * x57))) * (1. / sqrt(x60)); const FLT x63 = x62 * x50; const FLT x64 = (-1 * x31 * sensor_pt[0]) + (x32 * sensor_pt[2]) + (x43 * sensor_pt[1]); const FLT x65 = (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (*_x0).Object.Pose.Pos[1] + (2 * ((x38 * x37) + (-1 * x64 * x43))); const FLT x66 = x62 * x58; const FLT x67 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x64 * x31) + (-1 * x44 * x37))) + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0]; const FLT x68 = x62 * x56; const FLT x69 = (x67 * x68) + (x63 * x46) + (-1 * x65 * x66); const FLT x70 = x62 * x53; const FLT x71 = (x63 * x67) + (x70 * x65) + (-1 * x68 * x46); const FLT x72 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2]; const FLT x73 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1]; const FLT x74 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0]; const FLT x75 = (x74 * x68) + (x72 * x63) + (-1 * x73 * x66); const FLT x76 = (x70 * x73) + (-1 * x72 * x68) + (x74 * x63); const FLT x77 = x65 + (2 * ((-1 * x71 * x70) + (x66 * x69))) + (-1 * (x73 + (2 * ((-1 * x70 * x76) + (x75 * x66))))); const FLT x78 = x77 * x77; const FLT x79 = (x72 * x66) + (x73 * x63) + (-1 * x70 * x74); const FLT x80 = (x66 * x46) + (x63 * x65) + (-1 * x70 * x67); const FLT x81 = x46 + (-1 * (x72 + (2 * ((-1 * x79 * x66) + (x76 * x68))))) + (2 * ((-1 * x80 * x66) + (x71 * x68))); const FLT x82 = x67 + (-1 * (x74 + (2 * ((-1 * x75 * x68) + (x70 * x79))))) + (2 * ((-1 * x68 * x69) + (x80 * x70))); const FLT x83 = x82 * x82; const FLT x84 = x83 + (x81 * x81); const FLT x85 = x84 + x78; const FLT x86 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt); const FLT x87 = cos(x86); const FLT x88 = 1. / x87; const FLT x89 = x88 * (1. / sqrt(x85)); const FLT x90 = asin(x89 * x77); const FLT x91 = 8.0108022e-06 * x90; const FLT x92 = -8.0108022e-06 + (-1 * x91); const FLT x93 = 0.0028679863 + (x92 * x90); const FLT x94 = 5.3685255e-06 + (x93 * x90); const FLT x95 = 0.0076069798 + (x90 * x94); const FLT x96 = x90 * x95; const FLT x97 = -8.0108022e-06 + (-1.60216044e-05 * x90); const FLT x98 = x93 + (x90 * x97); const FLT x99 = x94 + (x90 * x98); const FLT x100 = x95 + (x90 * x99); const FLT x101 = (x90 * x100) + x96; const FLT x102 = atan2(-1 * x81, x82); const FLT x103 = tan(x86); const FLT x104 = (1. / sqrt(x84)) * x103; const FLT x105 = -1 * x77 * x104; const FLT x106 = asin(x105) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * x102); const FLT x107 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag; const FLT x108 = (*_x0).BSD1.curve + (-1 * x107 * sin(x106)) + (*error_model).BSD1.curve; const FLT x109 = sin(x86); const FLT x110 = x109 * x108; const FLT x111 = x87 + (x101 * x110); const FLT x112 = 1. / x111; const FLT x113 = x90 * x90; const FLT x114 = x108 * x113; const FLT x115 = x112 * x114; const FLT x116 = x105 + (x95 * x115); const FLT x117 = 1. / sqrt(1 + (-1 * (x116 * x116))); const FLT x118 = 1. / sqrt(1 + (-1 * (1. / x85) * (1. / (x87 * x87)) * x78)); const FLT x119 = (-1 * x28) + (-1 * x26) + x30 + (-1 * x24); const FLT x120 = 2 * x43; const FLT x121 = 2 * x37; const FLT x122 = x32 * x121; const FLT x123 = x122 + (-1 * x119 * x120); const FLT x124 = 2 * x31; const FLT x125 = 1 + (x119 * x124) + (-2 * (x37 * x37)); const FLT x126 = x32 * x124; const FLT x127 = (x37 * x120) + (-1 * x126); const FLT x128 = (-1 * x68 * x127) + (x70 * x123) + (x63 * x125); const FLT x129 = 2 * x70; const FLT x130 = 2 * ((-1 * x66 * x123) + (x68 * x125) + (x63 * x127)); const FLT x131 = x123 + (-1 * x128 * x129) + (x66 * x130); const FLT x132 = 2 * x77; const FLT x133 = (-1 * x70 * x125) + (x66 * x127) + (x63 * x123); const FLT x134 = x125 + (-1 * x68 * x130) + (x129 * x133); const FLT x135 = 2 * x82; const FLT x136 = 2 * x68; const FLT x137 = 2 * x66; const FLT x138 = x127 + (x128 * x136) + (-1 * x133 * x137); const FLT x139 = 2 * x81; const FLT x140 = (x138 * x139) + (x134 * x135); const FLT x141 = 1.0/2.0 * x77; const FLT x142 = x88 * (1. / (x85 * sqrt(x85))) * x141; const FLT x143 = x118 * ((x89 * x131) + (-1 * x142 * (x140 + (x131 * x132)))); const FLT x144 = x92 * x143; const FLT x145 = (x93 * x143) + (x90 * (x144 + (-1 * x91 * x143))); const FLT x146 = (x94 * x143) + (x90 * x145); const FLT x147 = 2 * x96 * x108 * x112; const FLT x148 = 1. / x84; const FLT x149 = 1. / sqrt(1 + (-1 * x78 * (x103 * x103) * x148)); const FLT x150 = (1. / (x84 * sqrt(x84))) * x103 * x141; const FLT x151 = (-1 * x104 * x131) + (x140 * x150); const FLT x152 = 1. / x82; const FLT x153 = x81 * (1. / x83); const FLT x154 = x83 * x148; const FLT x155 = ((x134 * x153) + (-1 * x138 * x152)) * x154; const FLT x156 = (-1 * x155) + (x149 * x151); const FLT x157 = x107 * cos(x106); const FLT x158 = x101 * x109; const FLT x159 = x157 * x158; const FLT x160 = 2.40324066e-05 * x90; const FLT x161 = x95 * (1. / (x111 * x111)) * x114; const FLT x162 = x95 * x112 * x113; const FLT x163 = x162 * x157; const FLT x164 = x155 + (-1 * x117 * (x151 + (-1 * x163 * x156) + (x115 * x146) + (-1 * x161 * ((x110 * ((x90 * x146) + (x90 * (x146 + (x99 * x143) + (x90 * (x145 + (x90 * (x144 + (-1 * x160 * x143) + (x97 * x143))) + (x98 * x143))))) + (x100 * x143) + (x95 * x143))) + (-1 * x156 * x159))) + (x143 * x147))); const FLT x165 = ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * cos((-1 * asin(x116)) + x102 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha); const FLT x166 = (-1 * x33) + (-1 * x36) + x34 + (-1 * x35); const FLT x167 = 1 + (x121 * x166) + (-2 * (x43 * x43)); const FLT x168 = (x31 * x120) + (-1 * x122); const FLT x169 = x32 * x120; const FLT x170 = x169 + (-1 * x124 * x166); const FLT x171 = (-1 * x68 * x170) + (x70 * x167) + (x63 * x168); const FLT x172 = (-1 * x66 * x167) + (x68 * x168) + (x63 * x170); const FLT x173 = (-1 * x129 * x171) + x167 + (x172 * x137); const FLT x174 = (-1 * x70 * x168) + (x66 * x170) + (x63 * x167); const FLT x175 = (-1 * x172 * x136) + x168 + (x129 * x174); const FLT x176 = x170 + (x171 * x136) + (-1 * x174 * x137); const FLT x177 = (x176 * x139) + (x175 * x135); const FLT x178 = x118 * ((x89 * x173) + (-1 * x142 * (x177 + (x173 * x132)))); const FLT x179 = x92 * x178; const FLT x180 = (x93 * x178) + (x90 * (x179 + (-1 * x91 * x178))); const FLT x181 = (x94 * x178) + (x90 * x180); const FLT x182 = (-1 * x104 * x173) + (x177 * x150); const FLT x183 = ((x175 * x153) + (-1 * x176 * x152)) * x154; const FLT x184 = x157 * ((-1 * x183) + (x182 * x149)); const FLT x185 = x183 + (-1 * x117 * (x182 + (-1 * x161 * ((x110 * ((x100 * x178) + (x90 * (x181 + (x99 * x178) + (x90 * (x180 + (x90 * ((-1 * x160 * x178) + x179 + (x97 * x178))) + (x98 * x178))))) + (x90 * x181) + (x95 * x178))) + (-1 * x184 * x158))) + (x115 * x181) + (-1 * x162 * x184) + (x178 * x147))); const FLT x186 = (x37 * x124) + (-1 * x169); const FLT x187 = (-1 * x41) + (-1 * x42) + (-1 * x40) + x39; const FLT x188 = x126 + (-1 * x121 * x187); const FLT x189 = 1 + (x120 * x187) + (-2 * (x31 * x31)); const FLT x190 = (-1 * x68 * x189) + (x70 * x186) + (x63 * x188); const FLT x191 = (x68 * x188) + (-1 * x66 * x186) + (x63 * x189); const FLT x192 = x186 + (-1 * x129 * x190) + (x191 * x137); const FLT x193 = (-1 * x70 * x188) + (x66 * x189) + (x63 * x186); const FLT x194 = x188 + (-1 * x191 * x136) + (x129 * x193); const FLT x195 = x189 + (x190 * x136) + (-1 * x193 * x137); const FLT x196 = (x195 * x139) + (x194 * x135); const FLT x197 = x118 * ((x89 * x192) + (-1 * x142 * (x196 + (x192 * x132)))); const FLT x198 = (-1 * x104 * x192) + (x196 * x150); const FLT x199 = ((x194 * x153) + (-1 * x195 * x152)) * x154; const FLT x200 = (-1 * x199) + (x198 * x149); const FLT x201 = x92 * x197; const FLT x202 = (x93 * x197) + (x90 * (x201 + (-1 * x91 * x197))); const FLT x203 = (x94 * x197) + (x90 * x202); const FLT x204 = x199 + (-1 * x117 * ((x203 * x115) + x198 + (x197 * x147) + (-1 * x200 * x163) + (-1 * x161 * ((x110 * ((x90 * x203) + (x100 * x197) + (x95 * x197) + (x90 * (x203 + (x99 * x197) + (x90 * (x202 + (x90 * ((-1 * x160 * x197) + x201 + (x97 * x197))) + (x98 * x197))))))) + (-1 * x200 * x159))))); cnMatrixOptionalSet(Hx, 0, 0, x164 + (x165 * x164)); cnMatrixOptionalSet(Hx, 0, 1, x185 + (x165 * x185)); cnMatrixOptionalSet(Hx, 0, 2, x204 + (x204 * x165)); } // Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) { if(hx != 0) { hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt); } if(Hx != 0) { SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt); } }