/// NOTE: This is a generated file; do not edit. #pragma once #include // clang-format off static inline void SurvivePoseToErrorModel(SurviveAxisAnglePose* out, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x2 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x3 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x4 = x0 * x0; out->Pos[0]=(*_x1).Pos[0] + (-1 * (*_x0).Pos[0]); out->Pos[1]=(*_x1).Pos[1] + (-1 * (*_x0).Pos[1]); out->Pos[2]=(*_x1).Pos[2] + (-1 * (*_x0).Pos[2]); out->AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x2 * x2) + x4))); out->AxisAngleRot[1]=asin(2 * ((x0 * x3) + (-1 * x2 * x1))); out->AxisAngleRot[2]=atan2(2 * ((x1 * x3) + (x0 * x2)), 1 + (-2 * (x4 + (x1 * x1)))); } // Jacobian of SurvivePoseToErrorModel 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 SurvivePoseToErrorModel_jac_x1(CnMat* Hx, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = x0 * (*_x0).Rot[3]; const FLT x2 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x3 = x2 * (*_x0).Rot[2]; const FLT x4 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x5 = x4 * (*_x0).Rot[1]; const FLT x6 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x7 = x6 * (*_x0).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).Rot[2]; const FLT x13 = 4 * x12; const FLT x14 = x6 * (*_x0).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).Rot[3]; const FLT x20 = x14 + (x4 * (*_x0).Rot[0]); const FLT x21 = x20 + x12 + (-1 * x19); const FLT x22 = 4 * x1; const FLT x23 = x6 * (*_x0).Rot[2]; const FLT x24 = x4 * (*_x0).Rot[3]; const FLT x25 = x2 * (*_x0).Rot[0]; const FLT x26 = x0 * (*_x0).Rot[1]; const FLT x27 = (-1 * x26) + x25; const FLT x28 = x0 * (*_x0).Rot[0]; const FLT x29 = -4 * x28; const FLT x30 = x6 * (*_x0).Rot[3]; const FLT x31 = x4 * (*_x0).Rot[2]; const FLT x32 = x2 * (*_x0).Rot[1]; const FLT x33 = x32 + x28; const FLT x34 = (-1 * x31) + x33 + x30; const FLT x35 = -4 * x26; 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)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x7) + x22)) + (x21 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24)))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x23) + x35) * x17) + (x34 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x34 * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x36 * (x37 + x26 + (-1 * x25))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x36 * x21); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x36 * (x38 + x5 + (-1 * x7))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8)))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x25)) * x43) + (x40 * (x20 + x19 + (-1 * x12))))); } // Full version Jacobian of SurvivePoseToErrorModel wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] // Jacobian of SurvivePoseToErrorModel wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseToErrorModel_jac_x0(CnMat* Hx, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = x0 * (*_x1).Rot[3]; const FLT x2 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x3 = x2 * (*_x1).Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x6 = x5 * (*_x1).Rot[1]; const FLT x7 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x8 = x7 * (*_x1).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).Rot[2]; const FLT x14 = -4 * x13; const FLT x15 = x2 * (*_x1).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).Rot[3]; const FLT x21 = x20 + (-1 * x5 * (*_x1).Rot[0]); const FLT x22 = -4 * x1; const FLT x23 = x5 * (*_x1).Rot[3]; const FLT x24 = x7 * (*_x1).Rot[0]; const FLT x25 = x0 * (*_x1).Rot[1]; const FLT x26 = x2 * (*_x1).Rot[2]; const FLT x27 = x26 + x25; const FLT x28 = x0 * (*_x1).Rot[0]; const FLT x29 = 4 * x28; const FLT x30 = x2 * (*_x1).Rot[3]; const FLT x31 = x5 * (*_x1).Rot[2]; const FLT x32 = x7 * (*_x1).Rot[1]; const FLT x33 = (-1 * x32) + x31; const FLT x34 = (-1 * x28) + x30; const FLT x35 = 4 * x25; const FLT x36 = 1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7))))); const FLT x37 = 2 * x36; const FLT x38 = x23 + x27 + x24; const FLT x39 = 2 * (x21 + x13 + (-1 * x15)); const FLT x40 = 1 + (-2 * (x10 + (x7 * x7))); const FLT x41 = 1. / x40; const FLT x42 = 2 * x41; const FLT x43 = x40 * x40; const FLT x44 = (x5 * x7) + (x0 * x2); const FLT x45 = 2 * (1. / x43) * x44; const FLT x46 = x43 * (1. / (x43 + (4 * (x44 * x44)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + x15 + (-1 * x13))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), ((-1 * ((-4 * x26) + x35) * x18) + ((x34 + x33) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x37 * (x28 + x33 + (-1 * x30))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x38 * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x36 * x39); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x46 * ((-1 * (x14 + (-4 * x20)) * x45) + (x42 * x38))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x46 * ((-1 * x45 * (x22 + (4 * x8))) + (x42 * (x34 + x32 + (-1 * x31))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x46 * ((-1 * (x29 + (-4 * x32)) * x45) + ((x9 + (-1 * x3) + (-1 * x1)) * x42))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x46 * ((-1 * (x35 + (4 * x24)) * x45) + (x41 * x39))); } // Full version Jacobian of SurvivePoseToErrorModel wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseAddErrorModel(SurvivePose* out, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*_x0).Rot[0]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x6 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x7 = (*_x0).Rot[1] + (x0 * (*error_state).AxisAngleRot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[0]); const FLT x8 = 1. / sqrt((x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5)); out->Pos[0]=(*_x0).Pos[0] + (*error_state).Pos[0]; out->Pos[1]=(*_x0).Pos[1] + (*error_state).Pos[1]; out->Pos[2]=(*_x0).Pos[2] + (*error_state).Pos[2]; out->Rot[0]=x6 * x8; out->Rot[1]=x8 * x7; out->Rot[2]=x5 * x8; out->Rot[3]=x3 * x8; } // Jacobian of SurvivePoseAddErrorModel wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseAddErrorModel_jac_x0(CnMat* Hx, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*_x0).Rot[0]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x6 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x7 = (*_x0).Rot[1] + (x0 * (*error_state).AxisAngleRot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[0]); const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5); const FLT x9 = 1. / sqrt(x8); const FLT x10 = 1.0 * x7; const FLT x11 = 1.0 * x3; const FLT x12 = 1.0 * (*error_state).AxisAngleRot[1]; const FLT x13 = 1.0/2.0 * (1. / (x8 * sqrt(x8))); const FLT x14 = x13 * ((x5 * x12) + (x11 * (*error_state).AxisAngleRot[2]) + (x10 * (*error_state).AxisAngleRot[0]) + (2 * x6)); const FLT x15 = 0.5 * x9; const FLT x16 = x15 * (*error_state).AxisAngleRot[0]; const FLT x17 = -1 * x16; const FLT x18 = 1.0 * x6; const FLT x19 = 1.0 * x5; const FLT x20 = x13 * ((-1 * x19 * (*error_state).AxisAngleRot[2]) + (x3 * x12) + (-1 * x18 * (*error_state).AxisAngleRot[0]) + (2 * x7)); const FLT x21 = x15 * (*error_state).AxisAngleRot[1]; const FLT x22 = -1 * x21; const FLT x23 = x13 * ((2 * x5) + (-1 * x11 * (*error_state).AxisAngleRot[0]) + (x10 * (*error_state).AxisAngleRot[2]) + (-1 * x6 * x12)); const FLT x24 = x15 * (*error_state).AxisAngleRot[2]; const FLT x25 = -1 * x24; const FLT x26 = x13 * ((x19 * (*error_state).AxisAngleRot[0]) + (2 * x3) + (-1 * x7 * x12) + (-1 * x18 * (*error_state).AxisAngleRot[2])); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x6 * x14) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x6 * x20) + x17); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x6 * x23) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x6 * x26) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x7 * x14) + x16); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x7 * x20) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x7 * x23) + x24); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x7 * x26) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x5 * x14) + x21); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x5 * x20) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x5 * x23) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x5 * x26) + x16); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x3 * x14) + x24); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x3 * x20) + x21); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x3 * x23) + x17); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x3 * x26) + x9); } // Full version Jacobian of SurvivePoseAddErrorModel wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] // Jacobian of SurvivePoseAddErrorModel 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 SurvivePoseAddErrorModel_jac_error_state(CnMat* Hx, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*error_state).AxisAngleRot[2]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (x1 * (*_x0).Rot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = 0.5 * (*_x0).Rot[0]; const FLT x6 = (x5 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x7 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x8 = (*_x0).Rot[1] + (x1 * (*_x0).Rot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x5 * (*error_state).AxisAngleRot[0]); 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 * x6; const FLT x17 = 1.0/2.0 * (1. / (x9 * sqrt(x9))); const FLT x18 = ((x16 * (*_x0).Rot[3]) + (-1 * x15 * (*_x0).Rot[2]) + (x13 * (*_x0).Rot[0]) + (-1 * x14 * (*_x0).Rot[1])) * x17; const FLT x19 = x0 * x10; const FLT x20 = -1 * x19; const FLT x21 = ((x16 * (*_x0).Rot[0]) + (x15 * (*_x0).Rot[1]) + (-1 * x13 * (*_x0).Rot[3]) + (-1 * x14 * (*_x0).Rot[2])) * x17; const FLT x22 = x4 * x10; const FLT x23 = -1 * x22; const FLT x24 = ((-1 * x16 * (*_x0).Rot[1]) + (-1 * x14 * (*_x0).Rot[3]) + (x15 * (*_x0).Rot[0]) + (x13 * (*_x0).Rot[2])) * x17; const FLT x25 = x5 * x10; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x7 * x18) + x12); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x7 * x21) + x20); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x7 * x24) + x23); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x8 * x18) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x8 * x21) + x23); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x8 * x24) + x19); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x18) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x21) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x24) + x12); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x3 * x18) + x20); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x3 * x21) + x11); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x3 * x24) + x25); } // Full version Jacobian of SurvivePoseAddErrorModel 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 SurvivePoseToErrorModelExact(SurviveAxisAnglePose* out, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x2 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x3 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x4 = x0 * x0; out->Pos[0]=(*_x1).Pos[0] + (-1 * (*_x0).Pos[0]); out->Pos[1]=(*_x1).Pos[1] + (-1 * (*_x0).Pos[1]); out->Pos[2]=(*_x1).Pos[2] + (-1 * (*_x0).Pos[2]); out->AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x2 * x2) + x4))); out->AxisAngleRot[1]=asin(2 * ((x0 * x3) + (-1 * x2 * x1))); out->AxisAngleRot[2]=atan2(2 * ((x1 * x3) + (x0 * x2)), 1 + (-2 * (x4 + (x1 * x1)))); } // Jacobian of SurvivePoseToErrorModelExact 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 SurvivePoseToErrorModelExact_jac_x1(CnMat* Hx, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = x0 * (*_x0).Rot[3]; const FLT x2 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x3 = x2 * (*_x0).Rot[2]; const FLT x4 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x5 = x4 * (*_x0).Rot[1]; const FLT x6 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x7 = x6 * (*_x0).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).Rot[2]; const FLT x13 = 4 * x12; const FLT x14 = x6 * (*_x0).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).Rot[3]; const FLT x20 = x14 + (x4 * (*_x0).Rot[0]); const FLT x21 = x20 + x12 + (-1 * x19); const FLT x22 = 4 * x1; const FLT x23 = x6 * (*_x0).Rot[2]; const FLT x24 = x4 * (*_x0).Rot[3]; const FLT x25 = x2 * (*_x0).Rot[0]; const FLT x26 = x0 * (*_x0).Rot[1]; const FLT x27 = (-1 * x26) + x25; const FLT x28 = x0 * (*_x0).Rot[0]; const FLT x29 = -4 * x28; const FLT x30 = x6 * (*_x0).Rot[3]; const FLT x31 = x4 * (*_x0).Rot[2]; const FLT x32 = x2 * (*_x0).Rot[1]; const FLT x33 = x32 + x28; const FLT x34 = (-1 * x31) + x33 + x30; const FLT x35 = -4 * x26; 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)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x7) + x22)) + (x21 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24)))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x23) + x35) * x17) + (x34 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x34 * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x36 * (x37 + x26 + (-1 * x25))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x36 * x21); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x36 * (x38 + x5 + (-1 * x7))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8)))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x25)) * x43) + (x40 * (x20 + x19 + (-1 * x12))))); } // Full version Jacobian of SurvivePoseToErrorModelExact wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]] // Jacobian of SurvivePoseToErrorModelExact wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseToErrorModelExact_jac_x0(CnMat* Hx, const SurvivePose* _x1, const SurvivePose* _x0) { const FLT x0 = ((*_x1).Rot[3] * (*_x0).Rot[1]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[2]) + ((*_x1).Rot[2] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[1] * (*_x0).Rot[3]); const FLT x1 = x0 * (*_x1).Rot[3]; const FLT x2 = (-1 * (*_x1).Rot[0] * (*_x0).Rot[1]) + ((*_x1).Rot[1] * (*_x0).Rot[0]) + ((*_x1).Rot[2] * (*_x0).Rot[3]) + (-1 * (*_x1).Rot[3] * (*_x0).Rot[2]); const FLT x3 = x2 * (*_x1).Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = ((*_x1).Rot[1] * (*_x0).Rot[1]) + ((*_x1).Rot[2] * (*_x0).Rot[2]) + ((*_x1).Rot[3] * (*_x0).Rot[3]) + ((*_x1).Rot[0] * (*_x0).Rot[0]); const FLT x6 = x5 * (*_x1).Rot[1]; const FLT x7 = (-1 * (*_x1).Rot[2] * (*_x0).Rot[1]) + ((*_x1).Rot[3] * (*_x0).Rot[0]) + (-1 * (*_x1).Rot[0] * (*_x0).Rot[3]) + ((*_x1).Rot[1] * (*_x0).Rot[2]); const FLT x8 = x7 * (*_x1).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).Rot[2]; const FLT x14 = -4 * x13; const FLT x15 = x2 * (*_x1).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).Rot[3]; const FLT x21 = x20 + (-1 * x5 * (*_x1).Rot[0]); const FLT x22 = -4 * x1; const FLT x23 = x5 * (*_x1).Rot[3]; const FLT x24 = x7 * (*_x1).Rot[0]; const FLT x25 = x0 * (*_x1).Rot[1]; const FLT x26 = x2 * (*_x1).Rot[2]; const FLT x27 = x26 + x25; const FLT x28 = x0 * (*_x1).Rot[0]; const FLT x29 = 4 * x28; const FLT x30 = x2 * (*_x1).Rot[3]; const FLT x31 = x5 * (*_x1).Rot[2]; const FLT x32 = x7 * (*_x1).Rot[1]; const FLT x33 = (-1 * x32) + x31; const FLT x34 = (-1 * x28) + x30; const FLT x35 = 4 * x25; const FLT x36 = 1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7))))); const FLT x37 = 2 * x36; const FLT x38 = x23 + x27 + x24; const FLT x39 = 2 * (x21 + x13 + (-1 * x15)); const FLT x40 = 1 + (-2 * (x10 + (x7 * x7))); const FLT x41 = 1. / x40; const FLT x42 = 2 * x41; const FLT x43 = x40 * x40; const FLT x44 = (x5 * x7) + (x0 * x2); const FLT x45 = 2 * (1. / x43) * x44; const FLT x46 = x43 * (1. / (x43 + (4 * (x44 * x44)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + x15 + (-1 * x13))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), ((-1 * ((-4 * x26) + x35) * x18) + ((x34 + x33) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x37 * (x28 + x33 + (-1 * x30))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x38 * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x36 * x39); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), x46 * ((-1 * (x14 + (-4 * x20)) * x45) + (x42 * x38))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), x46 * ((-1 * x45 * (x22 + (4 * x8))) + (x42 * (x34 + x32 + (-1 * x31))))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), x46 * ((-1 * (x29 + (-4 * x32)) * x45) + ((x9 + (-1 * x3) + (-1 * x1)) * x42))); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), x46 * ((-1 * (x35 + (4 * x24)) * x45) + (x41 * x39))); } // Full version Jacobian of SurvivePoseToErrorModelExact wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseAddErrorModelExact(SurvivePose* out, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*_x0).Rot[0]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x6 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x7 = (*_x0).Rot[1] + (x0 * (*error_state).AxisAngleRot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[0]); const FLT x8 = 1. / sqrt((x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5)); out->Pos[0]=(*_x0).Pos[0] + (*error_state).Pos[0]; out->Pos[1]=(*_x0).Pos[1] + (*error_state).Pos[1]; out->Pos[2]=(*_x0).Pos[2] + (*error_state).Pos[2]; out->Rot[0]=x6 * x8; out->Rot[1]=x8 * x7; out->Rot[2]=x5 * x8; out->Rot[3]=x3 * x8; } // Jacobian of SurvivePoseAddErrorModelExact wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] static inline void SurvivePoseAddErrorModelExact_jac_x0(CnMat* Hx, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*_x0).Rot[0]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x6 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x7 = (*_x0).Rot[1] + (x0 * (*error_state).AxisAngleRot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[0]); const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5); const FLT x9 = 1. / sqrt(x8); const FLT x10 = 1.0 * x7; const FLT x11 = 1.0 * x3; const FLT x12 = 1.0 * (*error_state).AxisAngleRot[1]; const FLT x13 = 1.0/2.0 * (1. / (x8 * sqrt(x8))); const FLT x14 = x13 * ((x5 * x12) + (x11 * (*error_state).AxisAngleRot[2]) + (x10 * (*error_state).AxisAngleRot[0]) + (2 * x6)); const FLT x15 = 0.5 * x9; const FLT x16 = x15 * (*error_state).AxisAngleRot[0]; const FLT x17 = -1 * x16; const FLT x18 = 1.0 * x6; const FLT x19 = 1.0 * x5; const FLT x20 = x13 * ((-1 * x19 * (*error_state).AxisAngleRot[2]) + (x3 * x12) + (-1 * x18 * (*error_state).AxisAngleRot[0]) + (2 * x7)); const FLT x21 = x15 * (*error_state).AxisAngleRot[1]; const FLT x22 = -1 * x21; const FLT x23 = x13 * ((2 * x5) + (-1 * x11 * (*error_state).AxisAngleRot[0]) + (x10 * (*error_state).AxisAngleRot[2]) + (-1 * x6 * x12)); const FLT x24 = x15 * (*error_state).AxisAngleRot[2]; const FLT x25 = -1 * x24; const FLT x26 = x13 * ((x19 * (*error_state).AxisAngleRot[0]) + (2 * x3) + (-1 * x7 * x12) + (-1 * x18 * (*error_state).AxisAngleRot[2])); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[0])/sizeof(FLT), offsetof(SurvivePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[1])/sizeof(FLT), offsetof(SurvivePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[2])/sizeof(FLT), offsetof(SurvivePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x6 * x14) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x6 * x20) + x17); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x6 * x23) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x6 * x26) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x7 * x14) + x16); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x7 * x20) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x7 * x23) + x24); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x7 * x26) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x5 * x14) + x21); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x5 * x20) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x5 * x23) + x9); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x5 * x26) + x16); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[0])/sizeof(FLT), (-1 * x3 * x14) + x24); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * x3 * x20) + x21); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-1 * x3 * x23) + x17); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * x3 * x26) + x9); } // Full version Jacobian of SurvivePoseAddErrorModelExact wrt [(*_x0).Pos[0], (*_x0).Pos[1], (*_x0).Pos[2], (*_x0).Rot[0], (*_x0).Rot[1], (*_x0).Rot[2], (*_x0).Rot[3]] // Jacobian of SurvivePoseAddErrorModelExact 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 SurvivePoseAddErrorModelExact_jac_error_state(CnMat* Hx, const SurvivePose* _x0, const SurviveAxisAnglePose* error_state) { const FLT x0 = 0.5 * (*_x0).Rot[2]; const FLT x1 = 0.5 * (*error_state).AxisAngleRot[2]; const FLT x2 = 0.5 * (*_x0).Rot[1]; const FLT x3 = (x2 * (*error_state).AxisAngleRot[1]) + (-1 * x0 * (*error_state).AxisAngleRot[0]) + (x1 * (*_x0).Rot[0]) + (*_x0).Rot[3]; const FLT x4 = 0.5 * (*_x0).Rot[3]; const FLT x5 = 0.5 * (*_x0).Rot[0]; const FLT x6 = (x5 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[2]) + (x4 * (*error_state).AxisAngleRot[0]) + (*_x0).Rot[2]; const FLT x7 = (-1 * x0 * (*error_state).AxisAngleRot[1]) + (-1 * x2 * (*error_state).AxisAngleRot[0]) + (-1 * x4 * (*error_state).AxisAngleRot[2]) + (*_x0).Rot[0]; const FLT x8 = (*_x0).Rot[1] + (x1 * (*_x0).Rot[2]) + (-1 * x4 * (*error_state).AxisAngleRot[1]) + (x5 * (*error_state).AxisAngleRot[0]); 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 * x6; const FLT x17 = 1.0/2.0 * (1. / (x9 * sqrt(x9))); const FLT x18 = ((x16 * (*_x0).Rot[3]) + (-1 * x15 * (*_x0).Rot[2]) + (x13 * (*_x0).Rot[0]) + (-1 * x14 * (*_x0).Rot[1])) * x17; const FLT x19 = x0 * x10; const FLT x20 = -1 * x19; const FLT x21 = ((x16 * (*_x0).Rot[0]) + (x15 * (*_x0).Rot[1]) + (-1 * x13 * (*_x0).Rot[3]) + (-1 * x14 * (*_x0).Rot[2])) * x17; const FLT x22 = x4 * x10; const FLT x23 = -1 * x22; const FLT x24 = ((-1 * x16 * (*_x0).Rot[1]) + (-1 * x14 * (*_x0).Rot[3]) + (x15 * (*_x0).Rot[0]) + (x13 * (*_x0).Rot[2])) * x17; const FLT x25 = x5 * x10; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x7 * x18) + x12); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x7 * x21) + x20); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x7 * x24) + x23); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x8 * x18) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x8 * x21) + x23); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x8 * x24) + x19); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x18) + x22); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x21) + x25); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x24) + x12); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x3 * x18) + x20); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (-1 * x3 * x21) + x11); cnMatrixOptionalSet(Hx, offsetof(SurvivePose, Rot[3])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x3 * x24) + x25); } // Full version Jacobian of SurvivePoseAddErrorModelExact 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 SurviveKalmanModelToErrorModel(SurviveKalmanErrorModel* out, const SurviveKalmanModel* _x1, const SurviveKalmanModel* _x0) { const FLT x0 = ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[3]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[2]); const FLT x1 = (-1 * (*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[2]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[0]); const FLT x2 = (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[2]); const FLT x3 = ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[0]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[2]); const FLT x4 = x0 * x0; const FLT x5 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[0]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x6 = (-1 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[0]); const FLT x7 = (-1 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[1]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[3]); const FLT x8 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[3]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x9 = x5 * x5; out->Pose.Pos[0]=(*_x1).Pose.Pos[0] + (-1 * (*_x0).Pose.Pos[0]); out->Pose.Pos[1]=(*_x1).Pose.Pos[1] + (-1 * (*_x0).Pose.Pos[1]); out->Pose.Pos[2]=(*_x1).Pose.Pos[2] + (-1 * (*_x0).Pose.Pos[2]); out->Pose.AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x2 * x2) + x4))); out->Pose.AxisAngleRot[1]=asin(2 * ((x0 * x3) + (-1 * x2 * x1))); out->Pose.AxisAngleRot[2]=atan2(2 * ((x1 * x3) + (x0 * x2)), 1 + (-2 * (x4 + (x1 * x1)))); out->Velocity.Pos[0]=(*_x1).Velocity.Pos[0] + (-1 * (*_x0).Velocity.Pos[0]); out->Velocity.Pos[1]=(*_x1).Velocity.Pos[1] + (-1 * (*_x0).Velocity.Pos[1]); out->Velocity.Pos[2]=(*_x1).Velocity.Pos[2] + (-1 * (*_x0).Velocity.Pos[2]); out->Velocity.AxisAngleRot[0]=(*_x1).Velocity.AxisAngleRot[0] + (-1 * (*_x0).Velocity.AxisAngleRot[0]); out->Velocity.AxisAngleRot[1]=(*_x1).Velocity.AxisAngleRot[1] + (-1 * (*_x0).Velocity.AxisAngleRot[1]); out->Velocity.AxisAngleRot[2]=(*_x1).Velocity.AxisAngleRot[2] + (-1 * (*_x0).Velocity.AxisAngleRot[2]); out->Acc[0]=(*_x1).Acc[0] + (-1 * (*_x0).Acc[0]); out->Acc[1]=(*_x1).Acc[1] + (-1 * (*_x0).Acc[1]); out->Acc[2]=(*_x1).Acc[2] + (-1 * (*_x0).Acc[2]); out->IMUBias.AccScale[0]=(*_x1).IMUBias.AccScale[0] + (-1 * (*_x0).IMUBias.AccScale[0]); out->IMUBias.AccScale[1]=(*_x1).IMUBias.AccScale[1] + (-1 * (*_x0).IMUBias.AccScale[1]); out->IMUBias.AccScale[2]=(*_x1).IMUBias.AccScale[2] + (-1 * (*_x0).IMUBias.AccScale[2]); out->IMUBias.IMUCorrection[0]=atan2(2 * ((x8 * x7) + (x6 * x5)), 1 + (-2 * ((x7 * x7) + x9))); out->IMUBias.IMUCorrection[1]=asin(2 * ((x5 * x8) + (-1 * x6 * x7))); out->IMUBias.IMUCorrection[2]=atan2(2 * ((x6 * x8) + (x5 * x7)), 1 + (-2 * (x9 + (x6 * x6)))); out->IMUBias.AccBias[0]=(*_x1).IMUBias.AccBias[0] + (-1 * (*_x0).IMUBias.AccBias[0]); out->IMUBias.AccBias[1]=(*_x1).IMUBias.AccBias[1] + (-1 * (*_x0).IMUBias.AccBias[1]); out->IMUBias.AccBias[2]=(*_x1).IMUBias.AccBias[2] + (-1 * (*_x0).IMUBias.AccBias[2]); out->IMUBias.GyroBias[0]=(*_x1).IMUBias.GyroBias[0] + (-1 * (*_x0).IMUBias.GyroBias[0]); out->IMUBias.GyroBias[1]=(*_x1).IMUBias.GyroBias[1] + (-1 * (*_x0).IMUBias.GyroBias[1]); out->IMUBias.GyroBias[2]=(*_x1).IMUBias.GyroBias[2] + (-1 * (*_x0).IMUBias.GyroBias[2]); } // Jacobian of SurviveKalmanModelToErrorModel wrt [(*_x1).Acc[0], (*_x1).Acc[1], (*_x1).Acc[2], (*_x1).IMUBias.AccBias[0], (*_x1).IMUBias.AccBias[1], (*_x1).IMUBias.AccBias[2], (*_x1).IMUBias.AccScale[0], (*_x1).IMUBias.AccScale[1], (*_x1).IMUBias.AccScale[2], (*_x1).IMUBias.GyroBias[0], (*_x1).IMUBias.GyroBias[1], (*_x1).IMUBias.GyroBias[2], (*_x1).IMUBias.IMUCorrection[0], (*_x1).IMUBias.IMUCorrection[1], (*_x1).IMUBias.IMUCorrection[2], (*_x1).IMUBias.IMUCorrection[3], (*_x1).Pose.Pos[0], (*_x1).Pose.Pos[1], (*_x1).Pose.Pos[2], (*_x1).Pose.Rot[0], (*_x1).Pose.Rot[1], (*_x1).Pose.Rot[2], (*_x1).Pose.Rot[3], (*_x1).Velocity.AxisAngleRot[0], (*_x1).Velocity.AxisAngleRot[1], (*_x1).Velocity.AxisAngleRot[2], (*_x1).Velocity.Pos[0], (*_x1).Velocity.Pos[1], (*_x1).Velocity.Pos[2]] static inline void SurviveKalmanModelToErrorModel_jac_x1(CnMat* Hx, const SurviveKalmanModel* _x1, const SurviveKalmanModel* _x0) { const FLT x0 = ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[3]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[2]); const FLT x1 = x0 * (*_x0).Pose.Rot[3]; const FLT x2 = (-1 * (*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[2]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[0]); const FLT x3 = x2 * (*_x0).Pose.Rot[2]; const FLT x4 = ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[0]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[2]); const FLT x5 = x4 * (*_x0).Pose.Rot[1]; const FLT x6 = (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[2]); const FLT x7 = x6 * (*_x0).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).Pose.Rot[2]; const FLT x13 = 4 * x12; const FLT x14 = x6 * (*_x0).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).Pose.Rot[3]; const FLT x20 = (x4 * (*_x0).Pose.Rot[0]) + x14; const FLT x21 = x20 + x12 + (-1 * x19); const FLT x22 = 4 * x1; const FLT x23 = x6 * (*_x0).Pose.Rot[2]; const FLT x24 = x4 * (*_x0).Pose.Rot[3]; const FLT x25 = x0 * (*_x0).Pose.Rot[1]; const FLT x26 = x2 * (*_x0).Pose.Rot[0]; const FLT x27 = x26 + (-1 * x25); const FLT x28 = x0 * (*_x0).Pose.Rot[0]; const FLT x29 = -4 * x28; const FLT x30 = x6 * (*_x0).Pose.Rot[3]; const FLT x31 = x4 * (*_x0).Pose.Rot[2]; const FLT x32 = x2 * (*_x0).Pose.Rot[1]; const FLT x33 = x32 + x28; 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 = 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 * x4) + (x0 * x6); const FLT x43 = 2 * (1. / x41) * x42; const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42)))); const FLT x45 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[0]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x46 = x45 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x47 = (-1 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[0]); const FLT x48 = x47 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x49 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[3]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x50 = x49 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x51 = (-1 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[1]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[3]); const FLT x52 = x51 * (*_x0).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).IMUBias.IMUCorrection[2]; const FLT x58 = 4 * x57; const FLT x59 = x51 * (*_x0).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).IMUBias.IMUCorrection[3]; const FLT x65 = x59 + (x49 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x66 = x65 + x57 + (-1 * x64); const FLT x67 = 4 * x46; const FLT x68 = x49 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x69 = x51 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x70 = x47 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x71 = x45 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x72 = (-1 * x71) + x70; const FLT x73 = x45 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x74 = -4 * x73; const FLT x75 = x51 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x76 = x49 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x77 = x47 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x78 = x77 + x73; const FLT x79 = x78 + x75 + (-1 * x76); const FLT x80 = -4 * x71; 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 = x48 + x46; 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)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x7) + x22)) + (x21 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x23) + x35) * x17) + (x34 * x11))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x34 * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x36 * (x37 + x25 + (-1 * x26))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x36 * x21); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x36 * (x38 + x5 + (-1 * x7))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x27 + x37) * x40)) * x44); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x8 + x38)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x26)) * x43) + (x40 * (x20 + x19 + (-1 * x12))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x63 * ((-1 * ((4 * x59) + x58) * x62) + (x56 * (x53 + (-1 * x46) + (-1 * x48))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x63 * ((-1 * ((-4 * x52) + x67) * x62) + (x66 * x56))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x63 * ((-1 * ((-4 * x75) + x74) * x62) + (x56 * (x72 + x68 + x69)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x63 * ((-1 * ((4 * x69) + x80) * x62) + (x79 * x56))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * x79); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * (x82 + x71 + (-1 * x70))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x66); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x50 + x83 + (-1 * x52))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x58 + (4 * x64)) * x88) + ((x72 + x82) * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (-4 * x48)) * x88) + (x85 * (x78 + (-1 * x75) + x76)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (4 * x77)) * x88) + ((x53 + x83) * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (-4 * x70)) * x88) + (x85 * (x65 + x64 + (-1 * x57))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); } // Full version Jacobian of SurviveKalmanModelToErrorModel wrt [(*_x1).Acc[0], (*_x1).Acc[1], (*_x1).Acc[2], (*_x1).IMUBias.AccBias[0], (*_x1).IMUBias.AccBias[1], (*_x1).IMUBias.AccBias[2], (*_x1).IMUBias.AccScale[0], (*_x1).IMUBias.AccScale[1], (*_x1).IMUBias.AccScale[2], (*_x1).IMUBias.GyroBias[0], (*_x1).IMUBias.GyroBias[1], (*_x1).IMUBias.GyroBias[2], (*_x1).IMUBias.IMUCorrection[0], (*_x1).IMUBias.IMUCorrection[1], (*_x1).IMUBias.IMUCorrection[2], (*_x1).IMUBias.IMUCorrection[3], (*_x1).Pose.Pos[0], (*_x1).Pose.Pos[1], (*_x1).Pose.Pos[2], (*_x1).Pose.Rot[0], (*_x1).Pose.Rot[1], (*_x1).Pose.Rot[2], (*_x1).Pose.Rot[3], (*_x1).Velocity.AxisAngleRot[0], (*_x1).Velocity.AxisAngleRot[1], (*_x1).Velocity.AxisAngleRot[2], (*_x1).Velocity.Pos[0], (*_x1).Velocity.Pos[1], (*_x1).Velocity.Pos[2]] // Jacobian of SurviveKalmanModelToErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModelToErrorModel_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x1, const SurviveKalmanModel* _x0) { const FLT x0 = ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[3]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[2]); const FLT x1 = x0 * (*_x1).Pose.Rot[3]; const FLT x2 = (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[0]) + (-1 * (*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[2]); const FLT x3 = x2 * (*_x1).Pose.Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[0]) + ((*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[2]); const FLT x6 = x5 * (*_x1).Pose.Rot[1]; const FLT x7 = (-1 * (*_x1).Pose.Rot[2] * (*_x0).Pose.Rot[1]) + ((*_x1).Pose.Rot[1] * (*_x0).Pose.Rot[2]) + (-1 * (*_x1).Pose.Rot[0] * (*_x0).Pose.Rot[3]) + ((*_x1).Pose.Rot[3] * (*_x0).Pose.Rot[0]); const FLT x8 = x7 * (*_x1).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).Pose.Rot[2]; const FLT x14 = -4 * x13; const FLT x15 = x2 * (*_x1).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).Pose.Rot[3]; const FLT x21 = x20 + (-1 * x5 * (*_x1).Pose.Rot[0]); const FLT x22 = -4 * x1; const FLT x23 = x5 * (*_x1).Pose.Rot[3]; const FLT x24 = x7 * (*_x1).Pose.Rot[0]; const FLT x25 = x0 * (*_x1).Pose.Rot[1]; const FLT x26 = x2 * (*_x1).Pose.Rot[2]; const FLT x27 = x26 + x25; const FLT x28 = x0 * (*_x1).Pose.Rot[0]; const FLT x29 = 4 * x28; const FLT x30 = x2 * (*_x1).Pose.Rot[3]; const FLT x31 = (-1 * x28) + x30; const FLT x32 = x5 * (*_x1).Pose.Rot[2]; const FLT x33 = x7 * (*_x1).Pose.Rot[1]; const FLT x34 = (-1 * x33) + x32; const FLT x35 = 4 * x25; 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 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[1]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[3]); const FLT x46 = x45 * (*_x1).IMUBias.IMUCorrection[0]; const FLT x47 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[0]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x48 = x47 * (*_x1).IMUBias.IMUCorrection[3]; const FLT x49 = x48 + x46; const FLT x50 = ((*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[0]) + ((*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[3]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[2]); const FLT x51 = x50 * (*_x1).IMUBias.IMUCorrection[1]; const FLT x52 = (-1 * (*_x0).IMUBias.IMUCorrection[1] * (*_x1).IMUBias.IMUCorrection[2]) + ((*_x0).IMUBias.IMUCorrection[2] * (*_x1).IMUBias.IMUCorrection[1]) + ((*_x0).IMUBias.IMUCorrection[0] * (*_x1).IMUBias.IMUCorrection[3]) + (-1 * (*_x0).IMUBias.IMUCorrection[3] * (*_x1).IMUBias.IMUCorrection[0]); const FLT x53 = x52 * (*_x1).IMUBias.IMUCorrection[2]; const FLT x54 = x53 + x51; const FLT x55 = x47 * x47; const FLT x56 = 1 + (-2 * ((x45 * x45) + x55)); const FLT x57 = 2 * (1. / x56); const FLT x58 = x47 * (*_x1).IMUBias.IMUCorrection[2]; const FLT x59 = -4 * x58; const FLT x60 = x45 * (*_x1).IMUBias.IMUCorrection[1]; const FLT x61 = x56 * x56; const FLT x62 = (x50 * x45) + (x52 * x47); const FLT x63 = 2 * (1. / x61) * x62; const FLT x64 = x61 * (1. / (x61 + (4 * (x62 * x62)))); const FLT x65 = x52 * (*_x1).IMUBias.IMUCorrection[3]; const FLT x66 = x65 + (-1 * x50 * (*_x1).IMUBias.IMUCorrection[0]); const FLT x67 = -4 * x48; const FLT x68 = x50 * (*_x1).IMUBias.IMUCorrection[3]; const FLT x69 = x52 * (*_x1).IMUBias.IMUCorrection[0]; const FLT x70 = x45 * (*_x1).IMUBias.IMUCorrection[2]; const FLT x71 = x47 * (*_x1).IMUBias.IMUCorrection[1]; const FLT x72 = x71 + x70; const FLT x73 = x47 * (*_x1).IMUBias.IMUCorrection[0]; const FLT x74 = 4 * x73; const FLT x75 = x45 * (*_x1).IMUBias.IMUCorrection[3]; const FLT x76 = x50 * (*_x1).IMUBias.IMUCorrection[2]; const FLT x77 = x52 * (*_x1).IMUBias.IMUCorrection[1]; const FLT x78 = (-1 * x77) + x76; const FLT x79 = (-1 * x73) + x75; 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 + (x52 * x52))); const FLT x85 = 2 * (1. / x84); const FLT x86 = x84 * x84; const FLT x87 = (x50 * x52) + (x45 * x47); const FLT x88 = 2 * (1. / x86) * x87; const FLT x89 = x86 * (1. / (x86 + (4 * (x87 * x87)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + x15 + (-1 * x13))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x26) + x35) * x18) + ((x34 + x31) * x12)) * x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x36 * (x34 + x28 + (-1 * x30))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x36 * x37); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x36 * x38); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x36); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x44 * ((-1 * (x14 + (-4 * x20)) * x43) + (x40 * x37))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (4 * x8))) + (x40 * (x31 + x33 + (-1 * x32))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (-4 * x33)) * x43) + ((x9 + (-1 * x1) + (-1 * x3)) * x40))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (4 * x24)) * x43) + (x40 * x38))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x64 * ((-1 * ((-4 * x60) + x59) * x63) + ((x54 + x49) * x57))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x64 * ((-1 * ((4 * x46) + x67) * x63) + (x57 * (x66 + x60 + (-1 * x58))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x64 * ((-1 * ((4 * x75) + x74) * x63) + (x57 * (x72 + (-1 * x68) + (-1 * x69))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x64 * ((-1 * ((-4 * x70) + x80) * x63) + ((x79 + x78) * x57))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * (x78 + x73 + (-1 * x75))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * x82); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x83); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x49 + (-1 * x51) + (-1 * x53))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x59 + (-4 * x65)) * x88) + (x82 * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (4 * x53)) * x88) + (x85 * (x79 + (-1 * x76) + x77)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (-4 * x77)) * x88) + (x85 * (x54 + (-1 * x46) + (-1 * x48))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (4 * x69)) * x88) + (x83 * x85))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), -1); } // Full version Jacobian of SurviveKalmanModelToErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModelAddErrorModel(SurviveKalmanModel* out, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Pose.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_state).Pose.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[2]; const FLT x4 = (-1 * x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[3]) + (-1 * x1 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0]; const FLT x5 = (x2 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[1]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x6 = (*_x0).Pose.Rot[1] + (x0 * (*_x0).Pose.Rot[0]) + (-1 * x1 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x7 = 1. / sqrt((x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4)); const FLT x8 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x9 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x10 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x11 = (-1 * x10 * (*error_state).IMUBias.IMUCorrection[2]) + (x9 * (*error_state).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2] + (x8 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x12 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x13 = (-1 * x10 * (*error_state).IMUBias.IMUCorrection[0]) + (-1 * x9 * (*error_state).IMUBias.IMUCorrection[2]) + (-1 * x12 * (*error_state).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[0]; const FLT x14 = (x8 * (*error_state).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[3] + (x10 * (*error_state).IMUBias.IMUCorrection[1]) + (-1 * x12 * (*error_state).IMUBias.IMUCorrection[0]); const FLT x15 = (*_x0).IMUBias.IMUCorrection[1] + (x8 * (*error_state).IMUBias.IMUCorrection[0]) + (x12 * (*error_state).IMUBias.IMUCorrection[2]) + (-1 * x9 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x16 = 1. / sqrt((x15 * x15) + (x14 * x14) + (x11 * x11) + (x13 * x13)); out->Pose.Pos[0]=(*_x0).Pose.Pos[0] + (*error_state).Pose.Pos[0]; out->Pose.Pos[1]=(*_x0).Pose.Pos[1] + (*error_state).Pose.Pos[1]; out->Pose.Pos[2]=(*_x0).Pose.Pos[2] + (*error_state).Pose.Pos[2]; out->Pose.Rot[0]=x4 * x7; out->Pose.Rot[1]=x6 * x7; out->Pose.Rot[2]=x3 * x7; out->Pose.Rot[3]=x5 * x7; out->Velocity.Pos[0]=(*_x0).Velocity.Pos[0] + (*error_state).Velocity.Pos[0]; out->Velocity.Pos[1]=(*_x0).Velocity.Pos[1] + (*error_state).Velocity.Pos[1]; out->Velocity.Pos[2]=(*_x0).Velocity.Pos[2] + (*error_state).Velocity.Pos[2]; out->Velocity.AxisAngleRot[0]=(*_x0).Velocity.AxisAngleRot[0] + (*error_state).Velocity.AxisAngleRot[0]; out->Velocity.AxisAngleRot[1]=(*_x0).Velocity.AxisAngleRot[1] + (*error_state).Velocity.AxisAngleRot[1]; out->Velocity.AxisAngleRot[2]=(*_x0).Velocity.AxisAngleRot[2] + (*error_state).Velocity.AxisAngleRot[2]; out->Acc[0]=(*_x0).Acc[0] + (*error_state).Acc[0]; out->Acc[1]=(*_x0).Acc[1] + (*error_state).Acc[1]; out->Acc[2]=(*_x0).Acc[2] + (*error_state).Acc[2]; out->IMUBias.AccScale[0]=(*_x0).IMUBias.AccScale[0] + (*error_state).IMUBias.AccScale[0]; out->IMUBias.AccScale[1]=(*_x0).IMUBias.AccScale[1] + (*error_state).IMUBias.AccScale[1]; out->IMUBias.AccScale[2]=(*_x0).IMUBias.AccScale[2] + (*error_state).IMUBias.AccScale[2]; out->IMUBias.IMUCorrection[0]=x13 * x16; out->IMUBias.IMUCorrection[1]=x15 * x16; out->IMUBias.IMUCorrection[2]=x11 * x16; out->IMUBias.IMUCorrection[3]=x14 * x16; out->IMUBias.AccBias[0]=(*_x0).IMUBias.AccBias[0] + (*error_state).IMUBias.AccBias[0]; out->IMUBias.AccBias[1]=(*_x0).IMUBias.AccBias[1] + (*error_state).IMUBias.AccBias[1]; out->IMUBias.AccBias[2]=(*_x0).IMUBias.AccBias[2] + (*error_state).IMUBias.AccBias[2]; out->IMUBias.GyroBias[0]=(*_x0).IMUBias.GyroBias[0] + (*error_state).IMUBias.GyroBias[0]; out->IMUBias.GyroBias[1]=(*_x0).IMUBias.GyroBias[1] + (*error_state).IMUBias.GyroBias[1]; out->IMUBias.GyroBias[2]=(*_x0).IMUBias.GyroBias[2] + (*error_state).IMUBias.GyroBias[2]; } // Jacobian of SurviveKalmanModelAddErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModelAddErrorModel_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*_x0).Pose.Rot[3]; const FLT x1 = 0.5 * (*error_state).Pose.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x0 * (*error_state).Pose.AxisAngleRot[0]) + (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[2]; const FLT x4 = 0.5 * (*error_state).Pose.AxisAngleRot[0]; const FLT x5 = (-1 * x4 * (*_x0).Pose.Rot[1]) + (-1 * x0 * (*error_state).Pose.AxisAngleRot[2]) + (-1 * x1 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0]; const FLT x6 = (x1 * (*_x0).Pose.Rot[1]) + (x2 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[3] + (-1 * x4 * (*_x0).Pose.Rot[2]); const FLT x7 = (*_x0).Pose.Rot[1] + (-1 * x0 * (*error_state).Pose.AxisAngleRot[1]) + (x4 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5); const FLT x9 = 1. / sqrt(x8); const FLT x10 = 1.0 * x7; const FLT x11 = 1.0 * (*error_state).Pose.AxisAngleRot[2]; const FLT x12 = 1.0 * x3; const FLT x13 = (2 * x5) + (x12 * (*error_state).Pose.AxisAngleRot[1]) + (x10 * (*error_state).Pose.AxisAngleRot[0]) + (x6 * x11); const FLT x14 = 1.0/2.0 * (1. / (x8 * sqrt(x8))); const FLT x15 = x14 * x13; const FLT x16 = 0.5 * x9; const FLT x17 = x16 * (*error_state).Pose.AxisAngleRot[0]; const FLT x18 = -1 * x17; const FLT x19 = 1.0 * x6; const FLT x20 = 1.0 * x5; const FLT x21 = (-1 * x3 * x11) + (-1 * x20 * (*error_state).Pose.AxisAngleRot[0]) + (x19 * (*error_state).Pose.AxisAngleRot[1]) + (2 * x7); const FLT x22 = x21 * x14; const FLT x23 = x16 * (*error_state).Pose.AxisAngleRot[1]; const FLT x24 = -1 * x23; const FLT x25 = x14 * ((-1 * x19 * (*error_state).Pose.AxisAngleRot[0]) + (x7 * x11) + (-1 * x20 * (*error_state).Pose.AxisAngleRot[1]) + (2 * x3)); const FLT x26 = x16 * (*error_state).Pose.AxisAngleRot[2]; const FLT x27 = -1 * x26; const FLT x28 = x14 * ((-1 * x5 * x11) + (-1 * x10 * (*error_state).Pose.AxisAngleRot[1]) + (2 * x6) + (x12 * (*error_state).Pose.AxisAngleRot[0])); const FLT x29 = x7 * x14; const FLT x30 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x31 = 0.5 * (*error_state).IMUBias.IMUCorrection[0]; const FLT x32 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x33 = (x31 * (*_x0).IMUBias.IMUCorrection[3]) + (*_x0).IMUBias.IMUCorrection[2] + (-1 * x32 * (*error_state).IMUBias.IMUCorrection[2]) + (x30 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x34 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x35 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x36 = (-1 * x31 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x34 * (*error_state).IMUBias.IMUCorrection[1]) + (-1 * x35 * (*error_state).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[0]; const FLT x37 = (x32 * (*error_state).IMUBias.IMUCorrection[1]) + (x30 * (*error_state).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[3] + (-1 * x31 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x38 = (*_x0).IMUBias.IMUCorrection[1] + (x31 * (*_x0).IMUBias.IMUCorrection[0]) + (x34 * (*error_state).IMUBias.IMUCorrection[2]) + (-1 * x35 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x39 = (x38 * x38) + (x37 * x37) + (x33 * x33) + (x36 * x36); const FLT x40 = 1. / sqrt(x39); const FLT x41 = 1.0 * x38; const FLT x42 = 1.0 * (*error_state).IMUBias.IMUCorrection[2]; const FLT x43 = 1.0 * x33; const FLT x44 = (2 * x36) + (x41 * (*error_state).IMUBias.IMUCorrection[0]) + (x43 * (*error_state).IMUBias.IMUCorrection[1]) + (x42 * x37); const FLT x45 = 1.0/2.0 * (1. / (x39 * sqrt(x39))); const FLT x46 = x44 * x45; const FLT x47 = x40 * x31; const FLT x48 = -1 * x47; const FLT x49 = 1.0 * x37; const FLT x50 = 1.0 * x36; const FLT x51 = (-1 * x42 * x33) + (x49 * (*error_state).IMUBias.IMUCorrection[1]) + (-1 * x50 * (*error_state).IMUBias.IMUCorrection[0]) + (2 * x38); const FLT x52 = x45 * x36; const FLT x53 = 0.5 * x40; const FLT x54 = x53 * (*error_state).IMUBias.IMUCorrection[1]; const FLT x55 = -1 * x54; const FLT x56 = (-1 * x50 * (*error_state).IMUBias.IMUCorrection[1]) + (x42 * x38) + (2 * x33) + (-1 * x49 * (*error_state).IMUBias.IMUCorrection[0]); const FLT x57 = x53 * (*error_state).IMUBias.IMUCorrection[2]; const FLT x58 = -1 * x57; const FLT x59 = (-1 * x42 * x36) + (2 * x37) + (x43 * (*error_state).IMUBias.IMUCorrection[0]) + (-1 * x41 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x60 = x59 * x45; const FLT x61 = x45 * x38; const FLT x62 = x45 * x33; const FLT x63 = x45 * x37; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x5 * x15) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x5 * x22) + x18); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x5 * x25) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x5 * x28) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x29 * x13) + x17); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x21 * x29) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x7 * x25) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x7 * x28) + x24); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x3 * x15) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x3 * x22) + x27); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x3 * x25) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x3 * x28) + x17); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x6 * x15) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x6 * x22) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x6 * x25) + x18); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x6 * x28) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x46 * x36) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x52 * x51) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x52 * x56) + x55); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x60 * x36) + x58); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x46 * x38) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x61 * x51) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x61 * x56) + x57); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x60 * x38) + x55); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x46 * x33) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x62 * x51) + x58); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x62 * x56) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x60 * x33) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x63 * x44) + x57); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x63 * x51) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x63 * x56) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x63 * x59) + x40); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); } // Full version Jacobian of SurviveKalmanModelAddErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveKalmanModelAddErrorModel wrt [(*error_state).Acc[0], (*error_state).Acc[1], (*error_state).Acc[2], (*error_state).IMUBias.AccBias[0], (*error_state).IMUBias.AccBias[1], (*error_state).IMUBias.AccBias[2], (*error_state).IMUBias.AccScale[0], (*error_state).IMUBias.AccScale[1], (*error_state).IMUBias.AccScale[2], (*error_state).IMUBias.GyroBias[0], (*error_state).IMUBias.GyroBias[1], (*error_state).IMUBias.GyroBias[2], (*error_state).IMUBias.IMUCorrection[0], (*error_state).IMUBias.IMUCorrection[1], (*error_state).IMUBias.IMUCorrection[2], (*error_state).Pose.AxisAngleRot[0], (*error_state).Pose.AxisAngleRot[1], (*error_state).Pose.AxisAngleRot[2], (*error_state).Pose.Pos[0], (*error_state).Pose.Pos[1], (*error_state).Pose.Pos[2], (*error_state).Velocity.AxisAngleRot[0], (*error_state).Velocity.AxisAngleRot[1], (*error_state).Velocity.AxisAngleRot[2], (*error_state).Velocity.Pos[0], (*error_state).Velocity.Pos[1], (*error_state).Velocity.Pos[2]] static inline void SurviveKalmanModelAddErrorModel_jac_error_state(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_state) { const FLT x0 = 0.5 * (*error_state).Pose.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_state).Pose.AxisAngleRot[1]; const FLT x2 = 0.5 * (*error_state).Pose.AxisAngleRot[2]; const FLT x3 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[2]; const FLT x4 = (-1 * x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[3]) + (-1 * x1 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0]; const FLT x5 = (x2 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[1]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x6 = (*_x0).Pose.Rot[1] + (x0 * (*_x0).Pose.Rot[0]) + (-1 * x1 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x7 = (x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4); const FLT x8 = 0.5 * (1. / sqrt(x7)); const FLT x9 = x8 * (*_x0).Pose.Rot[1]; const FLT x10 = -1 * x9; const FLT x11 = 1.0 * x5; const FLT x12 = 1.0 * x6; const FLT x13 = 1.0 * x3; const FLT x14 = 1.0 * x4; const FLT x15 = (-1 * x14 * (*_x0).Pose.Rot[1]) + (x13 * (*_x0).Pose.Rot[3]) + (-1 * x11 * (*_x0).Pose.Rot[2]) + (x12 * (*_x0).Pose.Rot[0]); const FLT x16 = 1.0/2.0 * (1. / (x7 * sqrt(x7))); const FLT x17 = x15 * x16; const FLT x18 = x8 * (*_x0).Pose.Rot[2]; const FLT x19 = -1 * x18; const FLT x20 = (-1 * x14 * (*_x0).Pose.Rot[2]) + (-1 * x12 * (*_x0).Pose.Rot[3]) + (x13 * (*_x0).Pose.Rot[0]) + (x11 * (*_x0).Pose.Rot[1]); const FLT x21 = x20 * x16; const FLT x22 = x8 * (*_x0).Pose.Rot[3]; const FLT x23 = -1 * x22; const FLT x24 = (-1 * x14 * (*_x0).Pose.Rot[3]) + (x12 * (*_x0).Pose.Rot[2]) + (-1 * x13 * (*_x0).Pose.Rot[1]) + (x11 * (*_x0).Pose.Rot[0]); const FLT x25 = x24 * x16; const FLT x26 = x8 * (*_x0).Pose.Rot[0]; const FLT x27 = x3 * x16; const FLT x28 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x29 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x30 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x31 = (-1 * x30 * (*error_state).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[2] + (x29 * (*error_state).IMUBias.IMUCorrection[0]) + (x28 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x32 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x33 = (-1 * x30 * (*error_state).IMUBias.IMUCorrection[0]) + (-1 * x29 * (*error_state).IMUBias.IMUCorrection[2]) + (-1 * x32 * (*error_state).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[0]; const FLT x34 = (*_x0).IMUBias.IMUCorrection[3] + (x30 * (*error_state).IMUBias.IMUCorrection[1]) + (x28 * (*error_state).IMUBias.IMUCorrection[2]) + (-1 * x32 * (*error_state).IMUBias.IMUCorrection[0]); const FLT x35 = (x28 * (*error_state).IMUBias.IMUCorrection[0]) + (x32 * (*error_state).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[1] + (-1 * x29 * (*error_state).IMUBias.IMUCorrection[1]); const FLT x36 = (x35 * x35) + (x34 * x34) + (x31 * x31) + (x33 * x33); const FLT x37 = 1. / sqrt(x36); const FLT x38 = x30 * x37; const FLT x39 = -1 * x38; const FLT x40 = 1.0 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x41 = 1.0 * x34; const FLT x42 = 1.0 * x31; const FLT x43 = 1.0 * x33; const FLT x44 = (-1 * x43 * (*_x0).IMUBias.IMUCorrection[1]) + (x42 * (*_x0).IMUBias.IMUCorrection[3]) + (x40 * x35) + (-1 * x41 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x45 = 1.0/2.0 * (1. / (x36 * sqrt(x36))); const FLT x46 = x45 * x33; const FLT x47 = x32 * x37; const FLT x48 = -1 * x47; const FLT x49 = 1.0 * x35; const FLT x50 = x45 * ((-1 * x43 * (*_x0).IMUBias.IMUCorrection[2]) + (x41 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x49 * (*_x0).IMUBias.IMUCorrection[3]) + (x40 * x31)); const FLT x51 = x37 * x29; const FLT x52 = -1 * x51; const FLT x53 = (-1 * x43 * (*_x0).IMUBias.IMUCorrection[3]) + (x40 * x34) + (x49 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x42 * (*_x0).IMUBias.IMUCorrection[1]); const FLT x54 = x37 * x28; const FLT x55 = x44 * x45; const FLT x56 = x53 * x45; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x4 * x17) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x4 * x21) + x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x4 * x25) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x17) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x21) + x23); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x25) + x18); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x27 * x15) + x22); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x20 * x27) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x24 * x27) + x10); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x5 * x17) + x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x5 * x21) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x5 * x25) + x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x44 * x46) + x39); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x33) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x46) + x52); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x55 * x35) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x35) + x52); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x35) + x47); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x55 * x31) + x51); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x31) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x31) + x39); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x55 * x34) + x48); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x34) + x38); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x34) + x54); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); } // Full version Jacobian of SurviveKalmanModelAddErrorModel wrt [(*error_state).Acc[0], (*error_state).Acc[1], (*error_state).Acc[2], (*error_state).IMUBias.AccBias[0], (*error_state).IMUBias.AccBias[1], (*error_state).IMUBias.AccBias[2], (*error_state).IMUBias.AccScale[0], (*error_state).IMUBias.AccScale[1], (*error_state).IMUBias.AccScale[2], (*error_state).IMUBias.GyroBias[0], (*error_state).IMUBias.GyroBias[1], (*error_state).IMUBias.GyroBias[2], (*error_state).IMUBias.IMUCorrection[0], (*error_state).IMUBias.IMUCorrection[1], (*error_state).IMUBias.IMUCorrection[2], (*error_state).Pose.AxisAngleRot[0], (*error_state).Pose.AxisAngleRot[1], (*error_state).Pose.AxisAngleRot[2], (*error_state).Pose.Pos[0], (*error_state).Pose.Pos[1], (*error_state).Pose.Pos[2], (*error_state).Velocity.AxisAngleRot[0], (*error_state).Velocity.AxisAngleRot[1], (*error_state).Velocity.AxisAngleRot[2], (*error_state).Velocity.Pos[0], (*error_state).Velocity.Pos[1], (*error_state).Velocity.Pos[2]] static inline void SurviveKalmanModelPredict(SurviveKalmanModel* out, const FLT t, const SurviveKalmanModel* kalman_model) { const FLT x0 = t * (*kalman_model).Acc[0]; const FLT x1 = 1.0/2.0 * fabs(t); const FLT x2 = t * (*kalman_model).Acc[1]; const FLT x3 = t * (*kalman_model).Acc[2]; const FLT x4 = t * t; const FLT x5 = x4 * ((*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Velocity.AxisAngleRot[2]); const FLT x6 = x4 * ((*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Velocity.AxisAngleRot[0]); const FLT x7 = x4 * ((*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Velocity.AxisAngleRot[1]); const FLT x8 = 1e-10 + x7 + x5 + x6; const FLT x9 = sqrt(x8); const FLT x10 = 0.5 * x9; const FLT x11 = sin(x10); const FLT x12 = (1. / x8) * (x11 * x11); const FLT x13 = cos(x10); const FLT x14 = 1. / sqrt((x6 * x12) + (x13 * x13) + (x5 * x12) + (x7 * x12)); const FLT x15 = t * (1. / x9) * x14 * x11; const FLT x16 = x15 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x17 = x15 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x18 = x14 * x13; const FLT x19 = x15 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x20 = x15 * (*kalman_model).Pose.Rot[0]; out->Pose.Pos[0]=(x0 * x1) + (t * (*kalman_model).Velocity.Pos[0]) + (*kalman_model).Pose.Pos[0]; out->Pose.Pos[1]=(t * (*kalman_model).Velocity.Pos[1]) + (x2 * x1) + (*kalman_model).Pose.Pos[1]; out->Pose.Pos[2]=(t * (*kalman_model).Velocity.Pos[2]) + (x1 * x3) + (*kalman_model).Pose.Pos[2]; out->Pose.Rot[0]=(-1 * x19 * (*kalman_model).Pose.Rot[1]) + (x18 * (*kalman_model).Pose.Rot[0]) + (-1 * x16 * (*kalman_model).Pose.Rot[3]) + (-1 * x17 * (*kalman_model).Pose.Rot[2]); out->Pose.Rot[1]=(x20 * (*kalman_model).Velocity.AxisAngleRot[0]) + (x18 * (*kalman_model).Pose.Rot[1]) + (-1 * x16 * (*kalman_model).Pose.Rot[2]) + (x17 * (*kalman_model).Pose.Rot[3]); out->Pose.Rot[2]=(-1 * x19 * (*kalman_model).Pose.Rot[3]) + (x18 * (*kalman_model).Pose.Rot[2]) + (x20 * (*kalman_model).Velocity.AxisAngleRot[1]) + (x16 * (*kalman_model).Pose.Rot[1]); out->Pose.Rot[3]=(x19 * (*kalman_model).Pose.Rot[2]) + (x18 * (*kalman_model).Pose.Rot[3]) + (x20 * (*kalman_model).Velocity.AxisAngleRot[2]) + (-1 * x17 * (*kalman_model).Pose.Rot[1]); out->Velocity.Pos[0]=(*kalman_model).Velocity.Pos[0] + x0; out->Velocity.Pos[1]=(*kalman_model).Velocity.Pos[1] + x2; out->Velocity.Pos[2]=(*kalman_model).Velocity.Pos[2] + x3; out->Velocity.AxisAngleRot[0]=(*kalman_model).Velocity.AxisAngleRot[0]; out->Velocity.AxisAngleRot[1]=(*kalman_model).Velocity.AxisAngleRot[1]; out->Velocity.AxisAngleRot[2]=(*kalman_model).Velocity.AxisAngleRot[2]; out->Acc[0]=(*kalman_model).Acc[0]; out->Acc[1]=(*kalman_model).Acc[1]; out->Acc[2]=(*kalman_model).Acc[2]; out->IMUBias=(*kalman_model).IMUBias; } // Jacobian of SurviveKalmanModelPredict wrt [t] static inline void SurviveKalmanModelPredict_jac_t(CnMat* Hx, const FLT t, const SurviveKalmanModel* kalman_model) { const FLT x0 = 1.0/2.0 * t; const FLT x1 = x0 * ((t) > 0 ? 1 : -1) /* Note: Maybe not valid for == 0 */; const FLT x2 = fabs(t); const FLT x3 = 1.0/2.0 * x2; const FLT x4 = t * t; const FLT x5 = (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x6 = x4 * x5; const FLT x7 = (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x8 = x4 * x7; const FLT x9 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x10 = x4 * x9; const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = 1. / x11; const FLT x13 = sqrt(x11); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = x15 * x15; const FLT x17 = x12 * x16; const FLT x18 = cos(x14); const FLT x19 = (x8 * x17) + (x6 * x17) + (x18 * x18) + (x10 * x17); const FLT x20 = 1. / sqrt(x19); const FLT x21 = x15 * (1. / x13); const FLT x22 = x21 * (*kalman_model).Pose.Rot[1]; const FLT x23 = x22 * x20; const FLT x24 = x18 * (*kalman_model).Pose.Rot[2]; const FLT x25 = 2 * t; const FLT x26 = x9 * x25; const FLT x27 = x5 * x25; const FLT x28 = x7 * x25; const FLT x29 = x28 + x26 + x27; const FLT x30 = 0.25 * x29; const FLT x31 = t * x30 * x12; const FLT x32 = x31 * x24; const FLT x33 = x32 * x20; const FLT x34 = x15 * (1. / (x11 * sqrt(x11))); const FLT x35 = x0 * x34 * x29; const FLT x36 = x35 * x20; const FLT x37 = x36 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x38 = (1. / (x11 * x11)) * x16; const FLT x39 = x38 * x29; const FLT x40 = 0.5 * x18; const FLT x41 = x40 * x29; const FLT x42 = x41 * x34; const FLT x43 = x29 * x10; const FLT x44 = (1. / (x19 * sqrt(x19))) * ((x6 * x42) + (x26 * x17) + (-1 * x6 * x39) + (x40 * x43 * x34) + (x27 * x17) + (x28 * x17) + (-1 * x43 * x38) + (-1 * x8 * x39) + (x8 * x42) + (-1 * x41 * x21)); const FLT x45 = x0 * x44; const FLT x46 = x45 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x47 = x46 * x21; const FLT x48 = x20 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x49 = x48 * (*kalman_model).Pose.Rot[3]; const FLT x50 = x31 * x18; const FLT x51 = x45 * x21; const FLT x52 = x51 * (*kalman_model).Pose.Rot[3]; const FLT x53 = x50 * x20; const FLT x54 = x53 * (*kalman_model).Pose.Rot[1]; const FLT x55 = 1.0/2.0 * x44; const FLT x56 = x55 * x18; const FLT x57 = x48 * x21; const FLT x58 = x20 * x21; const FLT x59 = x58 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x60 = x58 * (*kalman_model).Pose.Rot[0]; const FLT x61 = x45 * x22; const FLT x62 = x36 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x63 = (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Pose.Rot[0]; const FLT x64 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[3]; const FLT x65 = x51 * (*kalman_model).Pose.Rot[2]; const FLT x66 = x48 * x35; const FLT x67 = x58 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x68 = x50 * x48; const FLT x69 = x58 * x30; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 0, (x3 * (*kalman_model).Acc[0]) + (*kalman_model).Velocity.Pos[0] + (x1 * (*kalman_model).Acc[0])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 0, (x3 * (*kalman_model).Acc[1]) + (*kalman_model).Velocity.Pos[1] + (x1 * (*kalman_model).Acc[1])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 0, (x3 * (*kalman_model).Acc[2]) + (*kalman_model).Velocity.Pos[2] + (x1 * (*kalman_model).Acc[2])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), 0, (-1 * x60 * x30) + (x47 * (*kalman_model).Pose.Rot[2]) + (-1 * x57 * (*kalman_model).Pose.Rot[3]) + (-1 * x50 * x49) + (x61 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x59 * (*kalman_model).Pose.Rot[2]) + (-1 * x23 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x33 * (*kalman_model).Velocity.AxisAngleRot[1]) + (x52 * (*kalman_model).Velocity.AxisAngleRot[2]) + (x62 * (*kalman_model).Pose.Rot[1]) + (-1 * x56 * (*kalman_model).Pose.Rot[0]) + (x49 * x35) + (x37 * (*kalman_model).Pose.Rot[2]) + (-1 * x54 * (*kalman_model).Velocity.AxisAngleRot[0])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), 0, (x59 * (*kalman_model).Pose.Rot[3]) + (-1 * x47 * (*kalman_model).Pose.Rot[3]) + (-1 * x56 * (*kalman_model).Pose.Rot[1]) + (x66 * (*kalman_model).Pose.Rot[2]) + (x64 * x53) + (-1 * x48 * x32) + (x63 * x53) + (x60 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x57 * (*kalman_model).Pose.Rot[2]) + (-1 * x62 * (*kalman_model).Pose.Rot[0]) + (x65 * (*kalman_model).Velocity.AxisAngleRot[2]) + (-1 * x30 * x23) + (-1 * x63 * x51) + (-1 * x64 * x36)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), 0, (-1 * x53 * (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Pose.Rot[3]) + (-1 * x37 * (*kalman_model).Pose.Rot[0]) + (-1 * x47 * (*kalman_model).Pose.Rot[0]) + (x52 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x55 * x24) + (x53 * (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[0]) + (x60 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x61 * (*kalman_model).Velocity.AxisAngleRot[2]) + (x68 * (*kalman_model).Pose.Rot[1]) + (-1 * x69 * (*kalman_model).Pose.Rot[2]) + (x48 * x22) + (x62 * (*kalman_model).Pose.Rot[3]) + (-1 * x67 * (*kalman_model).Pose.Rot[3]) + (-1 * x66 * (*kalman_model).Pose.Rot[1])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), 0, (x33 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x56 * (*kalman_model).Pose.Rot[3]) + (-1 * x66 * (*kalman_model).Pose.Rot[0]) + (-1 * x51 * (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Pose.Rot[0]) + (-1 * x65 * (*kalman_model).Velocity.AxisAngleRot[0]) + (x46 * x22) + (x67 * (*kalman_model).Pose.Rot[2]) + (-1 * x54 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x62 * (*kalman_model).Pose.Rot[2]) + (-1 * x69 * (*kalman_model).Pose.Rot[3]) + (x37 * (*kalman_model).Pose.Rot[1]) + (x68 * (*kalman_model).Pose.Rot[0]) + (-1 * x23 * (*kalman_model).Velocity.AxisAngleRot[1]) + (x57 * (*kalman_model).Pose.Rot[0])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), 0, (*kalman_model).Acc[0]); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), 0, (*kalman_model).Acc[1]); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), 0, (*kalman_model).Acc[2]); } // Full version Jacobian of SurviveKalmanModelPredict wrt [t] // Jacobian of SurviveKalmanModelPredict wrt [(*kalman_model).Acc[0], (*kalman_model).Acc[1], (*kalman_model).Acc[2], (*kalman_model).IMUBias.AccBias[0], (*kalman_model).IMUBias.AccBias[1], (*kalman_model).IMUBias.AccBias[2], (*kalman_model).IMUBias.AccScale[0], (*kalman_model).IMUBias.AccScale[1], (*kalman_model).IMUBias.AccScale[2], (*kalman_model).IMUBias.GyroBias[0], (*kalman_model).IMUBias.GyroBias[1], (*kalman_model).IMUBias.GyroBias[2], (*kalman_model).IMUBias.IMUCorrection[0], (*kalman_model).IMUBias.IMUCorrection[1], (*kalman_model).IMUBias.IMUCorrection[2], (*kalman_model).IMUBias.IMUCorrection[3], (*kalman_model).Pose.Pos[0], (*kalman_model).Pose.Pos[1], (*kalman_model).Pose.Pos[2], (*kalman_model).Pose.Rot[0], (*kalman_model).Pose.Rot[1], (*kalman_model).Pose.Rot[2], (*kalman_model).Pose.Rot[3], (*kalman_model).Velocity.AxisAngleRot[0], (*kalman_model).Velocity.AxisAngleRot[1], (*kalman_model).Velocity.AxisAngleRot[2], (*kalman_model).Velocity.Pos[0], (*kalman_model).Velocity.Pos[1], (*kalman_model).Velocity.Pos[2]] static inline void SurviveKalmanModelPredict_jac_kalman_model(CnMat* Hx, const FLT t, const SurviveKalmanModel* kalman_model) { const FLT x0 = 1.0/2.0 * t; const FLT x1 = fabs(t) * x0; const FLT x2 = t * t; const FLT x3 = (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x4 = x2 * x3; const FLT x5 = (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x6 = x2 * x5; const FLT x7 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x8 = x2 * x7; const FLT x9 = 1e-10 + x8 + x4 + x6; const FLT x10 = 1. / x9; const FLT x11 = sqrt(x9); const FLT x12 = 0.5 * x11; const FLT x13 = sin(x12); const FLT x14 = x13 * x13; const FLT x15 = x14 * x10; const FLT x16 = cos(x12); const FLT x17 = (x6 * x15) + (x16 * x16) + (x4 * x15) + (x8 * x15); const FLT x18 = 1. / sqrt(x17); const FLT x19 = x18 * x16; const FLT x20 = x13 * x18; const FLT x21 = 1. / x11; const FLT x22 = t * x21; const FLT x23 = x22 * x20; const FLT x24 = x23 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x25 = -1 * x24; const FLT x26 = x23 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x27 = -1 * x26; const FLT x28 = x23 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x29 = -1 * x28; const FLT x30 = t * t * t; const FLT x31 = 0.5 * x30 * x10 * x19; const FLT x32 = x31 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x33 = x32 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x34 = x33 * (*kalman_model).Pose.Rot[2]; const FLT x35 = 1. / (x17 * sqrt(x17)); const FLT x36 = 2 * x15; const FLT x37 = x2 * x36; const FLT x38 = t * t * t * t; const FLT x39 = x38 * ((*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Velocity.AxisAngleRot[0]); const FLT x40 = 2 * (1. / (x9 * x9)) * x14; const FLT x41 = 1. / (x9 * sqrt(x9)); const FLT x42 = 1.0 * x13 * x16; const FLT x43 = x41 * x42; const FLT x44 = x43 * x38; const FLT x45 = x3 * x44; const FLT x46 = x40 * x38; const FLT x47 = x3 * x46; const FLT x48 = x7 * x46; const FLT x49 = x7 * x44; const FLT x50 = x42 * x21; const FLT x51 = x2 * x50; const FLT x52 = ((-1 * x51 * (*kalman_model).Velocity.AxisAngleRot[0]) + (x49 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x48 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x40 * x39) + (x37 * (*kalman_model).Velocity.AxisAngleRot[0]) + (x43 * x39) + (x45 * (*kalman_model).Velocity.AxisAngleRot[0]) + (-1 * x47 * (*kalman_model).Velocity.AxisAngleRot[0])) * x35; const FLT x53 = x0 * x21 * x13; const FLT x54 = x53 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x55 = x54 * x52; const FLT x56 = x20 * (*kalman_model).Pose.Rot[0]; const FLT x57 = 0.5 * x21; const FLT x58 = x2 * x57; const FLT x59 = x58 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x60 = x53 * x52; const FLT x61 = x60 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x62 = x20 * (*kalman_model).Pose.Rot[2]; const FLT x63 = x41 * x30; const FLT x64 = x63 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x65 = x64 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x66 = x62 * x65; const FLT x67 = 1.0/2.0 * x16; const FLT x68 = x67 * x52; const FLT x69 = x5 * x31; const FLT x70 = x23 * (*kalman_model).Pose.Rot[1]; const FLT x71 = -1 * x70; const FLT x72 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[2]; const FLT x73 = x5 * x63; const FLT x74 = x20 * (*kalman_model).Pose.Rot[1]; const FLT x75 = x20 * (*kalman_model).Pose.Rot[3]; const FLT x76 = x64 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x77 = x31 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x78 = x77 * (*kalman_model).Pose.Rot[3]; const FLT x79 = (-1 * x78 * (*kalman_model).Velocity.AxisAngleRot[0]) + (x75 * x76); const FLT x80 = x23 * (*kalman_model).Pose.Rot[2]; const FLT x81 = -1 * x80; const FLT x82 = x7 * x63; const FLT x83 = x78 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x84 = x5 * x46; const FLT x85 = x5 * x44; const FLT x86 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x87 = ((x37 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x86 * x46) + (-1 * x51 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x84 * (*kalman_model).Velocity.AxisAngleRot[1]) + (x86 * x44) + (x85 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x47 * (*kalman_model).Velocity.AxisAngleRot[1]) + (x45 * (*kalman_model).Velocity.AxisAngleRot[1])) * x35; const FLT x88 = x87 * x53; const FLT x89 = x88 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x90 = x7 * x31; const FLT x91 = x58 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x92 = x75 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x93 = x63 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x94 = x93 * x92; const FLT x95 = x87 * x54; const FLT x96 = x87 * x67; const FLT x97 = x74 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x98 = (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Pose.Rot[1]; const FLT x99 = (-1 * x98 * x32) + (x64 * x97); const FLT x100 = x74 * x76; const FLT x101 = x2 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x102 = (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x103 = ((-1 * x48 * (*kalman_model).Velocity.AxisAngleRot[2]) + (x49 * (*kalman_model).Velocity.AxisAngleRot[2]) + (-1 * x46 * x102) + (-1 * x50 * x101) + (x85 * (*kalman_model).Velocity.AxisAngleRot[2]) + (-1 * x84 * (*kalman_model).Velocity.AxisAngleRot[2]) + (x36 * x101) + (x44 * x102)) * x35; const FLT x104 = x103 * (*kalman_model).Pose.Rot[3]; const FLT x105 = x53 * x104; const FLT x106 = x53 * x103; const FLT x107 = x106 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x108 = x3 * x63; const FLT x109 = x23 * (*kalman_model).Pose.Rot[3]; const FLT x110 = -1 * x109; const FLT x111 = x57 * x101; const FLT x112 = x67 * x103; const FLT x113 = x3 * x31; const FLT x114 = x77 * x98; const FLT x115 = (*kalman_model).Velocity.AxisAngleRot[2] * (*kalman_model).Pose.Rot[2]; const FLT x116 = x93 * (*kalman_model).Velocity.AxisAngleRot[1]; const FLT x117 = (x62 * x116) + (-1 * x32 * x115); const FLT x118 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[3]; const FLT x119 = x33 * (*kalman_model).Pose.Rot[3]; const FLT x120 = x76 * x62; const FLT x121 = x75 * x65; const FLT x122 = x31 * (*kalman_model).Velocity.AxisAngleRot[0]; const FLT x123 = x115 * x122; const FLT x124 = x56 * x22; const FLT x125 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[0]; const FLT x126 = (x122 * x125) + (-1 * x65 * x56); const FLT x127 = (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Pose.Rot[0]; const FLT x128 = (x77 * x127) + (-1 * x76 * x56); const FLT x129 = x67 * (*kalman_model).Pose.Rot[2]; const FLT x130 = (*kalman_model).Velocity.AxisAngleRot[1] * (*kalman_model).Pose.Rot[1]; const FLT x131 = x77 * x130; const FLT x132 = x93 * x97; const FLT x133 = x106 * (*kalman_model).Velocity.AxisAngleRot[2]; const FLT x134 = (x77 * x125) + (-1 * x56 * x116); const FLT x135 = x54 * (*kalman_model).Pose.Rot[2]; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x25); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x27); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x29); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x79 + (x72 * x60) + x71 + (-1 * x69 * (*kalman_model).Pose.Rot[1]) + (-1 * x68 * (*kalman_model).Pose.Rot[0]) + (x55 * (*kalman_model).Pose.Rot[1]) + (-1 * x34) + (x73 * x74) + (-1 * x56 * x59) + (x61 * (*kalman_model).Pose.Rot[3]) + x66); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (x95 * (*kalman_model).Pose.Rot[1]) + x94 + (x82 * x62) + (-1 * x83) + (x88 * x72) + (-1 * x90 * (*kalman_model).Pose.Rot[2]) + x99 + (-1 * x96 * (*kalman_model).Pose.Rot[0]) + (-1 * x56 * x91) + x81 + (x89 * (*kalman_model).Pose.Rot[3])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (x98 * x106) + (-1 * x113 * (*kalman_model).Pose.Rot[3]) + (-1 * x56 * x111) + (-1 * x114) + (x105 * (*kalman_model).Velocity.AxisAngleRot[2]) + (-1 * x112 * (*kalman_model).Pose.Rot[0]) + (x75 * x108) + x100 + x117 + (x107 * (*kalman_model).Pose.Rot[2]) + x110); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x24); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x29); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x120 + (x69 * (*kalman_model).Pose.Rot[0]) + (-1 * x123) + (-1 * x73 * x56) + x119 + (-1 * x121) + (-1 * x55 * (*kalman_model).Pose.Rot[0]) + (-1 * x60 * x118) + (-1 * x74 * x59) + x124 + (-1 * x68 * (*kalman_model).Pose.Rot[1]) + (x60 * x115)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x126 + x117 + (-1 * x88 * x118) + x109 + (x88 * x115) + (-1 * x95 * (*kalman_model).Pose.Rot[0]) + (-1 * x96 * (*kalman_model).Pose.Rot[1]) + (x90 * (*kalman_model).Pose.Rot[3]) + (-1 * x58 * x97) + (-1 * x82 * x75)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x128 + (-1 * x105 * (*kalman_model).Velocity.AxisAngleRot[1]) + (-1 * x113 * (*kalman_model).Pose.Rot[2]) + (-1 * x112 * (*kalman_model).Pose.Rot[1]) + (-1 * x94) + (-1 * x74 * x111) + (x106 * x115) + x83 + (-1 * x106 * x127) + (x62 * x108) + x81); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x26); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x28); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x25); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x126 + (-1 * x62 * x59) + (-1 * x100) + (-1 * x69 * (*kalman_model).Pose.Rot[3]) + (-1 * x52 * x129) + x110 + (-1 * x60 * x125) + (x55 * (*kalman_model).Pose.Rot[3]) + x114 + (x73 * x75) + (-1 * x61 * (*kalman_model).Pose.Rot[1])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (x95 * (*kalman_model).Pose.Rot[3]) + x131 + (-1 * x87 * x129) + (-1 * x88 * x125) + (-1 * x82 * x56) + (-1 * x119) + x121 + (-1 * x62 * x91) + (x90 * (*kalman_model).Pose.Rot[0]) + (-1 * x132) + x124 + (-1 * x89 * (*kalman_model).Pose.Rot[1])); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x79 + x134 + (-1 * x62 * x111) + (-1 * x133 * (*kalman_model).Pose.Rot[1]) + (x54 * x104) + (-1 * x107 * (*kalman_model).Pose.Rot[0]) + (x113 * (*kalman_model).Pose.Rot[1]) + x70 + (-1 * x74 * x108) + (-1 * x103 * x129)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x28); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x27); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x24); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x128 + x80 + (x69 * (*kalman_model).Pose.Rot[2]) + (-1 * x68 * (*kalman_model).Pose.Rot[3]) + (-1 * x73 * x62) + x99 + (x60 * x130) + (-1 * x52 * x135) + (-1 * x61 * (*kalman_model).Pose.Rot[0]) + (-1 * x75 * x59)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x134 + x34 + (x82 * x74) + (-1 * x58 * x92) + x71 + (-1 * x87 * x135) + (-1 * x89 * (*kalman_model).Pose.Rot[0]) + (-1 * x96 * (*kalman_model).Pose.Rot[3]) + (-1 * x66) + (-1 * x90 * (*kalman_model).Pose.Rot[1]) + (x88 * x130)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (x113 * (*kalman_model).Pose.Rot[0]) + (-1 * x120) + (-1 * x131) + x123 + (-1 * x67 * x104) + (-1 * x133 * (*kalman_model).Pose.Rot[0]) + x132 + (-1 * x75 * x111) + (-1 * x106 * (*kalman_model).Velocity.AxisAngleRot[0] * (*kalman_model).Pose.Rot[2]) + x124 + (x107 * (*kalman_model).Pose.Rot[1]) + (-1 * x56 * x108)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), 1); } // Full version Jacobian of SurviveKalmanModelPredict wrt [(*kalman_model).Acc[0], (*kalman_model).Acc[1], (*kalman_model).Acc[2], (*kalman_model).IMUBias.AccBias[0], (*kalman_model).IMUBias.AccBias[1], (*kalman_model).IMUBias.AccBias[2], (*kalman_model).IMUBias.AccScale[0], (*kalman_model).IMUBias.AccScale[1], (*kalman_model).IMUBias.AccScale[2], (*kalman_model).IMUBias.GyroBias[0], (*kalman_model).IMUBias.GyroBias[1], (*kalman_model).IMUBias.GyroBias[2], (*kalman_model).IMUBias.IMUCorrection[0], (*kalman_model).IMUBias.IMUCorrection[1], (*kalman_model).IMUBias.IMUCorrection[2], (*kalman_model).IMUBias.IMUCorrection[3], (*kalman_model).Pose.Pos[0], (*kalman_model).Pose.Pos[1], (*kalman_model).Pose.Pos[2], (*kalman_model).Pose.Rot[0], (*kalman_model).Pose.Rot[1], (*kalman_model).Pose.Rot[2], (*kalman_model).Pose.Rot[3], (*kalman_model).Velocity.AxisAngleRot[0], (*kalman_model).Velocity.AxisAngleRot[1], (*kalman_model).Velocity.AxisAngleRot[2], (*kalman_model).Velocity.Pos[0], (*kalman_model).Velocity.Pos[1], (*kalman_model).Velocity.Pos[2]] static inline void AxisAngleFlip(CnMat* out, const FLT* _x0) { const FLT _x00 = _x0[0]; const FLT _x01 = _x0[1]; const FLT _x02 = _x0[2]; const FLT x0 = sqrt(1e-10 + (_x01 * _x01) + (_x02 * _x02) + (_x00 * _x00)); const FLT x1 = (1. / x0) * (-6.28318530717959 + x0); cnMatrixOptionalSet(out, 0, 0, x1 * _x00); cnMatrixOptionalSet(out, 1, 0, x1 * _x01); cnMatrixOptionalSet(out, 2, 0, x1 * _x02); } // Jacobian of AxisAngleFlip wrt [_x00, _x01, _x02] static inline void AxisAngleFlip_jac_x0(CnMat* Hx, const FLT* _x0) { const FLT _x00 = _x0[0]; const FLT _x01 = _x0[1]; const FLT _x02 = _x0[2]; const FLT x0 = _x00 * _x00; const FLT x1 = 1e-10 + (_x01 * _x01) + (_x02 * _x02) + x0; const FLT x2 = 1. / x1; const FLT x3 = sqrt(x1); const FLT x4 = -6.28318530717959 + x3; const FLT x5 = (1. / (x1 * sqrt(x1))) * x4; const FLT x6 = x2 * _x00; const FLT x7 = x5 * _x00; cnMatrixOptionalSet(Hx, 0, 0, (-1 * x0 * x5) + (x0 * x2) + (x4 * (1. / x3))); cnMatrixOptionalSet(Hx, 0, 1, (-1 * x7 * _x01) + (x6 * _x01)); cnMatrixOptionalSet(Hx, 0, 2, (-1 * x7 * _x02) + (x6 * _x02)); } // Full version Jacobian of AxisAngleFlip wrt [_x00, _x01, _x02] static inline void AxisAngleFlip_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT* _x0) { if(hx != 0) { AxisAngleFlip(hx, _x0); } if(Hx != 0) { AxisAngleFlip_jac_x0(Hx, _x0); } } static inline void SurviveKalmanModelErrorPredict(SurviveKalmanErrorModel* out, const FLT t, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = t * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x1 = 1.0/2.0 * fabs(t); const FLT x2 = t * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x3 = t * ((*_x0).Acc[2] + (*error_model).Acc[2]); const FLT x4 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x6 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x8 = (x7 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x5 * (*_x0).Pose.Rot[2]) + (x6 * (*_x0).Pose.Rot[0]); const FLT x9 = t * t; const FLT x10 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x11 = x9 * (x10 * x10); const FLT x12 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x13 = x9 * (x12 * x12); const FLT x14 = (x4 * x4) * x9; const FLT x15 = 1e-10 + x14 + x11 + x13; const FLT x16 = sqrt(x15); const FLT x17 = 0.5 * x16; const FLT x18 = sin(x17); const FLT x19 = (-1 * x6 * (*_x0).Pose.Rot[1]) + (x7 * (*_x0).Pose.Rot[0]) + (x5 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x20 = (-1 * x7 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x5 * (*_x0).Pose.Rot[1]) + (-1 * x6 * (*_x0).Pose.Rot[3]); const FLT x21 = (*_x0).Pose.Rot[1] + (x5 * (*_x0).Pose.Rot[0]) + (-1 * x7 * (*_x0).Pose.Rot[3]) + (x6 * (*_x0).Pose.Rot[2]); const FLT x22 = (1. / x15) * (x18 * x18); const FLT x23 = cos(x17); const FLT x24 = (1. / sqrt((x21 * x21) + (x20 * x20) + (x8 * x8) + (x19 * x19))) * (1. / sqrt((x23 * x23) + (x22 * x11) + (x22 * x13) + (x22 * x14))); const FLT x25 = t * x24 * x18 * (1. / x16); const FLT x26 = x8 * x25; const FLT x27 = x24 * x23; const FLT x28 = x25 * x19; const FLT x29 = x25 * x20; const FLT x30 = (x29 * x12) + (-1 * x28 * x10) + (x4 * x26) + (x21 * x27); const FLT x31 = x25 * x12; const FLT x32 = (-1 * x31 * x21) + (x20 * x27) + (-1 * x26 * x10) + (-1 * x4 * x28); const FLT x33 = x25 * x21; const FLT x34 = (x29 * x10) + (-1 * x4 * x33) + (x28 * x12) + (x8 * x27); const FLT x35 = (x4 * x29) + (x33 * x10) + (-1 * x8 * x31) + (x27 * x19); const FLT x36 = (x34 * (*_x0).Pose.Rot[0]) + (x30 * (*_x0).Pose.Rot[2]) + (-1 * x35 * (*_x0).Pose.Rot[1]) + (-1 * x32 * (*_x0).Pose.Rot[3]); const FLT x37 = (x34 * (*_x0).Pose.Rot[1]) + (x35 * (*_x0).Pose.Rot[0]) + (-1 * x30 * (*_x0).Pose.Rot[3]) + (-1 * x32 * (*_x0).Pose.Rot[2]); const FLT x38 = (x35 * (*_x0).Pose.Rot[3]) + (-1 * x34 * (*_x0).Pose.Rot[2]) + (-1 * x32 * (*_x0).Pose.Rot[1]) + (x30 * (*_x0).Pose.Rot[0]); const FLT x39 = (x30 * (*_x0).Pose.Rot[1]) + (x32 * (*_x0).Pose.Rot[0]) + (x35 * (*_x0).Pose.Rot[2]) + (x34 * (*_x0).Pose.Rot[3]); const FLT x40 = x37 * x37; const FLT x41 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x42 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x43 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x44 = (-1 * x43 * (*error_model).IMUBias.IMUCorrection[2]) + (x42 * (*error_model).IMUBias.IMUCorrection[1]) + (x41 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x45 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x46 = (x42 * (*error_model).IMUBias.IMUCorrection[2]) + (x43 * (*error_model).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[3] + (-1 * x45 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x47 = (-1 * x43 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[0] + (-1 * x41 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x45 * (*error_model).IMUBias.IMUCorrection[1]); const FLT x48 = (*_x0).IMUBias.IMUCorrection[1] + (x45 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x41 * (*error_model).IMUBias.IMUCorrection[1]) + (x42 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x49 = 1. / sqrt((x48 * x48) + (x47 * x47) + (x44 * x44) + (x46 * x46)); const FLT x50 = x47 * x49; const FLT x51 = x48 * x49; const FLT x52 = x44 * x49; const FLT x53 = x46 * x49; const FLT x54 = (x52 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x50 * (*_x0).IMUBias.IMUCorrection[2]) + (x53 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x51 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x55 = (-1 * x50 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x52 * (*_x0).IMUBias.IMUCorrection[1]) + (x53 * (*_x0).IMUBias.IMUCorrection[0]) + (x51 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x56 = (-1 * x50 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x53 * (*_x0).IMUBias.IMUCorrection[2]) + (x52 * (*_x0).IMUBias.IMUCorrection[3]) + (x51 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x57 = (x51 * (*_x0).IMUBias.IMUCorrection[1]) + (x50 * (*_x0).IMUBias.IMUCorrection[0]) + (x53 * (*_x0).IMUBias.IMUCorrection[3]) + (x52 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x58 = x54 * x54; out->Pose.Pos[0]=(t * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])) + (*error_model).Pose.Pos[0] + (x0 * x1); out->Pose.Pos[1]=(t * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (*error_model).Pose.Pos[1] + (x2 * x1); out->Pose.Pos[2]=(t * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + (x1 * x3); out->Pose.AxisAngleRot[0]=atan2(2 * ((x38 * x39) + (x36 * x37)), 1 + (-2 * ((x38 * x38) + x40))); out->Pose.AxisAngleRot[1]=asin(2 * ((x37 * x39) + (-1 * x36 * x38))); out->Pose.AxisAngleRot[2]=atan2(2 * ((x36 * x39) + (x38 * x37)), 1 + (-2 * (x40 + (x36 * x36)))); out->Velocity.Pos[0]=x0 + (*error_model).Velocity.Pos[0]; out->Velocity.Pos[1]=(*error_model).Velocity.Pos[1] + x2; out->Velocity.Pos[2]=x3 + (*error_model).Velocity.Pos[2]; out->Velocity.AxisAngleRot[0]=(*error_model).Velocity.AxisAngleRot[0]; out->Velocity.AxisAngleRot[1]=(*error_model).Velocity.AxisAngleRot[1]; out->Velocity.AxisAngleRot[2]=(*error_model).Velocity.AxisAngleRot[2]; out->Acc[0]=(*error_model).Acc[0]; out->Acc[1]=(*error_model).Acc[1]; out->Acc[2]=(*error_model).Acc[2]; out->IMUBias.AccScale[0]=(*error_model).IMUBias.AccScale[0]; out->IMUBias.AccScale[1]=(*error_model).IMUBias.AccScale[1]; out->IMUBias.AccScale[2]=(*error_model).IMUBias.AccScale[2]; out->IMUBias.IMUCorrection[0]=atan2(2 * ((x57 * x56) + (x54 * x55)), 1 + (-2 * ((x56 * x56) + x58))); out->IMUBias.IMUCorrection[1]=asin(2 * ((x54 * x57) + (-1 * x56 * x55))); out->IMUBias.IMUCorrection[2]=atan2(2 * ((x57 * x55) + (x54 * x56)), 1 + (-2 * (x58 + (x55 * x55)))); out->IMUBias.AccBias[0]=(*error_model).IMUBias.AccBias[0]; out->IMUBias.AccBias[1]=(*error_model).IMUBias.AccBias[1]; out->IMUBias.AccBias[2]=(*error_model).IMUBias.AccBias[2]; out->IMUBias.GyroBias[0]=(*error_model).IMUBias.GyroBias[0]; out->IMUBias.GyroBias[1]=(*error_model).IMUBias.GyroBias[1]; out->IMUBias.GyroBias[2]=(*error_model).IMUBias.GyroBias[2]; } // Jacobian of SurviveKalmanModelErrorPredict wrt [t] static inline void SurviveKalmanModelErrorPredict_jac_t(CnMat* Hx, const FLT t, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = (*_x0).Acc[0] + (*error_model).Acc[0]; const FLT x1 = 1.0/2.0 * t; const FLT x2 = x1 * ((t) > 0 ? 1 : -1) /* Note: Maybe not valid for == 0 */; const FLT x3 = fabs(t); const FLT x4 = 1.0/2.0 * x3; const FLT x5 = (*_x0).Acc[1] + (*error_model).Acc[1]; const FLT x6 = (*_x0).Acc[2] + (*error_model).Acc[2]; const FLT x7 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x8 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x9 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x10 = (x9 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x7 * (*error_model).Pose.AxisAngleRot[0]) + (x8 * (*_x0).Pose.Rot[0]); const FLT x11 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x12 = t * t; const FLT x13 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x14 = x13 * x13; const FLT x15 = x14 * x12; const FLT x16 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x17 = x16 * x16; const FLT x18 = x12 * x17; const FLT x19 = x11 * x11; const FLT x20 = x12 * x19; const FLT x21 = 1e-10 + x20 + x15 + x18; const FLT x22 = sqrt(x21); const FLT x23 = 0.5 * x22; const FLT x24 = sin(x23); const FLT x25 = x24 * (1. / x22); const FLT x26 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x27 = (-1 * x8 * (*_x0).Pose.Rot[1]) + (x9 * (*_x0).Pose.Rot[0]) + (x26 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x28 = (-1 * x26 * (*_x0).Pose.Rot[1]) + (-1 * x7 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[0] + (-1 * x8 * (*_x0).Pose.Rot[3]); const FLT x29 = (x26 * (*_x0).Pose.Rot[0]) + (-1 * x9 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[1] + (x7 * (*error_model).Pose.AxisAngleRot[2]); const FLT x30 = 1. / sqrt((x29 * x29) + (x28 * x28) + (x10 * x10) + (x27 * x27)); const FLT x31 = x24 * x24; const FLT x32 = 1. / x21; const FLT x33 = x32 * x31; const FLT x34 = cos(x23); const FLT x35 = (x33 * x18) + (x34 * x34) + (x33 * x15) + (x33 * x20); const FLT x36 = 1. / sqrt(x35); const FLT x37 = x30 * x36; const FLT x38 = x37 * x25; const FLT x39 = x38 * x11; const FLT x40 = x39 * x10; const FLT x41 = x30 * x34; const FLT x42 = x41 * x36; const FLT x43 = x42 * x29; const FLT x44 = x38 * x13; const FLT x45 = x44 * x27; const FLT x46 = x38 * x16; const FLT x47 = x46 * x28; const FLT x48 = (t * x47) + (-1 * t * x45) + (t * x40) + x43; const FLT x49 = x44 * x10; const FLT x50 = x39 * x27; const FLT x51 = x42 * x28; const FLT x52 = x46 * x29; const FLT x53 = (-1 * t * x52) + x51 + (-1 * t * x49) + (-1 * t * x50); const FLT x54 = x44 * x29; const FLT x55 = x42 * x27; const FLT x56 = x39 * x28; const FLT x57 = x46 * x10; const FLT x58 = (-1 * t * x57) + (t * x56) + (t * x54) + x55; const FLT x59 = x39 * x29; const FLT x60 = x42 * x10; const FLT x61 = x44 * x28; const FLT x62 = x46 * x27; const FLT x63 = (t * x62) + (t * x61) + (-1 * t * x59) + x60; const FLT x64 = (x58 * (*_x0).Pose.Rot[0]) + (x63 * (*_x0).Pose.Rot[1]) + (-1 * x48 * (*_x0).Pose.Rot[3]) + (-1 * x53 * (*_x0).Pose.Rot[2]); const FLT x65 = x64 * x64; const FLT x66 = (-1 * x53 * (*_x0).Pose.Rot[1]) + (x58 * (*_x0).Pose.Rot[3]) + (-1 * x63 * (*_x0).Pose.Rot[2]) + (x48 * (*_x0).Pose.Rot[0]); const FLT x67 = 1 + (-2 * ((x66 * x66) + x65)); const FLT x68 = x67 * x67; const FLT x69 = (x63 * (*_x0).Pose.Rot[0]) + (-1 * x58 * (*_x0).Pose.Rot[1]) + (x48 * (*_x0).Pose.Rot[2]) + (-1 * x53 * (*_x0).Pose.Rot[3]); const FLT x70 = (x53 * (*_x0).Pose.Rot[0]) + (x58 * (*_x0).Pose.Rot[2]) + (x48 * (*_x0).Pose.Rot[1]) + (x63 * (*_x0).Pose.Rot[3]); const FLT x71 = (x70 * x66) + (x64 * x69); const FLT x72 = 2 * t; const FLT x73 = x72 * x19; const FLT x74 = x72 * x14; const FLT x75 = x72 * x17; const FLT x76 = x75 + x73 + x74; const FLT x77 = 0.25 * t * x76 * x32; const FLT x78 = x77 * x16; const FLT x79 = x1 * x13; const FLT x80 = x76 * x25; const FLT x81 = 0.5 * x34; const FLT x82 = x31 * (1. / (x21 * x21)); const FLT x83 = x82 * x76; const FLT x84 = 1. / (x21 * sqrt(x21)); const FLT x85 = x81 * x84 * x24; const FLT x86 = x85 * x76; const FLT x87 = x76 * x15; const FLT x88 = ((x85 * x87) + (x86 * x18) + (-1 * x83 * x20) + (x75 * x33) + (x73 * x33) + (-1 * x80 * x81) + (-1 * x83 * x18) + (-1 * x82 * x87) + (x86 * x20) + (x74 * x33)) * (1. / (x35 * sqrt(x35))); const FLT x89 = x88 * x10; const FLT x90 = x30 * x25; const FLT x91 = x89 * x90; const FLT x92 = 1.0/2.0 * x41; const FLT x93 = x88 * x92; const FLT x94 = x77 * x13; const FLT x95 = x1 * x11; const FLT x96 = x88 * x90; const FLT x97 = x96 * x27; const FLT x98 = x84 * x76 * x24; const FLT x99 = x79 * x98; const FLT x100 = x37 * x10; const FLT x101 = x37 * x29; const FLT x102 = x1 * x16; const FLT x103 = x98 * x102; const FLT x104 = x37 * x27; const FLT x105 = x98 * x95; const FLT x106 = x77 * x11; const FLT x107 = x96 * x102; const FLT x108 = 0.25 * x80; const FLT x109 = x37 * x108; const FLT x110 = (x29 * x107) + (-1 * x50) + (x79 * x91) + (-1 * x28 * x109) + (-1 * x78 * x43) + (x101 * x103) + (-1 * x49) + (-1 * x52) + (x97 * x95) + (-1 * x93 * x28) + (x105 * x104) + (-1 * x60 * x94) + (x99 * x100) + (-1 * x55 * x106); const FLT x111 = x77 * x51; const FLT x112 = x37 * x28; const FLT x113 = (-1 * x103 * x112) + (-1 * x100 * x105) + (-1 * x93 * x29) + (x16 * x111) + x40 + x47 + (x99 * x104) + (-1 * x101 * x108) + (-1 * x91 * x95) + (-1 * x45) + (x60 * x106) + (-1 * x55 * x94) + (-1 * x28 * x107) + (x79 * x97); const FLT x114 = x96 * x29; const FLT x115 = x96 * x28; const FLT x116 = (x91 * x102) + (-1 * x57) + x54 + (-1 * x78 * x60) + (x100 * x103) + (-1 * x27 * x109) + (-1 * x93 * x27) + x56 + (-1 * x79 * x114) + (x11 * x111) + (-1 * x99 * x101) + (x94 * x43) + (-1 * x95 * x115) + (-1 * x105 * x112); const FLT x117 = (x78 * x55) + x61 + (-1 * x10 * x109) + (-1 * x89 * x92) + (-1 * x43 * x106) + x62 + (-1 * x103 * x104) + (x95 * x114) + (-1 * x99 * x112) + (x101 * x105) + (-1 * x27 * x107) + (-1 * x79 * x115) + (-1 * x59) + (x13 * x111); const FLT x118 = (x117 * (*_x0).Pose.Rot[3]) + (x110 * (*_x0).Pose.Rot[0]) + (x116 * (*_x0).Pose.Rot[2]) + (x113 * (*_x0).Pose.Rot[1]); const FLT x119 = (-1 * x117 * (*_x0).Pose.Rot[2]) + (x113 * (*_x0).Pose.Rot[0]) + (-1 * x110 * (*_x0).Pose.Rot[1]) + (x116 * (*_x0).Pose.Rot[3]); const FLT x120 = (-1 * x110 * (*_x0).Pose.Rot[2]) + (-1 * x113 * (*_x0).Pose.Rot[3]) + (x117 * (*_x0).Pose.Rot[1]) + (x116 * (*_x0).Pose.Rot[0]); const FLT x121 = (x113 * (*_x0).Pose.Rot[2]) + (-1 * x110 * (*_x0).Pose.Rot[3]) + (-1 * x116 * (*_x0).Pose.Rot[1]) + (x117 * (*_x0).Pose.Rot[0]); const FLT x122 = -4 * x64 * x120; const FLT x123 = 1 + (-2 * (x65 + (x69 * x69))); const FLT x124 = x123 * x123; const FLT x125 = (x70 * x69) + (x64 * x66); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), 0, (x0 * x4) + (*error_model).Velocity.Pos[0] + (*_x0).Velocity.Pos[0] + (x0 * x2)); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), 0, (x4 * x5) + (x2 * x5) + (*error_model).Velocity.Pos[1] + (*_x0).Velocity.Pos[1]); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), 0, (x2 * x6) + (*error_model).Velocity.Pos[2] + (x4 * x6) + (*_x0).Velocity.Pos[2]); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), 0, x68 * (1. / (x68 + (4 * (x71 * x71)))) * ((-2 * x71 * (1. / x68) * ((-4 * x66 * x119) + x122)) + (2 * ((x69 * x120) + (x66 * x118) + (x64 * x121) + (x70 * x119)) * (1. / x67)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), 0, 2 * ((-1 * x69 * x119) + (x70 * x120) + (x64 * x118) + (-1 * x66 * x121)) * (1. / sqrt(1 + (-4 * (((x70 * x64) + (-1 * x66 * x69)) * ((x70 * x64) + (-1 * x66 * x69))))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), 0, x124 * (1. / (x124 + (4 * (x125 * x125)))) * ((-2 * (1. / x124) * x125 * (x122 + (-4 * x69 * x121))) + (2 * ((x64 * x119) + (x66 * x120) + (x70 * x121) + (x69 * x118)) * (1. / x123)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), 0, x0); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), 0, x5); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), 0, x6); } // Full version Jacobian of SurviveKalmanModelErrorPredict wrt [t] // Jacobian of SurviveKalmanModelErrorPredict wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModelErrorPredict_jac_x0(CnMat* Hx, const FLT t, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = 1.0/2.0 * t; const FLT x1 = fabs(t) * x0; const FLT x2 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x3 = 0.5 * (*_x0).Pose.Rot[3]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x6 = (*_x0).Pose.Rot[1] + (x5 * (*_x0).Pose.Rot[0]) + (-1 * x3 * (*error_model).Pose.AxisAngleRot[1]) + (x4 * (*_x0).Pose.Rot[2]); const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x8 = (x7 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x5 * (*_x0).Pose.Rot[2]) + (x4 * (*_x0).Pose.Rot[0]); const FLT x9 = (-1 * x4 * (*_x0).Pose.Rot[1]) + (x7 * (*_x0).Pose.Rot[0]) + (x3 * (*error_model).Pose.AxisAngleRot[0]) + (*_x0).Pose.Rot[2]; const FLT x10 = (-1 * x5 * (*_x0).Pose.Rot[1]) + (-1 * x7 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x3 * (*error_model).Pose.AxisAngleRot[2]); const FLT x11 = (x10 * x10) + (x6 * x6) + (x8 * x8) + (x9 * x9); const FLT x12 = 1. / sqrt(x11); const FLT x13 = t * t; const FLT x14 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x15 = x14 * x14; const FLT x16 = x15 * x13; const FLT x17 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x18 = x17 * x17; const FLT x19 = x13 * x18; const FLT x20 = x2 * x2; const FLT x21 = x20 * x13; const FLT x22 = 1e-10 + x21 + x16 + x19; const FLT x23 = sqrt(x22); const FLT x24 = 0.5 * x23; const FLT x25 = sin(x24); const FLT x26 = x25 * x25; const FLT x27 = 1. / x22; const FLT x28 = x26 * x27; const FLT x29 = cos(x24); const FLT x30 = (x28 * x19) + (x29 * x29) + (x28 * x16) + (x21 * x28); const FLT x31 = 1. / sqrt(x30); const FLT x32 = x31 * x12; const FLT x33 = x25 * (1. / x23); const FLT x34 = t * x33; const FLT x35 = x32 * x34; const FLT x36 = x6 * x35; const FLT x37 = x2 * x36; const FLT x38 = x32 * x29; const FLT x39 = x8 * x38; const FLT x40 = x35 * x14; const FLT x41 = x40 * x10; const FLT x42 = x9 * x32; const FLT x43 = x42 * x34; const FLT x44 = x43 * x17; const FLT x45 = x44 + x41 + (-1 * x37) + x39; const FLT x46 = x2 * x35; const FLT x47 = x8 * x46; const FLT x48 = x6 * x38; const FLT x49 = x43 * x14; const FLT x50 = x35 * x10; const FLT x51 = x50 * x17; const FLT x52 = (-1 * x49) + x51 + x47 + x48; const FLT x53 = x6 * x40; const FLT x54 = x9 * x38; const FLT x55 = x46 * x10; const FLT x56 = x8 * x35; const FLT x57 = x56 * x17; const FLT x58 = x55 + (-1 * x57) + x53 + x54; const FLT x59 = x8 * x40; const FLT x60 = x2 * x43; const FLT x61 = x38 * x10; const FLT x62 = x36 * x17; const FLT x63 = (-1 * x62) + (-1 * x59) + x61 + (-1 * x60); const FLT x64 = (x58 * (*_x0).Pose.Rot[3]) + (-1 * x45 * (*_x0).Pose.Rot[2]) + (-1 * x63 * (*_x0).Pose.Rot[1]) + (x52 * (*_x0).Pose.Rot[0]); const FLT x65 = x35 * x17; const FLT x66 = x7 * x65; const FLT x67 = x0 * x33; const FLT x68 = x67 * x17; const FLT x69 = 1.0 * (*error_model).Pose.AxisAngleRot[2]; const FLT x70 = 1.0 * x6; const FLT x71 = 1.0 * x9; const FLT x72 = (x70 * (*error_model).Pose.AxisAngleRot[0]) + (x71 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x10) + (x8 * x69); const FLT x73 = x31 * (1. / (x11 * sqrt(x11))); const FLT x74 = x73 * x72; const FLT x75 = x9 * x74; const FLT x76 = x2 * x67; const FLT x77 = x6 * x76; const FLT x78 = 1.0/2.0 * x29; const FLT x79 = x8 * x78; const FLT x80 = x67 * x14; const FLT x81 = x80 * x10; const FLT x82 = -1 * x5 * x46; const FLT x83 = x4 * x38; const FLT x84 = x83 + x82; const FLT x85 = x84 + (-1 * x81 * x74) + x66 + x40 + (-1 * x75 * x68) + (x74 * x77) + (-1 * x79 * x74); const FLT x86 = x7 * x38; const FLT x87 = x6 * x74; const FLT x88 = x8 * x74; const FLT x89 = x9 * x78; const FLT x90 = x76 * x10; const FLT x91 = -1 * x4 * x65; const FLT x92 = x5 * x40; const FLT x93 = x92 + x91; const FLT x94 = (-1 * x74 * x90) + (-1 * x89 * x74) + x46 + x93 + x86 + (-1 * x80 * x87) + (x88 * x68); const FLT x95 = x68 * x10; const FLT x96 = x4 * x46; const FLT x97 = x6 * x78; const FLT x98 = -1 * x7 * x40; const FLT x99 = x5 * x38; const FLT x100 = x99 + x98; const FLT x101 = x100 + (-1 * x74 * x97) + x65 + (-1 * x88 * x76) + (-1 * x74 * x95) + (x80 * x75) + x96; const FLT x102 = x7 * x46; const FLT x103 = -1 * x102; const FLT x104 = x78 * x10; const FLT x105 = x73 * x104; const FLT x106 = x4 * x40; const FLT x107 = -1 * x106; const FLT x108 = x5 * x65; const FLT x109 = x38 + (-1 * x108); const FLT x110 = x109 + x103 + (x75 * x76) + x107 + (x80 * x88) + (x87 * x68) + (-1 * x72 * x105); const FLT x111 = (x110 * (*_x0).Pose.Rot[0]) + (x101 * (*_x0).Pose.Rot[1]) + (x85 * (*_x0).Pose.Rot[3]) + x63 + (x94 * (*_x0).Pose.Rot[2]); const FLT x112 = (-1 * x101 * (*_x0).Pose.Rot[3]) + (x85 * (*_x0).Pose.Rot[1]) + x58 + (-1 * x110 * (*_x0).Pose.Rot[2]) + (x94 * (*_x0).Pose.Rot[0]); const FLT x113 = (-1 * x58 * (*_x0).Pose.Rot[1]) + (x45 * (*_x0).Pose.Rot[0]) + (x52 * (*_x0).Pose.Rot[2]) + (-1 * x63 * (*_x0).Pose.Rot[3]); const FLT x114 = x52 + (x101 * (*_x0).Pose.Rot[0]) + (-1 * x85 * (*_x0).Pose.Rot[2]) + (-1 * x110 * (*_x0).Pose.Rot[1]) + (x94 * (*_x0).Pose.Rot[3]); const FLT x115 = (x52 * (*_x0).Pose.Rot[1]) + (x63 * (*_x0).Pose.Rot[0]) + (x58 * (*_x0).Pose.Rot[2]) + (x45 * (*_x0).Pose.Rot[3]); const FLT x116 = (x45 * (*_x0).Pose.Rot[1]) + (x58 * (*_x0).Pose.Rot[0]) + (-1 * x52 * (*_x0).Pose.Rot[3]) + (-1 * x63 * (*_x0).Pose.Rot[2]); const FLT x117 = x45 + (x85 * (*_x0).Pose.Rot[0]) + (-1 * x110 * (*_x0).Pose.Rot[3]) + (-1 * x94 * (*_x0).Pose.Rot[1]) + (x101 * (*_x0).Pose.Rot[2]); const FLT x118 = x116 * x116; const FLT x119 = 1 + (-2 * ((x64 * x64) + x118)); const FLT x120 = 2 * (1. / x119); const FLT x121 = 4 * x116; const FLT x122 = -1 * x112 * x121; const FLT x123 = 4 * x64; const FLT x124 = x119 * x119; const FLT x125 = (x64 * x115) + (x113 * x116); const FLT x126 = 2 * (1. / x124) * x125; const FLT x127 = x124 * (1. / (x124 + (4 * (x125 * x125)))); const FLT x128 = -1 * x65; const FLT x129 = 1.0 * x10; const FLT x130 = 1.0 * x8; const FLT x131 = x73 * ((-1 * x9 * x69) + (2 * x6) + (x130 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x129 * (*error_model).Pose.AxisAngleRot[0])); const FLT x132 = x6 * x131; const FLT x133 = x8 * x131; const FLT x134 = x9 * x131; const FLT x135 = (-1 * x99) + x98; const FLT x136 = x135 + (x76 * x134) + x96 + (-1 * x104 * x131) + x128 + (x68 * x132) + (x80 * x133); const FLT x137 = x10 * x131; const FLT x138 = x109 + x106 + (-1 * x97 * x131) + (-1 * x68 * x137) + (x80 * x134) + x102 + (-1 * x76 * x133); const FLT x139 = -1 * x46; const FLT x140 = x91 + (-1 * x92); const FLT x141 = x140 + x86 + (-1 * x80 * x137) + (-1 * x68 * x134) + (-1 * x79 * x131) + (x77 * x131) + x139; const FLT x142 = -1 * x66; const FLT x143 = (-1 * x83) + x82; const FLT x144 = x142 + x143 + (-1 * x76 * x137) + (-1 * x89 * x131) + (x68 * x133) + x40 + (-1 * x80 * x132); const FLT x145 = x62 + x59 + (-1 * x61) + x60; const FLT x146 = x145 + (-1 * x141 * (*_x0).Pose.Rot[2]) + (x144 * (*_x0).Pose.Rot[3]) + (-1 * x136 * (*_x0).Pose.Rot[1]) + (x138 * (*_x0).Pose.Rot[0]); const FLT x147 = (x144 * (*_x0).Pose.Rot[2]) + (x141 * (*_x0).Pose.Rot[3]) + (x138 * (*_x0).Pose.Rot[1]) + x52 + (x136 * (*_x0).Pose.Rot[0]); const FLT x148 = x45 + (-1 * x136 * (*_x0).Pose.Rot[2]) + (x144 * (*_x0).Pose.Rot[0]) + (-1 * x138 * (*_x0).Pose.Rot[3]) + (x141 * (*_x0).Pose.Rot[1]); const FLT x149 = (-1 * x55) + (-1 * x53) + (-1 * x144 * (*_x0).Pose.Rot[1]) + (-1 * x54) + (-1 * x136 * (*_x0).Pose.Rot[3]) + (x141 * (*_x0).Pose.Rot[0]) + x57 + (x138 * (*_x0).Pose.Rot[2]); const FLT x150 = -1 * x121 * x148; const FLT x151 = -1 * x40; const FLT x152 = (2 * x9) + (x6 * x69) + (-1 * x130 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x129 * (*error_model).Pose.AxisAngleRot[1]); const FLT x153 = x73 * x152; const FLT x154 = x9 * x153; const FLT x155 = x8 * x153; const FLT x156 = x84 + x151 + (-1 * x95 * x153) + (x80 * x154) + (-1 * x97 * x153) + x142 + (-1 * x76 * x155); const FLT x157 = -1 * x96; const FLT x158 = x65 + x157 + (-1 * x68 * x154) + (x77 * x153) + x135 + (-1 * x79 * x153) + (-1 * x81 * x153); const FLT x159 = x6 * x153; const FLT x160 = x108 + x38; const FLT x161 = (-1 * x89 * x153) + x106 + x103 + x160 + (x68 * x155) + (-1 * x90 * x153) + (-1 * x80 * x159); const FLT x162 = -1 * x86; const FLT x163 = x93 + (x76 * x154) + x162 + (-1 * x105 * x152) + (x68 * x159) + x139 + (x80 * x155); const FLT x164 = x58 + (x163 * (*_x0).Pose.Rot[0]) + (x161 * (*_x0).Pose.Rot[2]) + (x156 * (*_x0).Pose.Rot[1]) + (x158 * (*_x0).Pose.Rot[3]); const FLT x165 = (-1 * x163 * (*_x0).Pose.Rot[2]) + x145 + (-1 * x156 * (*_x0).Pose.Rot[3]) + (x158 * (*_x0).Pose.Rot[1]) + (x161 * (*_x0).Pose.Rot[0]); const FLT x166 = (-1 * x44) + (-1 * x39) + (x161 * (*_x0).Pose.Rot[3]) + (x156 * (*_x0).Pose.Rot[0]) + x37 + (-1 * x163 * (*_x0).Pose.Rot[1]) + (-1 * x158 * (*_x0).Pose.Rot[2]) + (-1 * x41); const FLT x167 = x52 + (x158 * (*_x0).Pose.Rot[0]) + (x156 * (*_x0).Pose.Rot[2]) + (-1 * x161 * (*_x0).Pose.Rot[1]) + (-1 * x163 * (*_x0).Pose.Rot[3]); const FLT x168 = -1 * x121 * x165; const FLT x169 = x73 * ((x71 * (*error_model).Pose.AxisAngleRot[0]) + (2 * x8) + (-1 * x70 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x69 * x10)); const FLT x170 = x6 * x169; const FLT x171 = x8 * x169; const FLT x172 = x9 * x169; const FLT x173 = x143 + x66 + (x68 * x170) + (-1 * x104 * x169) + (x76 * x172) + (x80 * x171) + x151; const FLT x174 = x140 + (-1 * x97 * x169) + (-1 * x76 * x171) + x46 + (-1 * x95 * x169) + (x80 * x172) + x162; const FLT x175 = (-1 * x79 * x169) + x160 + x102 + (-1 * x68 * x172) + x107 + (-1 * x81 * x169) + (x77 * x169); const FLT x176 = x100 + (x68 * x171) + x128 + (-1 * x89 * x169) + x157 + (-1 * x80 * x170) + (-1 * x90 * x169); const FLT x177 = x58 + (x176 * (*_x0).Pose.Rot[3]) + (-1 * x175 * (*_x0).Pose.Rot[2]) + (-1 * x173 * (*_x0).Pose.Rot[1]) + (x174 * (*_x0).Pose.Rot[0]); const FLT x178 = (x175 * (*_x0).Pose.Rot[3]) + x45 + (x176 * (*_x0).Pose.Rot[2]) + (x174 * (*_x0).Pose.Rot[1]) + (x173 * (*_x0).Pose.Rot[0]); const FLT x179 = (-1 * x48) + (-1 * x47) + (x175 * (*_x0).Pose.Rot[1]) + (-1 * x173 * (*_x0).Pose.Rot[2]) + (-1 * x51) + (x176 * (*_x0).Pose.Rot[0]) + x49 + (-1 * x174 * (*_x0).Pose.Rot[3]); const FLT x180 = x145 + (-1 * x176 * (*_x0).Pose.Rot[1]) + (-1 * x173 * (*_x0).Pose.Rot[3]) + (x174 * (*_x0).Pose.Rot[2]) + (x175 * (*_x0).Pose.Rot[0]); const FLT x181 = -1 * x121 * x179; const FLT x182 = -1 * x56; const FLT x183 = x6 * x32; const FLT x184 = x14 * x183; const FLT x185 = t * t * t; const FLT x186 = x25 * (1. / (x22 * sqrt(x22))); const FLT x187 = x186 * x185; const FLT x188 = x17 * x187; const FLT x189 = x188 * x184; const FLT x190 = t * t * t * t; const FLT x191 = (x17 * x17 * x17) * x190; const FLT x192 = 1.0 * x29; const FLT x193 = x186 * x192; const FLT x194 = 2 * (1. / (x22 * x22)) * x26; const FLT x195 = x190 * x193; const FLT x196 = x15 * x195; const FLT x197 = x13 * x17; const FLT x198 = 2 * x28; const FLT x199 = x20 * x195; const FLT x200 = x190 * x194; const FLT x201 = x15 * x200; const FLT x202 = x33 * x192; const FLT x203 = x20 * x200; const FLT x204 = (1. / (x30 * sqrt(x30))) * x12; const FLT x205 = ((-1 * x17 * x203) + (-1 * x202 * x197) + (-1 * x17 * x201) + (-1 * x191 * x194) + (x191 * x193) + (x17 * x196) + (x197 * x198) + (x17 * x199)) * x204; const FLT x206 = 0.5 * x33; const FLT x207 = x206 * x197; const FLT x208 = x80 * x205; const FLT x209 = 0.5 * x27 * x185; const FLT x210 = x48 * x209; const FLT x211 = x17 * x210; const FLT x212 = x14 * x211; const FLT x213 = x8 * x32; const FLT x214 = x18 * x187; const FLT x215 = x76 * x205; const FLT x216 = x68 * x205; const FLT x217 = x39 * x209; const FLT x218 = x61 * x209; const FLT x219 = x17 * x218; const FLT x220 = x32 * x10; const FLT x221 = x2 * x187; const FLT x222 = x17 * x221; const FLT x223 = (-1 * x220 * x222) + (x2 * x219); const FLT x224 = (-1 * x18 * x217) + (-1 * x10 * x215) + (x213 * x214) + (-1 * x89 * x205) + x212 + x182 + (x8 * x216) + (-1 * x189) + (-1 * x42 * x207) + x223 + (-1 * x6 * x208); const FLT x225 = -1 * x36; const FLT x226 = x42 * x222; const FLT x227 = x54 * x209; const FLT x228 = x2 * x17; const FLT x229 = x227 * x228; const FLT x230 = x14 * x17; const FLT x231 = x14 * x213; const FLT x232 = (x231 * x188) + (-1 * x217 * x230); const FLT x233 = (-1 * x207 * x220) + (x9 * x215) + (-1 * x205 * x104) + x226 + x225 + (x6 * x216) + (-1 * x229) + x232 + (-1 * x18 * x210) + (x214 * x183) + (x8 * x208); const FLT x234 = x14 * x220; const FLT x235 = (x14 * x219) + (-1 * x234 * x188); const FLT x236 = (x222 * x183) + (-1 * x2 * x211); const FLT x237 = x236 + x235 + (-1 * x9 * x216) + (x18 * x227) + (-1 * x42 * x214) + (x77 * x205) + x43 + (-1 * x10 * x208) + (-1 * x79 * x205) + (-1 * x213 * x207); const FLT x238 = x213 * x222; const FLT x239 = x230 * x227; const FLT x240 = x217 * x228; const FLT x241 = x42 * x14; const FLT x242 = x241 * x188; const FLT x243 = (-1 * x207 * x183) + (-1 * x238) + (-1 * x10 * x216) + (-1 * x97 * x205) + x242 + (x9 * x208) + (x18 * x218) + (-1 * x8 * x215) + x50 + (-1 * x239) + (-1 * x214 * x220) + x240; const FLT x244 = (x243 * (*_x0).Pose.Rot[0]) + (-1 * x237 * (*_x0).Pose.Rot[2]) + (x224 * (*_x0).Pose.Rot[3]) + (-1 * x233 * (*_x0).Pose.Rot[1]); const FLT x245 = (-1 * x233 * (*_x0).Pose.Rot[2]) + (-1 * x243 * (*_x0).Pose.Rot[3]) + (x224 * (*_x0).Pose.Rot[0]) + (x237 * (*_x0).Pose.Rot[1]); const FLT x246 = (x224 * (*_x0).Pose.Rot[2]) + (x237 * (*_x0).Pose.Rot[3]) + (x233 * (*_x0).Pose.Rot[0]) + (x243 * (*_x0).Pose.Rot[1]); const FLT x247 = (x243 * (*_x0).Pose.Rot[2]) + (-1 * x224 * (*_x0).Pose.Rot[1]) + (-1 * x233 * (*_x0).Pose.Rot[3]) + (x237 * (*_x0).Pose.Rot[0]); const FLT x248 = -1 * x245 * x121; const FLT x249 = x18 * x195; const FLT x250 = x13 * x202; const FLT x251 = x13 * x198; const FLT x252 = x18 * x200; const FLT x253 = x2 * x2 * x2; const FLT x254 = (x253 * x195) + (-1 * x200 * x253) + (-1 * x2 * x252) + (-1 * x2 * x250) + (-1 * x2 * x201) + (x2 * x196) + (x2 * x249) + (x2 * x251); const FLT x255 = x204 * x254; const FLT x256 = x20 * x187; const FLT x257 = x68 * x255; const FLT x258 = x13 * x206; const FLT x259 = x2 * x258; const FLT x260 = x2 * x14; const FLT x261 = (x218 * x260) + (-1 * x234 * x221); const FLT x262 = (-1 * x81 * x255) + x225 + (-1 * x20 * x210) + (x256 * x183) + (-1 * x213 * x259) + (x77 * x255) + x261 + (-1 * x226) + (-1 * x9 * x257) + (-1 * x79 * x255) + x229; const FLT x263 = -1 * x43; const FLT x264 = x6 * x255; const FLT x265 = x76 * x255; const FLT x266 = x80 * x255; const FLT x267 = x231 * x221; const FLT x268 = x217 * x260; const FLT x269 = x204 * x104; const FLT x270 = (-1 * x20 * x227) + (-1 * x259 * x220) + (-1 * x268) + x267 + (x68 * x264) + (x9 * x265) + (-1 * x269 * x254) + (x42 * x256) + x236 + x263 + (x8 * x266); const FLT x271 = x221 * x184; const FLT x272 = x210 * x260; const FLT x273 = (-1 * x240) + (-1 * x10 * x265) + (x8 * x257) + (-1 * x80 * x264) + (-1 * x89 * x255) + (-1 * x271) + x272 + (-1 * x42 * x259) + (x20 * x218) + x238 + (-1 * x256 * x220) + x50; const FLT x274 = (x221 * x241) + (-1 * x260 * x227); const FLT x275 = (-1 * x213 * x256) + (-1 * x97 * x255) + (-1 * x8 * x265) + (x9 * x266) + x274 + (-1 * x10 * x257) + (-1 * x259 * x183) + x56 + x223 + (x20 * x217); const FLT x276 = (x275 * (*_x0).Pose.Rot[0]) + (x273 * (*_x0).Pose.Rot[3]) + (-1 * x262 * (*_x0).Pose.Rot[2]) + (-1 * x270 * (*_x0).Pose.Rot[1]); const FLT x277 = (-1 * x270 * (*_x0).Pose.Rot[2]) + (x262 * (*_x0).Pose.Rot[1]) + (x273 * (*_x0).Pose.Rot[0]) + (-1 * x275 * (*_x0).Pose.Rot[3]); const FLT x278 = (x273 * (*_x0).Pose.Rot[2]) + (x275 * (*_x0).Pose.Rot[1]) + (x262 * (*_x0).Pose.Rot[3]) + (x270 * (*_x0).Pose.Rot[0]); const FLT x279 = (x275 * (*_x0).Pose.Rot[2]) + (-1 * x273 * (*_x0).Pose.Rot[1]) + (-1 * x270 * (*_x0).Pose.Rot[3]) + (x262 * (*_x0).Pose.Rot[0]); const FLT x280 = -1 * x277 * x121; const FLT x281 = x14 * x14 * x14; const FLT x282 = (-1 * x14 * x203) + (x14 * x251) + (-1 * x14 * x250) + (x14 * x199) + (x14 * x249) + (x281 * x195) + (-1 * x14 * x252) + (-1 * x200 * x281); const FLT x283 = x204 * x282; const FLT x284 = x8 * x283; const FLT x285 = x15 * x187; const FLT x286 = x6 * x283; const FLT x287 = x10 * x283; const FLT x288 = x261 + (-1 * x76 * x287) + x36 + (-1 * x258 * x241) + x232 + (-1 * x285 * x183) + (x15 * x210) + (x68 * x284) + (-1 * x80 * x286) + (-1 * x89 * x283); const FLT x289 = x9 * x283; const FLT x290 = (-1 * x269 * x282) + (x213 * x285) + (-1 * x234 * x258) + x189 + (x76 * x289) + (x68 * x286) + x274 + (x80 * x284) + (-1 * x15 * x217) + (-1 * x212) + x182; const FLT x291 = x271 + (-1 * x272) + (-1 * x79 * x283) + (-1 * x285 * x220) + (-1 * x242) + (-1 * x80 * x287) + (-1 * x68 * x289) + (x77 * x283) + x239 + (-1 * x231 * x258) + x50 + (x15 * x218); const FLT x292 = x235 + x263 + (x42 * x285) + (x80 * x289) + (-1 * x267) + (-1 * x68 * x287) + (-1 * x97 * x283) + (-1 * x15 * x227) + x268 + (-1 * x76 * x284) + (-1 * x258 * x184); const FLT x293 = (x292 * (*_x0).Pose.Rot[0]) + (-1 * x291 * (*_x0).Pose.Rot[2]) + (x288 * (*_x0).Pose.Rot[3]) + (-1 * x290 * (*_x0).Pose.Rot[1]); const FLT x294 = (x288 * (*_x0).Pose.Rot[2]) + (x291 * (*_x0).Pose.Rot[3]) + (x290 * (*_x0).Pose.Rot[0]) + (x292 * (*_x0).Pose.Rot[1]); const FLT x295 = (-1 * x290 * (*_x0).Pose.Rot[2]) + (-1 * x292 * (*_x0).Pose.Rot[3]) + (x291 * (*_x0).Pose.Rot[1]) + (x288 * (*_x0).Pose.Rot[0]); const FLT x296 = (-1 * x290 * (*_x0).Pose.Rot[3]) + (-1 * x288 * (*_x0).Pose.Rot[1]) + (x291 * (*_x0).Pose.Rot[0]) + (x292 * (*_x0).Pose.Rot[2]); const FLT x297 = -1 * x295 * x121; const FLT x298 = 2 * (1. / sqrt(1 + (-4 * (((x115 * x116) + (-1 * x64 * x113)) * ((x115 * x116) + (-1 * x64 * x113)))))); const FLT x299 = 1 + (-2 * (x118 + (x113 * x113))); const FLT x300 = 2 * (1. / x299); const FLT x301 = 4 * x113; const FLT x302 = x299 * x299; const FLT x303 = (x113 * x115) + (x64 * x116); const FLT x304 = 2 * (1. / x302) * x303; const FLT x305 = x302 * (1. / (x302 + (4 * (x303 * x303)))); const FLT x306 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x307 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x308 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x309 = (-1 * x308 * (*error_model).IMUBias.IMUCorrection[2]) + (x306 * (*error_model).IMUBias.IMUCorrection[0]) + (x307 * (*error_model).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x310 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x311 = (*_x0).IMUBias.IMUCorrection[1] + (x310 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x306 * (*error_model).IMUBias.IMUCorrection[1]) + (x307 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x312 = 1.0 * x311; const FLT x313 = (*_x0).IMUBias.IMUCorrection[0] + (-1 * x306 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x308 * (*error_model).IMUBias.IMUCorrection[0]) + (-1 * x310 * (*error_model).IMUBias.IMUCorrection[1]); const FLT x314 = 1.0 * x309; const FLT x315 = (*_x0).IMUBias.IMUCorrection[3] + (x308 * (*error_model).IMUBias.IMUCorrection[1]) + (x307 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x310 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x316 = 1.0 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x317 = (x311 * x311) + (x313 * x313) + (x309 * x309) + (x315 * x315); const FLT x318 = 1.0/2.0 * (1. / (x317 * sqrt(x317))); const FLT x319 = x318 * ((x315 * x316) + (x314 * (*error_model).IMUBias.IMUCorrection[1]) + (x312 * (*error_model).IMUBias.IMUCorrection[0]) + (2 * x313)); const FLT x320 = x309 * x319; const FLT x321 = x313 * x319; const FLT x322 = x319 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x323 = x311 * x319; const FLT x324 = 1. / sqrt(x317); const FLT x325 = x324 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x326 = 0.5 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x327 = x326 * x325; const FLT x328 = x324 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x329 = x309 * x324; const FLT x330 = x329 + (-1 * x328); const FLT x331 = x324 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x332 = 0.5 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x333 = x332 * x331; const FLT x334 = x324 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x335 = 0.5 * x334; const FLT x336 = x335 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x337 = (-1 * x336) + x333; const FLT x338 = x337 + x330 + (x321 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x315 * x322) + (-1 * x320 * (*_x0).IMUBias.IMUCorrection[0]) + (x323 * (*_x0).IMUBias.IMUCorrection[3]) + x327; const FLT x339 = x313 * x324; const FLT x340 = x311 * x324; const FLT x341 = x315 * x324; const FLT x342 = (x341 * (*_x0).IMUBias.IMUCorrection[1]) + (x329 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x339 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x340 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x343 = 4 * x342; const FLT x344 = -1 * x338 * x343; const FLT x345 = -1 * x331; const FLT x346 = x315 * x319; const FLT x347 = x335 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x348 = 0.5 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x349 = x325 * x348; const FLT x350 = x349 + x347; const FLT x351 = x328 * x332; const FLT x352 = x340 + (-1 * x351); const FLT x353 = x352 + x350 + (x313 * x322) + (x346 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x320 * (*_x0).IMUBias.IMUCorrection[3]) + x345 + (-1 * x323 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x354 = (-1 * x341 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x339 * (*_x0).IMUBias.IMUCorrection[1]) + (x329 * (*_x0).IMUBias.IMUCorrection[3]) + (x340 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x355 = 4 * x354; const FLT x356 = (-1 * x329 * (*_x0).IMUBias.IMUCorrection[1]) + (x341 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x339 * (*_x0).IMUBias.IMUCorrection[3]) + (x340 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x357 = (x339 * (*_x0).IMUBias.IMUCorrection[0]) + (x341 * (*_x0).IMUBias.IMUCorrection[3]) + (x340 * (*_x0).IMUBias.IMUCorrection[1]) + (x329 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x358 = (x354 * x357) + (x356 * x342); const FLT x359 = x342 * x342; const FLT x360 = 1 + (-2 * ((x354 * x354) + x359)); const FLT x361 = x360 * x360; const FLT x362 = 2 * (1. / x361) * x358; const FLT x363 = x348 * x331; const FLT x364 = x334 * x332; const FLT x365 = x326 * x328; const FLT x366 = x365 + x325; const FLT x367 = x366 + (-1 * x320 * (*_x0).IMUBias.IMUCorrection[2]) + x339 + (-1 * x321 * (*_x0).IMUBias.IMUCorrection[0]) + x364 + (-1 * x346 * (*_x0).IMUBias.IMUCorrection[3]) + x363 + (-1 * x323 * (*_x0).IMUBias.IMUCorrection[1]); const FLT x368 = x325 * x332; const FLT x369 = (-1 * x334) + x341; const FLT x370 = x328 * x348; const FLT x371 = x326 * x331; const FLT x372 = (-1 * x371) + x370; const FLT x373 = x369 + (-1 * x323 * (*_x0).IMUBias.IMUCorrection[2]) + (x309 * x322) + (-1 * x346 * (*_x0).IMUBias.IMUCorrection[0]) + x372 + (x321 * (*_x0).IMUBias.IMUCorrection[3]) + x368; const FLT x374 = 2 * (1. / x360); const FLT x375 = x361 * (1. / (x361 + (4 * (x358 * x358)))); const FLT x376 = x315 * x318; const FLT x377 = 1.0 * x313; const FLT x378 = 1.0 * x315; const FLT x379 = (2 * x311) + (-1 * x377 * (*error_model).IMUBias.IMUCorrection[0]) + (x378 * (*error_model).IMUBias.IMUCorrection[1]) + (-1 * x309 * x316); const FLT x380 = x379 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x381 = x379 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x382 = x313 * x318; const FLT x383 = x309 * x318; const FLT x384 = x379 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x385 = -1 * x368; const FLT x386 = x379 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x387 = x386 * x318; const FLT x388 = x370 + x371; const FLT x389 = x369 + x388 + (x381 * x382) + (-1 * x376 * x380) + (-1 * x383 * x384) + (x387 * x311) + x385; const FLT x390 = -1 * x389 * x343; const FLT x391 = -1 * x339; const FLT x392 = -1 * x364; const FLT x393 = x311 * x318; const FLT x394 = (-1 * x365) + x325; const FLT x395 = (-1 * x393 * x384) + (x376 * x381) + x392 + x394 + x363 + (x382 * x380) + x391 + (-1 * x383 * x386); const FLT x396 = (-1 * x349) + x347; const FLT x397 = x396 + x352 + (-1 * x393 * x380) + (-1 * x384 * x382) + x331 + (-1 * x383 * x381) + (-1 * x387 * x315); const FLT x398 = x336 + x333; const FLT x399 = (-1 * x393 * x381) + x328 + x327 + (-1 * x376 * x384) + (x383 * x380) + x398 + (-1 * x329) + (x387 * x313); const FLT x400 = (2 * x309) + (-1 * x378 * (*error_model).IMUBias.IMUCorrection[0]) + (-1 * x377 * (*error_model).IMUBias.IMUCorrection[1]) + (x311 * x316); const FLT x401 = x400 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x402 = x400 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x403 = x400 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x404 = x403 * x318; const FLT x405 = x400 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x406 = x405 * x318; const FLT x407 = (-1 * x363) + x391; const FLT x408 = x407 + x366 + (-1 * x402 * x376) + (-1 * x401 * x383) + (x404 * x313) + x392 + (x406 * x311); const FLT x409 = -1 * x408 * x343; const FLT x410 = x402 * x318; const FLT x411 = x401 * x318; const FLT x412 = x388 + x334 + (x403 * x376) + x368 + (-1 * x411 * x311) + (x410 * x313) + (-1 * x405 * x383) + (-1 * x341); const FLT x413 = -1 * x327; const FLT x414 = x328 + x413 + (-1 * x403 * x383) + x329 + (-1 * x411 * x313) + x337 + (-1 * x410 * x311) + (-1 * x405 * x376); const FLT x415 = x396 + x351 + (-1 * x404 * x311) + x345 + (x402 * x383) + (x406 * x313) + x340 + (-1 * x401 * x376); const FLT x416 = (2 * x315) + (x314 * (*error_model).IMUBias.IMUCorrection[0]) + (-1 * x312 * (*error_model).IMUBias.IMUCorrection[1]) + (-1 * x377 * (*error_model).IMUBias.IMUCorrection[2]); const FLT x417 = x416 * x376; const FLT x418 = x416 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x419 = x416 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x420 = x416 * x393; const FLT x421 = (-1 * x340) + x350 + (x420 * (*_x0).IMUBias.IMUCorrection[3]) + x331 + (-1 * x418 * x383) + (x419 * x382) + (-1 * x417 * (*_x0).IMUBias.IMUCorrection[1]) + x351; const FLT x422 = -1 * x421 * x343; const FLT x423 = x416 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x424 = x416 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x425 = x330 + (x419 * x376) + (x423 * x382) + (-1 * x424 * x383) + x413 + x398 + (-1 * x420 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x426 = x394 + x364 + x407 + (x423 * x383) + (x424 * x382) + (-1 * x417 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x420 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x427 = (-1 * x420 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x419 * x383) + x385 + x341 + x334 + x372 + (-1 * x418 * x382) + (-1 * x417 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x428 = 2 * (1. / sqrt(1 + (-4 * (((x357 * x342) + (-1 * x354 * x356)) * ((x357 * x342) + (-1 * x354 * x356)))))); const FLT x429 = 4 * x356; const FLT x430 = (x356 * x357) + (x354 * x342); const FLT x431 = 1 + (-2 * (x359 + (x356 * x356))); const FLT x432 = x431 * x431; const FLT x433 = 2 * (1. / x432) * x430; const FLT x434 = 2 * (1. / x431); const FLT x435 = x432 * (1. / (x432 + (4 * (x430 * x430)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x114 * x123) + x122)) + (x120 * ((x116 * x117) + (x114 * x115) + (x64 * x111) + (x112 * x113))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x123 * x146) + x150)) + (x120 * ((x116 * x149) + (x113 * x148) + (x115 * x146) + (x64 * x147))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x123 * x166) + x168)) + (x120 * ((x116 * x167) + (x115 * x166) + (x64 * x164) + (x113 * x165))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x123 * x177) + x181)) + (x120 * ((x116 * x180) + (x113 * x179) + (x115 * x177) + (x64 * x178))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x244 * x123) + x248)) + (x120 * ((x247 * x116) + (x64 * x246) + (x244 * x115) + (x245 * x113))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x276 * x123) + x280)) + (x120 * ((x64 * x278) + (x276 * x115) + (x279 * x116) + (x277 * x113))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x127 * ((-1 * x126 * ((-1 * x293 * x123) + x297)) + (x120 * ((x296 * x116) + (x295 * x113) + (x293 * x115) + (x64 * x294))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x298 * ((-1 * x113 * x114) + (x111 * x116) + (-1 * x64 * x117) + (x112 * x115))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x298 * ((-1 * x113 * x146) + (-1 * x64 * x149) + (x115 * x148) + (x116 * x147))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x298 * ((-1 * x113 * x166) + (-1 * x64 * x167) + (x115 * x165) + (x116 * x164))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x298 * ((-1 * x113 * x177) + (-1 * x64 * x180) + (x116 * x178) + (x115 * x179))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x298 * ((-1 * x64 * x247) + (x246 * x116) + (-1 * x244 * x113) + (x245 * x115))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x298 * ((x277 * x115) + (-1 * x276 * x113) + (-1 * x64 * x279) + (x278 * x116))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x298 * ((-1 * x293 * x113) + (x294 * x116) + (-1 * x64 * x296) + (x295 * x115))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x305 * ((-1 * x304 * (x122 + (-1 * x301 * x117))) + (x300 * ((x114 * x116) + (x111 * x113) + (x115 * x117) + (x64 * x112))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x305 * ((-1 * x304 * (x150 + (-1 * x301 * x149))) + (x300 * ((x115 * x149) + (x116 * x146) + (x64 * x148) + (x113 * x147))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x305 * ((-1 * x304 * (x168 + (-1 * x301 * x167))) + (x300 * ((x116 * x166) + (x64 * x165) + (x115 * x167) + (x113 * x164))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x305 * ((-1 * x304 * (x181 + (-1 * x301 * x180))) + (x300 * ((x116 * x177) + (x113 * x178) + (x115 * x180) + (x64 * x179))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x305 * ((-1 * x304 * (x248 + (-1 * x247 * x301))) + (x300 * ((x244 * x116) + (x64 * x245) + (x246 * x113) + (x247 * x115))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x305 * ((-1 * x304 * (x280 + (-1 * x279 * x301))) + (x300 * ((x276 * x116) + (x64 * x277) + (x278 * x113) + (x279 * x115))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x305 * ((-1 * x304 * (x297 + (-1 * x296 * x301))) + (x300 * ((x296 * x115) + (x293 * x116) + (x64 * x295) + (x294 * x113))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x375 * ((((x356 * x338) + (x373 * x342) + (x353 * x357) + (x367 * x354)) * x374) + (-1 * x362 * ((-1 * x353 * x355) + x344)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x375 * ((((x389 * x356) + (x399 * x342) + (x395 * x357) + (x397 * x354)) * x374) + (-1 * x362 * ((-1 * x395 * x355) + x390)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x375 * ((((x415 * x342) + (x412 * x357) + (x408 * x356) + (x414 * x354)) * x374) + (-1 * x362 * ((-1 * x412 * x355) + x409)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x375 * ((((x427 * x354) + (x421 * x356) + (x426 * x342) + (x425 * x357)) * x374) + (-1 * x362 * ((-1 * x425 * x355) + x422)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), ((-1 * x373 * x354) + (x357 * x338) + (x367 * x342) + (-1 * x353 * x356)) * x428); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), ((-1 * x399 * x354) + (x397 * x342) + (x389 * x357) + (-1 * x395 * x356)) * x428); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), ((-1 * x415 * x354) + (x414 * x342) + (x408 * x357) + (-1 * x412 * x356)) * x428); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), ((-1 * x426 * x354) + (-1 * x425 * x356) + (x421 * x357) + (x427 * x342)) * x428); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x435 * ((((x354 * x338) + (x353 * x342) + (x373 * x357) + (x367 * x356)) * x434) + (-1 * x433 * (x344 + (-1 * x429 * x373))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x435 * ((((x389 * x354) + (x395 * x342) + (x399 * x357) + (x397 * x356)) * x434) + (-1 * x433 * (x390 + (-1 * x429 * x399))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x435 * ((((x408 * x354) + (x412 * x342) + (x414 * x356) + (x415 * x357)) * x434) + (-1 * x433 * (x409 + (-1 * x415 * x429))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x435 * ((((x425 * x342) + (x426 * x357) + (x421 * x354) + (x427 * x356)) * x434) + (-1 * x433 * (x422 + (-1 * x426 * x429))))); } // Full version Jacobian of SurviveKalmanModelErrorPredict wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveKalmanModelErrorPredict wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanModelErrorPredict_jac_error_model(CnMat* Hx, const FLT t, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = 1.0/2.0 * t; const FLT x1 = fabs(t) * x0; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x5 = (*_x0).Pose.Rot[3] + (x4 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (x3 * (*_x0).Pose.Rot[0]); const FLT x6 = (-1 * x3 * (*_x0).Pose.Rot[1]) + (x4 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (-1 * x4 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x3 * (*_x0).Pose.Rot[3]); const FLT x8 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x4 * (*_x0).Pose.Rot[3]) + (x3 * (*_x0).Pose.Rot[2]); const FLT x9 = (x7 * x7) + (x5 * x5) + (x8 * x8) + (x6 * x6); const FLT x10 = 1. / (x9 * sqrt(x9)); const FLT x11 = 1.0 * x7; const FLT x12 = 1.0 * x5; const FLT x13 = 1.0 * (*_x0).Pose.Rot[0]; const FLT x14 = 1.0 * x6; const FLT x15 = x10 * ((x8 * x13) + (x14 * (*_x0).Pose.Rot[3]) + (-1 * x11 * (*_x0).Pose.Rot[1]) + (-1 * x12 * (*_x0).Pose.Rot[2])); const FLT x16 = t * t; const FLT x17 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x18 = x17 * x17; const FLT x19 = x18 * x16; const FLT x20 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x21 = x20 * x20; const FLT x22 = x21 * x16; const FLT x23 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x24 = x23 * x23; const FLT x25 = x24 * x16; const FLT x26 = 1e-10 + x25 + x19 + x22; const FLT x27 = sqrt(x26); const FLT x28 = 0.5 * x27; const FLT x29 = cos(x28); const FLT x30 = sin(x28); const FLT x31 = x30 * x30; const FLT x32 = 1. / x26; const FLT x33 = x32 * x31; const FLT x34 = (x33 * x22) + (x33 * x19) + (x29 * x29) + (x33 * x25); const FLT x35 = 1. / sqrt(x34); const FLT x36 = x35 * x29; const FLT x37 = 1.0/2.0 * x36; const FLT x38 = x7 * x37; const FLT x39 = x30 * (1. / x27); const FLT x40 = x0 * x39; const FLT x41 = x40 * x20; const FLT x42 = x8 * x35; const FLT x43 = x42 * x15; const FLT x44 = x40 * x17; const FLT x45 = x5 * x35; const FLT x46 = x45 * x15; const FLT x47 = x39 * x23; const FLT x48 = x0 * x47; const FLT x49 = x6 * x35; const FLT x50 = x49 * x15; const FLT x51 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x52 = 1. / sqrt(x9); const FLT x53 = t * x35; const FLT x54 = x53 * x39; const FLT x55 = x54 * x52; const FLT x56 = x55 * x20; const FLT x57 = x51 * x56; const FLT x58 = x52 * x36; const FLT x59 = 0.5 * x58; const FLT x60 = x59 * (*_x0).Pose.Rot[1]; const FLT x61 = x55 * x17; const FLT x62 = 0.5 * x61; const FLT x63 = x62 * (*_x0).Pose.Rot[2]; const FLT x64 = x53 * x47; const FLT x65 = x64 * x52; const FLT x66 = 0.5 * (*_x0).Pose.Rot[3]; const FLT x67 = x65 * x66; const FLT x68 = (-1 * x67) + x63 + (-1 * x57) + (-1 * x60); const FLT x69 = (x50 * x48) + x68 + (x44 * x46) + (-1 * x38 * x15) + (x41 * x43); const FLT x70 = 0.5 * x56; const FLT x71 = x70 * (*_x0).Pose.Rot[2]; const FLT x72 = x7 * x35; const FLT x73 = x72 * x15; const FLT x74 = x41 * x15; const FLT x75 = x61 * x51; const FLT x76 = x6 * x37; const FLT x77 = x59 * (*_x0).Pose.Rot[3]; const FLT x78 = 0.5 * x65; const FLT x79 = x78 * (*_x0).Pose.Rot[1]; const FLT x80 = (-1 * x79) + x77 + (-1 * x76 * x15) + (-1 * x73 * x48) + x71 + (x74 * x45) + x75 + (-1 * x43 * x44); const FLT x81 = x5 * x37; const FLT x82 = x66 * x56; const FLT x83 = x62 * (*_x0).Pose.Rot[1]; const FLT x84 = x59 * (*_x0).Pose.Rot[2]; const FLT x85 = x65 * x51; const FLT x86 = (-1 * x85) + x82 + (-1 * x84) + (-1 * x83); const FLT x87 = (-1 * x73 * x44) + (-1 * x81 * x15) + (x43 * x48) + x86 + (-1 * x50 * x41); const FLT x88 = 1.0/2.0 * x8; const FLT x89 = x88 * x36; const FLT x90 = (-1 * x78 * (*_x0).Pose.Rot[2]) + (x59 * (*_x0).Pose.Rot[0]) + (-1 * x61 * x66) + (-1 * x70 * (*_x0).Pose.Rot[1]); const FLT x91 = x90 + (x50 * x44) + (-1 * x89 * x15) + (-1 * x46 * x48) + (-1 * x72 * x74); const FLT x92 = (-1 * x87 * (*_x0).Pose.Rot[2]) + (x91 * (*_x0).Pose.Rot[0]) + (-1 * x69 * (*_x0).Pose.Rot[1]) + (x80 * (*_x0).Pose.Rot[3]); const FLT x93 = x8 * x55; const FLT x94 = x6 * x58; const FLT x95 = x7 * x52; const FLT x96 = x5 * x52; const FLT x97 = x54 * x96; const FLT x98 = (x64 * x95) + (-1 * x97 * x20) + (x93 * x17) + x94; const FLT x99 = x8 * x52; const FLT x100 = x96 * x36; const FLT x101 = x7 * x55; const FLT x102 = x6 * x52; const FLT x103 = x54 * x102; const FLT x104 = (x20 * x103) + (x17 * x101) + (-1 * x64 * x99) + x100; const FLT x105 = x7 * x58; const FLT x106 = (-1 * x93 * x20) + x105 + (-1 * x97 * x17) + (-1 * x64 * x102); const FLT x107 = x8 * x58; const FLT x108 = (x20 * x101) + (-1 * x17 * x103) + (x64 * x96) + x107; const FLT x109 = (x108 * (*_x0).Pose.Rot[1]) + (x98 * (*_x0).Pose.Rot[2]) + (x106 * (*_x0).Pose.Rot[0]) + (x104 * (*_x0).Pose.Rot[3]); const FLT x110 = (x87 * (*_x0).Pose.Rot[3]) + (x80 * (*_x0).Pose.Rot[2]) + (x91 * (*_x0).Pose.Rot[1]) + (x69 * (*_x0).Pose.Rot[0]); const FLT x111 = (-1 * x106 * (*_x0).Pose.Rot[1]) + (x98 * (*_x0).Pose.Rot[3]) + (-1 * x104 * (*_x0).Pose.Rot[2]) + (x108 * (*_x0).Pose.Rot[0]); const FLT x112 = (-1 * x69 * (*_x0).Pose.Rot[2]) + (-1 * x91 * (*_x0).Pose.Rot[3]) + (x87 * (*_x0).Pose.Rot[1]) + (x80 * (*_x0).Pose.Rot[0]); const FLT x113 = (-1 * x98 * (*_x0).Pose.Rot[1]) + (x104 * (*_x0).Pose.Rot[0]) + (x108 * (*_x0).Pose.Rot[2]) + (-1 * x106 * (*_x0).Pose.Rot[3]); const FLT x114 = (x91 * (*_x0).Pose.Rot[2]) + (x87 * (*_x0).Pose.Rot[0]) + (-1 * x69 * (*_x0).Pose.Rot[3]) + (-1 * x80 * (*_x0).Pose.Rot[1]); const FLT x115 = (x104 * (*_x0).Pose.Rot[1]) + (x98 * (*_x0).Pose.Rot[0]) + (-1 * x108 * (*_x0).Pose.Rot[3]) + (-1 * x106 * (*_x0).Pose.Rot[2]); const FLT x116 = x115 * x115; const FLT x117 = 1 + (-2 * ((x111 * x111) + x116)); const FLT x118 = 2 * (1. / x117); const FLT x119 = 4 * x115; const FLT x120 = -1 * x112 * x119; const FLT x121 = 4 * x111; const FLT x122 = x117 * x117; const FLT x123 = (x109 * x111) + (x113 * x115); const FLT x124 = 2 * x123 * (1. / x122); const FLT x125 = x122 * (1. / (x122 + (4 * (x123 * x123)))); const FLT x126 = 1.0 * x8; const FLT x127 = x10 * ((x6 * x13) + (x12 * (*_x0).Pose.Rot[1]) + (-1 * x126 * (*_x0).Pose.Rot[3]) + (-1 * x11 * (*_x0).Pose.Rot[2])); const FLT x128 = x41 * x127; const FLT x129 = x5 * x127; const FLT x130 = x35 * x129; const FLT x131 = x49 * x127; const FLT x132 = x86 + (x48 * x131) + (x44 * x130) + (-1 * x38 * x127) + (x42 * x128); const FLT x133 = x72 * x127; const FLT x134 = x42 * x127; const FLT x135 = x90 + (x41 * x130) + (-1 * x76 * x127) + (-1 * x48 * x133) + (-1 * x44 * x134); const FLT x136 = x60 + x57 + (x48 * x134) + x67 + (-1 * x37 * x129) + (-1 * x44 * x133) + (-1 * x41 * x131) + (-1 * x63); const FLT x137 = (-1 * x75) + x79 + (-1 * x71) + (-1 * x77); const FLT x138 = (-1 * x72 * x128) + x137 + (-1 * x89 * x127) + (x44 * x131) + (-1 * x48 * x130); const FLT x139 = (-1 * x136 * (*_x0).Pose.Rot[2]) + (-1 * x132 * (*_x0).Pose.Rot[1]) + (x138 * (*_x0).Pose.Rot[0]) + (x135 * (*_x0).Pose.Rot[3]); const FLT x140 = (x136 * (*_x0).Pose.Rot[3]) + (x135 * (*_x0).Pose.Rot[2]) + (x138 * (*_x0).Pose.Rot[1]) + (x132 * (*_x0).Pose.Rot[0]); const FLT x141 = (-1 * x132 * (*_x0).Pose.Rot[2]) + (-1 * x138 * (*_x0).Pose.Rot[3]) + (x136 * (*_x0).Pose.Rot[1]) + (x135 * (*_x0).Pose.Rot[0]); const FLT x142 = (-1 * x132 * (*_x0).Pose.Rot[3]) + (-1 * x135 * (*_x0).Pose.Rot[1]) + (x138 * (*_x0).Pose.Rot[2]) + (x136 * (*_x0).Pose.Rot[0]); const FLT x143 = -1 * x119 * x141; const FLT x144 = x10 * ((x5 * x13) + (-1 * x14 * (*_x0).Pose.Rot[1]) + (x126 * (*_x0).Pose.Rot[2]) + (-1 * x11 * (*_x0).Pose.Rot[3])); const FLT x145 = x45 * x144; const FLT x146 = x42 * x144; const FLT x147 = x49 * x144; const FLT x148 = (x48 * x147) + x137 + (x41 * x146) + (-1 * x38 * x144) + (x44 * x145); const FLT x149 = x72 * x144; const FLT x150 = (-1 * x89 * x144) + x85 + x83 + (-1 * x82) + (-1 * x41 * x149) + (x44 * x147) + (-1 * x48 * x145) + x84; const FLT x151 = x68 + (-1 * x44 * x146) + (x41 * x145) + (-1 * x48 * x149) + (-1 * x76 * x144); const FLT x152 = x90 + (-1 * x81 * x144) + (x48 * x146) + (-1 * x41 * x147) + (-1 * x44 * x149); const FLT x153 = (x152 * (*_x0).Pose.Rot[3]) + (x148 * (*_x0).Pose.Rot[0]) + (x151 * (*_x0).Pose.Rot[2]) + (x150 * (*_x0).Pose.Rot[1]); const FLT x154 = (x150 * (*_x0).Pose.Rot[0]) + (-1 * x152 * (*_x0).Pose.Rot[2]) + (x151 * (*_x0).Pose.Rot[3]) + (-1 * x148 * (*_x0).Pose.Rot[1]); const FLT x155 = (-1 * x148 * (*_x0).Pose.Rot[2]) + (-1 * x150 * (*_x0).Pose.Rot[3]) + (x151 * (*_x0).Pose.Rot[0]) + (x152 * (*_x0).Pose.Rot[1]); const FLT x156 = (-1 * x148 * (*_x0).Pose.Rot[3]) + (x150 * (*_x0).Pose.Rot[2]) + (-1 * x151 * (*_x0).Pose.Rot[1]) + (x152 * (*_x0).Pose.Rot[0]); const FLT x157 = -1 * x119 * x155; const FLT x158 = -1 * x97; const FLT x159 = t * t * t; const FLT x160 = x30 * (1. / (x26 * sqrt(x26))); const FLT x161 = x35 * x160; const FLT x162 = x161 * x159; const FLT x163 = x17 * x162; const FLT x164 = x99 * x20; const FLT x165 = x164 * x163; const FLT x166 = x20 * x20 * x20; const FLT x167 = t * t * t * t; const FLT x168 = 1.0 * x29; const FLT x169 = x160 * x168; const FLT x170 = x169 * x167; const FLT x171 = 2 * x31 * (1. / (x26 * x26)); const FLT x172 = x167 * x171; const FLT x173 = x18 * x170; const FLT x174 = x20 * x16; const FLT x175 = 2 * x33; const FLT x176 = x24 * x20; const FLT x177 = x18 * x172; const FLT x178 = x39 * x168; const FLT x179 = (-1 * x176 * x172) + (-1 * x20 * x177) + (-1 * x178 * x174) + (-1 * x166 * x172) + (x166 * x170) + (x174 * x175) + (x20 * x173) + (x170 * x176); const FLT x180 = 1. / (x34 * sqrt(x34)); const FLT x181 = x29 * x180; const FLT x182 = x179 * x181; const FLT x183 = 1.0/2.0 * x102; const FLT x184 = 0.5 * x35; const FLT x185 = x174 * x184; const FLT x186 = x39 * x185; const FLT x187 = x179 * x180; const FLT x188 = x99 * x44; const FLT x189 = 0.5 * x32; const FLT x190 = x189 * x159; const FLT x191 = x17 * x190; const FLT x192 = x20 * x107; const FLT x193 = x191 * x192; const FLT x194 = x21 * x162; const FLT x195 = x48 * x180; const FLT x196 = x179 * x195; const FLT x197 = x96 * x20; const FLT x198 = x40 * x187; const FLT x199 = x21 * x190; const FLT x200 = x23 * x190; const FLT x201 = x20 * x200; const FLT x202 = x23 * x162; const FLT x203 = x95 * x20; const FLT x204 = (-1 * x202 * x203) + (x201 * x105); const FLT x205 = (x96 * x194) + x193 + (-1 * x95 * x196) + (-1 * x165) + (-1 * x187 * x188) + x204 + (x197 * x198) + x158 + (-1 * x100 * x199) + (-1 * x183 * x182) + (-1 * x102 * x186); const FLT x206 = -1 * x93; const FLT x207 = x20 * x102; const FLT x208 = x202 * x207; const FLT x209 = x94 * x201; const FLT x210 = x96 * x39; const FLT x211 = x0 * x17; const FLT x212 = x211 * x187; const FLT x213 = 1.0/2.0 * x7; const FLT x214 = x52 * x182; const FLT x215 = x95 * x39; const FLT x216 = x20 * x191; const FLT x217 = (x163 * x197) + (-1 * x216 * x100); const FLT x218 = (-1 * x215 * x185) + (x102 * x196) + (-1 * x107 * x199) + (-1 * x213 * x214) + x208 + (x99 * x194) + x206 + x217 + (x164 * x198) + (-1 * x209) + (x210 * x212); const FLT x219 = 1.0/2.0 * x96; const FLT x220 = (x216 * x105) + (-1 * x203 * x163); const FLT x221 = (-1 * x200 * x192) + (x202 * x164); const FLT x222 = x221 + (x94 * x199) + (-1 * x102 * x194) + (-1 * x212 * x215) + (-1 * x207 * x198) + (x99 * x196) + x103 + (-1 * x219 * x182) + x220 + (-1 * x210 * x185); const FLT x223 = x202 * x197; const FLT x224 = x94 * x216; const FLT x225 = x201 * x100; const FLT x226 = x207 * x163; const FLT x227 = x44 * x102; const FLT x228 = (x105 * x199) + x225 + x101 + (-1 * x95 * x194) + (-1 * x88 * x214) + (-1 * x99 * x186) + (-1 * x96 * x196) + (-1 * x224) + (-1 * x223) + (x227 * x187) + x226 + (-1 * x203 * x198); const FLT x229 = (x228 * (*_x0).Pose.Rot[0]) + (x205 * (*_x0).Pose.Rot[3]) + (-1 * x222 * (*_x0).Pose.Rot[2]) + (-1 * x218 * (*_x0).Pose.Rot[1]); const FLT x230 = (-1 * x218 * (*_x0).Pose.Rot[2]) + (-1 * x228 * (*_x0).Pose.Rot[3]) + (x205 * (*_x0).Pose.Rot[0]) + (x222 * (*_x0).Pose.Rot[1]); const FLT x231 = (x222 * (*_x0).Pose.Rot[3]) + (x205 * (*_x0).Pose.Rot[2]) + (x218 * (*_x0).Pose.Rot[0]) + (x228 * (*_x0).Pose.Rot[1]); const FLT x232 = (-1 * x218 * (*_x0).Pose.Rot[3]) + (x228 * (*_x0).Pose.Rot[2]) + (-1 * x205 * (*_x0).Pose.Rot[1]) + (x222 * (*_x0).Pose.Rot[0]); const FLT x233 = -1 * x230 * x119; const FLT x234 = x23 * x21; const FLT x235 = x47 * x16; const FLT x236 = x16 * x175; const FLT x237 = (x23 * x23 * x23) * x167; const FLT x238 = (x237 * x169) + (-1 * x237 * x171) + (-1 * x235 * x168) + (x234 * x170) + (-1 * x23 * x177) + (-1 * x234 * x172) + (x23 * x173) + (x23 * x236); const FLT x239 = x238 * x195; const FLT x240 = x24 * x162; const FLT x241 = x238 * x181; const FLT x242 = x238 * x180; const FLT x243 = x40 * x242; const FLT x244 = x24 * x190; const FLT x245 = x235 * x184; const FLT x246 = x211 * x242; const FLT x247 = x23 * x191; const FLT x248 = x17 * x202; const FLT x249 = (-1 * x95 * x248) + (x247 * x105); const FLT x250 = x249 + (-1 * x244 * x107) + (-1 * x207 * x243) + (-1 * x219 * x241) + (x99 * x239) + (-1 * x215 * x246) + (-1 * x208) + x206 + (x99 * x240) + (-1 * x96 * x245) + x209; const FLT x251 = -1 * x103; const FLT x252 = x96 * x248; const FLT x253 = x247 * x100; const FLT x254 = x52 * x241; const FLT x255 = (-1 * x213 * x254) + (-1 * x253) + x221 + x252 + (x210 * x246) + (-1 * x94 * x244) + (-1 * x95 * x245) + (x239 * x102) + x251 + (x243 * x164) + (x240 * x102); const FLT x256 = x99 * x248; const FLT x257 = x247 * x107; const FLT x258 = (-1 * x225) + (-1 * x95 * x239) + (-1 * x242 * x188) + x257 + (-1 * x245 * x102) + (x243 * x197) + (-1 * x241 * x183) + (-1 * x256) + (x244 * x105) + x223 + (-1 * x95 * x240) + x101; const FLT x259 = (x248 * x102) + (-1 * x94 * x247); const FLT x260 = x204 + x259 + (-1 * x96 * x239) + (-1 * x99 * x245) + (-1 * x88 * x254) + (-1 * x96 * x240) + (x227 * x242) + (-1 * x203 * x243) + (x244 * x100) + x97; const FLT x261 = (x260 * (*_x0).Pose.Rot[0]) + (x258 * (*_x0).Pose.Rot[3]) + (-1 * x250 * (*_x0).Pose.Rot[2]) + (-1 * x255 * (*_x0).Pose.Rot[1]); const FLT x262 = (-1 * x255 * (*_x0).Pose.Rot[2]) + (x250 * (*_x0).Pose.Rot[1]) + (x258 * (*_x0).Pose.Rot[0]) + (-1 * x260 * (*_x0).Pose.Rot[3]); const FLT x263 = (x250 * (*_x0).Pose.Rot[3]) + (x258 * (*_x0).Pose.Rot[2]) + (x260 * (*_x0).Pose.Rot[1]) + (x255 * (*_x0).Pose.Rot[0]); const FLT x264 = (-1 * x255 * (*_x0).Pose.Rot[3]) + (x260 * (*_x0).Pose.Rot[2]) + (-1 * x258 * (*_x0).Pose.Rot[1]) + (x250 * (*_x0).Pose.Rot[0]); const FLT x265 = -1 * x262 * x119; const FLT x266 = x17 * x170; const FLT x267 = x17 * x16; const FLT x268 = x17 * x17 * x17; const FLT x269 = x17 * x172; const FLT x270 = (-1 * x24 * x269) + (x24 * x266) + (x17 * x236) + (-1 * x267 * x178) + (-1 * x268 * x172) + (x21 * x266) + (-1 * x21 * x269) + (x268 * x170); const FLT x271 = x270 * x181; const FLT x272 = x18 * x159; const FLT x273 = x272 * x189; const FLT x274 = x270 * x180; const FLT x275 = x40 * x274; const FLT x276 = x272 * x161; const FLT x277 = x267 * x184; const FLT x278 = x39 * x277; const FLT x279 = x270 * x195; const FLT x280 = x217 + (-1 * x95 * x279) + (-1 * x278 * x102) + (x273 * x107) + x249 + (-1 * x99 * x276) + (-1 * x271 * x183) + x93 + (-1 * x274 * x188) + (x275 * x197); const FLT x281 = x211 * x274; const FLT x282 = x52 * x271; const FLT x283 = (-1 * x193) + (x96 * x276) + x165 + x259 + (x210 * x281) + x158 + (-1 * x213 * x282) + (x275 * x164) + (x279 * x102) + (-1 * x215 * x277) + (-1 * x273 * x100); const FLT x284 = x224 + (-1 * x226) + x256 + (-1 * x219 * x271) + (x273 * x105) + (-1 * x257) + (-1 * x207 * x275) + (x99 * x279) + (-1 * x210 * x277) + (-1 * x215 * x281) + (-1 * x95 * x276) + x101; const FLT x285 = (-1 * x94 * x273) + x251 + (-1 * x252) + (-1 * x96 * x279) + (-1 * x99 * x278) + x220 + (-1 * x203 * x275) + (x274 * x227) + (-1 * x88 * x282) + (x276 * x102) + x253; const FLT x286 = (x285 * (*_x0).Pose.Rot[0]) + (x280 * (*_x0).Pose.Rot[3]) + (-1 * x284 * (*_x0).Pose.Rot[2]) + (-1 * x283 * (*_x0).Pose.Rot[1]); const FLT x287 = (x284 * (*_x0).Pose.Rot[3]) + (x280 * (*_x0).Pose.Rot[2]) + (x283 * (*_x0).Pose.Rot[0]) + (x285 * (*_x0).Pose.Rot[1]); const FLT x288 = (-1 * x283 * (*_x0).Pose.Rot[2]) + (x284 * (*_x0).Pose.Rot[1]) + (-1 * x285 * (*_x0).Pose.Rot[3]) + (x280 * (*_x0).Pose.Rot[0]); const FLT x289 = (-1 * x283 * (*_x0).Pose.Rot[3]) + (x284 * (*_x0).Pose.Rot[0]) + (-1 * x280 * (*_x0).Pose.Rot[1]) + (x285 * (*_x0).Pose.Rot[2]); const FLT x290 = -1 * x288 * x119; const FLT x291 = 2 * (1. / sqrt(1 + (-4 * (((x109 * x115) + (-1 * x111 * x113)) * ((x109 * x115) + (-1 * x111 * x113)))))); const FLT x292 = 1 + (-2 * (x116 + (x113 * x113))); const FLT x293 = 2 * (1. / x292); const FLT x294 = 4 * x113; const FLT x295 = x292 * x292; const FLT x296 = (x109 * x113) + (x111 * x115); const FLT x297 = 2 * x296 * (1. / x295); const FLT x298 = x295 * (1. / (x295 + (4 * (x296 * x296)))); const FLT x299 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x300 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x301 = 0.5 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x302 = (x301 * (*_x0).IMUBias.IMUCorrection[1]) + (x300 * (*error_model).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[3] + (-1 * x299 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x303 = (*_x0).IMUBias.IMUCorrection[1] + (-1 * x301 * (*_x0).IMUBias.IMUCorrection[3]) + (x299 * (*error_model).IMUBias.IMUCorrection[2]) + (x300 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x304 = 1.0 * x303; const FLT x305 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x306 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x307 = (-1 * x306 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[0] + (-1 * x305 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x301 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x308 = 1.0 * x307; const FLT x309 = (-1 * x306 * (*error_model).IMUBias.IMUCorrection[2]) + (x301 * (*_x0).IMUBias.IMUCorrection[0]) + (x305 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x310 = 1.0 * x309; const FLT x311 = 1.0 * x302; const FLT x312 = (x307 * x307) + (x303 * x303) + (x309 * x309) + (x302 * x302); const FLT x313 = 1.0/2.0 * (1. / (x312 * sqrt(x312))); const FLT x314 = ((x310 * (*_x0).IMUBias.IMUCorrection[3]) + (x304 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x311 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x308 * (*_x0).IMUBias.IMUCorrection[1])) * x313; const FLT x315 = x302 * x314; const FLT x316 = x314 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x317 = x303 * x314; const FLT x318 = x307 * x314; const FLT x319 = (x318 * (*_x0).IMUBias.IMUCorrection[2]) + (x317 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x315 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x309 * x316); const FLT x320 = 1. / sqrt(x312); const FLT x321 = x307 * x320; const FLT x322 = x303 * x320; const FLT x323 = x309 * x320; const FLT x324 = x302 * x320; const FLT x325 = (x323 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x321 * (*_x0).IMUBias.IMUCorrection[2]) + (x324 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x322 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x326 = 4 * x325; const FLT x327 = -1 * x326 * x319; const FLT x328 = x309 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x329 = 0.5 * x320; const FLT x330 = (x329 * ((*_x0).IMUBias.IMUCorrection[2] * (*_x0).IMUBias.IMUCorrection[2])) + (x329 * ((*_x0).IMUBias.IMUCorrection[1] * (*_x0).IMUBias.IMUCorrection[1])) + (x329 * ((*_x0).IMUBias.IMUCorrection[0] * (*_x0).IMUBias.IMUCorrection[0])) + (x329 * ((*_x0).IMUBias.IMUCorrection[3] * (*_x0).IMUBias.IMUCorrection[3])); const FLT x331 = (-1 * x303 * x316) + (x318 * (*_x0).IMUBias.IMUCorrection[1]) + (x315 * (*_x0).IMUBias.IMUCorrection[2]) + x330 + (-1 * x328 * x314); const FLT x332 = (-1 * x324 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x321 * (*_x0).IMUBias.IMUCorrection[1]) + (x323 * (*_x0).IMUBias.IMUCorrection[3]) + (x322 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x333 = 4 * x332; const FLT x334 = (x324 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x321 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x323 * (*_x0).IMUBias.IMUCorrection[1]) + (x322 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x335 = (x322 * (*_x0).IMUBias.IMUCorrection[1]) + (x321 * (*_x0).IMUBias.IMUCorrection[0]) + (x324 * (*_x0).IMUBias.IMUCorrection[3]) + (x323 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x336 = (x335 * x332) + (x325 * x334); const FLT x337 = x325 * x325; const FLT x338 = 1 + (-2 * ((x332 * x332) + x337)); const FLT x339 = x338 * x338; const FLT x340 = 2 * (1. / x339) * x336; const FLT x341 = x307 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x342 = x309 * x314; const FLT x343 = (-1 * x342 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x317 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x314 * x341) + (-1 * x315 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x344 = (x318 * (*_x0).IMUBias.IMUCorrection[3]) + (x342 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x317 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x302 * x316); const FLT x345 = 2 * (1. / x338); const FLT x346 = x339 * (1. / (x339 + (4 * (x336 * x336)))); const FLT x347 = ((x310 * (*_x0).IMUBias.IMUCorrection[0]) + (x311 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x304 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x308 * (*_x0).IMUBias.IMUCorrection[2])) * x313; const FLT x348 = x302 * x347; const FLT x349 = x309 * x347; const FLT x350 = x347 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x351 = x303 * x347; const FLT x352 = x330 + (x350 * x307) + (-1 * x348 * (*_x0).IMUBias.IMUCorrection[1]) + (x351 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x349 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x353 = -1 * x352 * x326; const FLT x354 = x307 * x347; const FLT x355 = (-1 * x349 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x351 * (*_x0).IMUBias.IMUCorrection[0]) + (x354 * (*_x0).IMUBias.IMUCorrection[1]) + (x350 * x302); const FLT x356 = (-1 * x347 * x341) + (-1 * x348 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x351 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x350 * x309); const FLT x357 = (x354 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x350 * x303) + (x349 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x348 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x358 = ((x311 * (*_x0).IMUBias.IMUCorrection[0]) + (-1 * x310 * (*_x0).IMUBias.IMUCorrection[1]) + (x304 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x308 * (*_x0).IMUBias.IMUCorrection[3])) * x313; const FLT x359 = x358 * x302; const FLT x360 = x358 * x309; const FLT x361 = x358 * x307; const FLT x362 = x358 * x303; const FLT x363 = (x362 * (*_x0).IMUBias.IMUCorrection[3]) + (x361 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x359 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x360 * (*_x0).IMUBias.IMUCorrection[0]); const FLT x364 = -1 * x363 * x326; const FLT x365 = (-1 * x358 * x328) + (-1 * x362 * (*_x0).IMUBias.IMUCorrection[0]) + (x361 * (*_x0).IMUBias.IMUCorrection[1]) + (x359 * (*_x0).IMUBias.IMUCorrection[2]); const FLT x366 = (-1 * x359 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x360 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x362 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x358 * x341); const FLT x367 = (-1 * x362 * (*_x0).IMUBias.IMUCorrection[2]) + x330 + (x360 * (*_x0).IMUBias.IMUCorrection[1]) + (-1 * x359 * (*_x0).IMUBias.IMUCorrection[0]) + (x361 * (*_x0).IMUBias.IMUCorrection[3]); const FLT x368 = 2 * (1. / sqrt(1 + (-4 * (((x325 * x335) + (-1 * x334 * x332)) * ((x325 * x335) + (-1 * x334 * x332)))))); const FLT x369 = 4 * x334; const FLT x370 = (x335 * x334) + (x325 * x332); const FLT x371 = 1 + (-2 * (x337 + (x334 * x334))); const FLT x372 = x371 * x371; const FLT x373 = 2 * (1. / x372) * x370; const FLT x374 = 2 * (1. / x371); const FLT x375 = x372 * (1. / (x372 + (4 * (x370 * x370)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), x1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x92 * x121) + x120)) + (x118 * ((x114 * x115) + (x112 * x113) + (x92 * x109) + (x110 * x111))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x121 * x139) + x143)) + (((x115 * x142) + (x113 * x141) + (x109 * x139) + (x111 * x140)) * x118))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x121 * x154) + x157)) + (((x115 * x156) + (x113 * x155) + (x111 * x153) + (x109 * x154)) * x118))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x229 * x121) + x233)) + (((x232 * x115) + (x231 * x111) + (x229 * x109) + (x230 * x113)) * x118))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x261 * x121) + x265)) + (((x263 * x111) + (x264 * x115) + (x261 * x109) + (x262 * x113)) * x118))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x125 * ((-1 * x124 * ((-1 * x286 * x121) + x290)) + (((x286 * x109) + (x289 * x115) + (x288 * x113) + (x287 * x111)) * x118))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x291 * ((-1 * x92 * x113) + (-1 * x111 * x114) + (x110 * x115) + (x109 * x112))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), ((-1 * x113 * x139) + (x109 * x141) + (-1 * x111 * x142) + (x115 * x140)) * x291); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), ((-1 * x113 * x154) + (-1 * x111 * x156) + (x115 * x153) + (x109 * x155)) * x291); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), ((-1 * x229 * x113) + (x231 * x115) + (-1 * x232 * x111) + (x230 * x109)) * x291); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), ((x262 * x109) + (-1 * x261 * x113) + (-1 * x264 * x111) + (x263 * x115)) * x291); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), ((-1 * x286 * x113) + (x287 * x115) + (-1 * x289 * x111) + (x288 * x109)) * x291); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x298 * ((-1 * x297 * (x120 + (-1 * x294 * x114))) + (x293 * ((x92 * x115) + (x111 * x112) + (x109 * x114) + (x110 * x113))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x298 * ((-1 * x297 * (x143 + (-1 * x294 * x142))) + (((x115 * x139) + (x113 * x140) + (x111 * x141) + (x109 * x142)) * x293))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x298 * ((-1 * x297 * (x157 + (-1 * x294 * x156))) + (((x111 * x155) + (x109 * x156) + (x115 * x154) + (x113 * x153)) * x293))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x298 * ((-1 * x297 * (x233 + (-1 * x232 * x294))) + (((x229 * x115) + (x230 * x111) + (x231 * x113) + (x232 * x109)) * x293))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x298 * ((-1 * x297 * (x265 + (-1 * x294 * x264))) + (((x261 * x115) + (x262 * x111) + (x263 * x113) + (x264 * x109)) * x293))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x298 * ((-1 * x297 * (x290 + (-1 * x294 * x289))) + (((x288 * x111) + (x289 * x109) + (x286 * x115) + (x287 * x113)) * x293))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), t); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x346 * ((((x325 * x344) + (x334 * x319) + (x335 * x331) + (x332 * x343)) * x345) + (-1 * x340 * ((-1 * x333 * x331) + x327)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x346 * ((((x357 * x325) + (x352 * x334) + (x356 * x332) + (x355 * x335)) * x345) + (-1 * x340 * ((-1 * x355 * x333) + x353)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x346 * ((((x367 * x325) + (x365 * x335) + (x363 * x334) + (x366 * x332)) * x345) + (-1 * x340 * ((-1 * x365 * x333) + x364)))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), ((-1 * x334 * x331) + (x335 * x319) + (-1 * x332 * x344) + (x325 * x343)) * x368); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), ((-1 * x357 * x332) + (-1 * x355 * x334) + (x352 * x335) + (x356 * x325)) * x368); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), ((-1 * x367 * x332) + (x363 * x335) + (x366 * x325) + (-1 * x365 * x334)) * x368); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x375 * ((((x332 * x319) + (x325 * x331) + (x335 * x344) + (x334 * x343)) * x374) + (-1 * x373 * (x327 + (-1 * x369 * x344))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x375 * ((((x355 * x325) + (x357 * x335) + (x352 * x332) + (x356 * x334)) * x374) + (-1 * x373 * (x353 + (-1 * x369 * x357))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x375 * ((((x365 * x325) + (x366 * x334) + (x363 * x332) + (x367 * x335)) * x374) + (-1 * x373 * (x364 + (-1 * x369 * x367))))); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); } // Full version Jacobian of SurviveKalmanModelErrorPredict wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveObsErrorModelNoFlip(SurviveAxisAnglePose* out, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { const FLT x0 = sqrt(1e-10 + ((*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[2]) + ((*_x0).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x0).Pose.Rot[1] * (*_x0).Pose.Rot[1])); const FLT x1 = 2 * (1. / x0) * atan2(x0, (*_x0).Pose.Rot[0]); out->Pos[0]=(*_x0).Pose.Pos[0] + (-1 * (*Z).Pos[0]); out->Pos[1]=(*_x0).Pose.Pos[1] + (-1 * (*Z).Pos[1]); out->Pos[2]=(*_x0).Pose.Pos[2] + (-1 * (*Z).Pos[2]); out->AxisAngleRot[0]=(x1 * (*_x0).Pose.Rot[1]) + (-1 * (*Z).AxisAngleRot[0]); out->AxisAngleRot[1]=(x1 * (*_x0).Pose.Rot[2]) + (-1 * (*Z).AxisAngleRot[1]); out->AxisAngleRot[2]=(x1 * (*_x0).Pose.Rot[3]) + (-1 * (*Z).AxisAngleRot[2]); } // Jacobian of SurviveObsErrorModelNoFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveObsErrorModelNoFlip_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { const FLT x0 = (*_x0).Pose.Rot[3] * (*_x0).Pose.Rot[3]; const FLT x1 = (*_x0).Pose.Rot[1] * (*_x0).Pose.Rot[1]; const FLT x2 = (*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[2]; const FLT x3 = 1e-10 + x2 + x0 + x1; const FLT x4 = 2 * (1. / (x3 + ((*_x0).Pose.Rot[0] * (*_x0).Pose.Rot[0]))); const FLT x5 = (1. / x3) * (*_x0).Pose.Rot[0]; const FLT x6 = x4 * x5; const FLT x7 = sqrt(x3); const FLT x8 = 2 * atan2(x7, (*_x0).Pose.Rot[0]); const FLT x9 = x8 * (1. / x7); const FLT x10 = (1. / (x3 * sqrt(x3))) * x8; const FLT x11 = x4 * (*_x0).Pose.Rot[2]; const FLT x12 = x5 * (*_x0).Pose.Rot[1]; const FLT x13 = (x12 * x11) + (-1 * x10 * (*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[1]); const FLT x14 = x10 * (*_x0).Pose.Rot[3]; const FLT x15 = x4 * (*_x0).Pose.Rot[3]; const FLT x16 = (x15 * x12) + (-1 * x14 * (*_x0).Pose.Rot[1]); const FLT x17 = (x5 * x15 * (*_x0).Pose.Rot[2]) + (-1 * x14 * (*_x0).Pose.Rot[2]); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), -1 * x4 * (*_x0).Pose.Rot[1]); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x1 * x10) + (x1 * x6) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x13); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x16); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), -1 * x11); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x13); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x2 * x10) + (x2 * x6) + x9); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x17); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), -1 * x15); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x16); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x17); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x0 * x6) + (-1 * x0 * x10) + x9); } // Full version Jacobian of SurviveObsErrorModelNoFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveObsErrorModelNoFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorModelNoFlip_jac_Z(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), -1); } // Full version Jacobian of SurviveObsErrorModelNoFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorModelFlip(SurviveAxisAnglePose* out, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { const FLT x0 = sqrt(1e-10 + ((*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[2]) + ((*_x0).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x0).Pose.Rot[1] * (*_x0).Pose.Rot[1])); const FLT x1 = 2 * (1. / x0) * atan2(x0, (*_x0).Pose.Rot[0]); const FLT x2 = (x1 * (*_x0).Pose.Rot[1]) + (-1 * (*Z).AxisAngleRot[0]); const FLT x3 = (x1 * (*_x0).Pose.Rot[3]) + (-1 * (*Z).AxisAngleRot[2]); const FLT x4 = (x1 * (*_x0).Pose.Rot[2]) + (-1 * (*Z).AxisAngleRot[1]); const FLT x5 = sqrt(1e-10 + (x4 * x4) + (x3 * x3) + (x2 * x2)); const FLT x6 = (1. / x5) * (-6.28318530717959 + x5); out->Pos[0]=(*_x0).Pose.Pos[0] + (-1 * (*Z).Pos[0]); out->Pos[1]=(*_x0).Pose.Pos[1] + (-1 * (*Z).Pos[1]); out->Pos[2]=(*_x0).Pose.Pos[2] + (-1 * (*Z).Pos[2]); out->AxisAngleRot[0]=x2 * x6; out->AxisAngleRot[1]=x4 * x6; out->AxisAngleRot[2]=x3 * x6; } // Jacobian of SurviveObsErrorModelFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveObsErrorModelFlip_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { const FLT x0 = (*_x0).Pose.Rot[3] * (*_x0).Pose.Rot[3]; const FLT x1 = (*_x0).Pose.Rot[1] * (*_x0).Pose.Rot[1]; const FLT x2 = (*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[2]; const FLT x3 = 1e-10 + x2 + x0 + x1; const FLT x4 = sqrt(x3); const FLT x5 = 2 * atan2(x4, (*_x0).Pose.Rot[0]); const FLT x6 = (1. / x4) * x5; const FLT x7 = (x6 * (*_x0).Pose.Rot[2]) + (-1 * (*Z).AxisAngleRot[1]); const FLT x8 = 1. / (x3 + ((*_x0).Pose.Rot[0] * (*_x0).Pose.Rot[0])); const FLT x9 = 4 * x8; const FLT x10 = (x6 * (*_x0).Pose.Rot[3]) + (-1 * (*Z).AxisAngleRot[2]); const FLT x11 = (x6 * (*_x0).Pose.Rot[1]) + (-1 * (*Z).AxisAngleRot[0]); const FLT x12 = (-1 * x9 * x11 * (*_x0).Pose.Rot[1]) + (-1 * x7 * x9 * (*_x0).Pose.Rot[2]) + (-1 * x9 * x10 * (*_x0).Pose.Rot[3]); const FLT x13 = 1e-10 + (x10 * x10) + (x7 * x7) + (x11 * x11); const FLT x14 = 1.0/2.0 * (1. / x13); const FLT x15 = x14 * x11; const FLT x16 = sqrt(x13); const FLT x17 = -6.28318530717959 + x16; const FLT x18 = 1.0/2.0 * (1. / (x13 * sqrt(x13))) * x17; const FLT x19 = x11 * x18; const FLT x20 = x17 * (1. / x16); const FLT x21 = 2 * x8; const FLT x22 = x20 * x21; const FLT x23 = (1. / (x3 * sqrt(x3))) * x5; const FLT x24 = (*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[1]; const FLT x25 = (1. / x3) * x21 * (*_x0).Pose.Rot[0]; const FLT x26 = (x24 * x25) + (-1 * x24 * x23); const FLT x27 = 2 * x7; const FLT x28 = x23 * (*_x0).Pose.Rot[3]; const FLT x29 = x25 * (*_x0).Pose.Rot[3]; const FLT x30 = (x29 * (*_x0).Pose.Rot[1]) + (-1 * x28 * (*_x0).Pose.Rot[1]); const FLT x31 = 2 * x10; const FLT x32 = (-1 * x1 * x23) + (x1 * x25) + x6; const FLT x33 = 2 * x11; const FLT x34 = (x32 * x33) + (x26 * x27) + (x30 * x31); const FLT x35 = (-1 * x2 * x23) + (x2 * x25) + x6; const FLT x36 = (x29 * (*_x0).Pose.Rot[2]) + (-1 * x28 * (*_x0).Pose.Rot[2]); const FLT x37 = (x33 * x26) + (x35 * x27) + (x31 * x36); const FLT x38 = x20 * x26; const FLT x39 = (-1 * x0 * x23) + (x0 * x25) + x6; const FLT x40 = (x30 * x33) + (x36 * x27) + (x31 * x39); const FLT x41 = x30 * x20; const FLT x42 = x7 * x14; const FLT x43 = x7 * x18; const FLT x44 = x36 * x20; const FLT x45 = x14 * x10; const FLT x46 = x10 * x18; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x22 * (*_x0).Pose.Rot[1]) + (x15 * x12) + (-1 * x12 * x19)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x32 * x20) + (x34 * x15) + (-1 * x34 * x19)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x38 + (x37 * x15) + (-1 * x37 * x19)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x41 + (x40 * x15) + (-1 * x40 * x19)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x22 * (*_x0).Pose.Rot[2]) + (x42 * x12) + (-1 * x43 * x12)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x42 * x34) + x38 + (-1 * x43 * x34)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x35 * x20) + (x42 * x37) + (-1 * x43 * x37)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x44 + (x40 * x42) + (-1 * x40 * x43)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x22 * (*_x0).Pose.Rot[3]) + (x45 * x12) + (-1 * x46 * x12)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x41 + (x45 * x34) + (-1 * x46 * x34)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x45 * x37) + x44 + (-1 * x46 * x37)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x40 * x45) + (x39 * x20) + (-1 * x40 * x46)); } // Full version Jacobian of SurviveObsErrorModelFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveObsErrorModelFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorModelFlip_jac_Z(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveAxisAnglePose* Z) { const FLT x0 = sqrt(1e-10 + ((*_x0).Pose.Rot[2] * (*_x0).Pose.Rot[2]) + ((*_x0).Pose.Rot[3] * (*_x0).Pose.Rot[3]) + ((*_x0).Pose.Rot[1] * (*_x0).Pose.Rot[1])); const FLT x1 = 2 * (1. / x0) * atan2(x0, (*_x0).Pose.Rot[0]); const FLT x2 = (x1 * (*_x0).Pose.Rot[3]) + (-1 * (*Z).AxisAngleRot[2]); const FLT x3 = x2 * x2; const FLT x4 = (x1 * (*_x0).Pose.Rot[1]) + (-1 * (*Z).AxisAngleRot[0]); const FLT x5 = x4 * x4; const FLT x6 = (x1 * (*_x0).Pose.Rot[2]) + (-1 * (*Z).AxisAngleRot[1]); const FLT x7 = x6 * x6; const FLT x8 = 1e-10 + x7 + x3 + x5; const FLT x9 = 1. / x8; const FLT x10 = sqrt(x8); const FLT x11 = -6.28318530717959 + x10; const FLT x12 = (1. / (x8 * sqrt(x8))) * x11; const FLT x13 = -1 * x11 * (1. / x10); const FLT x14 = x4 * x6; const FLT x15 = (x14 * x12) + (-1 * x9 * x14); const FLT x16 = x2 * x9; const FLT x17 = x2 * x12; const FLT x18 = (x4 * x17) + (-1 * x4 * x16); const FLT x19 = (x6 * x17) + (-1 * x6 * x16); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x5 * x9) + x13 + (x5 * x12)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), x15); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), x18); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), x15); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), x13 + (-1 * x7 * x9) + (x7 * x12)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), x18); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), x19); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x3 * x9) + x13 + (x3 * x12)); } // Full version Jacobian of SurviveObsErrorModelFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorStateErrorModelNoFlip(SurviveAxisAnglePose* out, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x3 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x4 = (x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[0]); const FLT x5 = x4 * x4; const FLT x6 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = x6 * x6; const FLT x8 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x9 = x3 * x3; const FLT x10 = x9 + (x8 * x8) + x5 + x7; const FLT x11 = 1. / sqrt(x10); const FLT x12 = 1. / x10; const FLT x13 = sqrt(1e-10 + (x7 * x12) + (x5 * x12) + (x9 * x12)); const FLT x14 = 2 * (1. / x13) * x11 * atan2(x13, x8 * x11); out->Pos[0]=(*_x0).Pose.Pos[0] + (-1 * (*Z).Pos[0]) + (*err).Pose.Pos[0]; out->Pos[1]=(*_x0).Pose.Pos[1] + (-1 * (*Z).Pos[1]) + (*err).Pose.Pos[1]; out->Pos[2]=(*_x0).Pose.Pos[2] + (-1 * (*Z).Pos[2]) + (*err).Pose.Pos[2]; out->AxisAngleRot[0]=(x3 * x14) + (-1 * (*Z).AxisAngleRot[0]); out->AxisAngleRot[1]=(x6 * x14) + (-1 * (*Z).AxisAngleRot[1]); out->AxisAngleRot[2]=(x4 * x14) + (-1 * (*Z).AxisAngleRot[2]); } // Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveObsErrorStateErrorModelNoFlip_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[1]; const FLT x3 = (x2 * (*err).Pose.AxisAngleRot[1]) + (-1 * x1 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x0 * (*_x0).Pose.Rot[0]); const FLT x4 = x3 * x3; const FLT x5 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x6 = (-1 * x2 * (*err).Pose.AxisAngleRot[2]) + (x5 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = x6 * x6; const FLT x8 = (-1 * x2 * (*err).Pose.AxisAngleRot[0]) + (*_x0).Pose.Rot[0] + (-1 * x0 * (*_x0).Pose.Rot[3]) + (-1 * x5 * (*_x0).Pose.Rot[2]); const FLT x9 = x8 * x8; const FLT x10 = (*_x0).Pose.Rot[1] + (x1 * (*_x0).Pose.Rot[0]) + (-1 * x5 * (*_x0).Pose.Rot[3]) + (x0 * (*_x0).Pose.Rot[2]); const FLT x11 = x10 * x10; const FLT x12 = x11 + x9 + x4 + x7; const FLT x13 = sqrt(x12); const FLT x14 = 1. / x13; const FLT x15 = 1. / x12; const FLT x16 = 1e-10 + (x4 * x15) + (x7 * x15) + (x15 * x11); const FLT x17 = sqrt(x16); const FLT x18 = 1. / x17; const FLT x19 = atan2(x17, x8 * x14); const FLT x20 = x19 * x18; const FLT x21 = x20 * x14; const FLT x22 = 1.0 * x21; const FLT x23 = x22 * (*err).Pose.AxisAngleRot[0]; const FLT x24 = 1. / (x12 * x12); const FLT x25 = 1.0 * x10; const FLT x26 = x25 * (*err).Pose.AxisAngleRot[0]; const FLT x27 = 1.0 * x3; const FLT x28 = x27 * (*err).Pose.AxisAngleRot[2]; const FLT x29 = 1.0 * (*err).Pose.AxisAngleRot[1]; const FLT x30 = x6 * x29; const FLT x31 = x30 + x28 + x26 + (2 * x8); const FLT x32 = x31 * x24; const FLT x33 = x24 * x11; const FLT x34 = (x30 * x15) + (x28 * x15) + (-1 * x7 * x32) + (-1 * x4 * x32) + (x26 * x15) + (-1 * x31 * x33); const FLT x35 = x14 * x19 * (1. / (x16 * sqrt(x16))); const FLT x36 = x35 * x10; const FLT x37 = 1.0/2.0 * (1. / x8); const FLT x38 = x37 * x13 * x18; const FLT x39 = x37 * x14 * x17; const FLT x40 = (1. / x9) * x13 * x17; const FLT x41 = (-1 * x40) + (x34 * x38) + (x31 * x39); const FLT x42 = 2 * x10; const FLT x43 = 1. / (x12 * sqrt(x12)); const FLT x44 = x9 * x43 * x18 * (1. / (x16 + (x9 * x15))); const FLT x45 = x42 * x44; const FLT x46 = x43 * x20; const FLT x47 = x46 * x31; const FLT x48 = 2 * x21; const FLT x49 = 1.0 * x8; const FLT x50 = x3 * x29; const FLT x51 = 1.0 * x6; const FLT x52 = x51 * (*err).Pose.AxisAngleRot[2]; const FLT x53 = x50 + (-1 * x52) + x42 + (-1 * x49 * (*err).Pose.AxisAngleRot[0]); const FLT x54 = x53 * x46; const FLT x55 = x53 * x24; const FLT x56 = (-1 * x7 * x55) + (-1 * x53 * x33) + (-1 * x4 * x55) + (x42 * x15) + (-1 * x52 * x15) + (x50 * x15); const FLT x57 = (x1 * x40) + (x56 * x38) + (x53 * x39); const FLT x58 = x22 * (*err).Pose.AxisAngleRot[2]; const FLT x59 = x25 * (*err).Pose.AxisAngleRot[2]; const FLT x60 = x27 * (*err).Pose.AxisAngleRot[0]; const FLT x61 = 2 * x6; const FLT x62 = x61 + (-1 * x60) + x59 + (-1 * x8 * x29); const FLT x63 = x46 * x10; const FLT x64 = x62 * x24; const FLT x65 = (x61 * x15) + (-1 * x4 * x64) + (-1 * x60 * x15) + (-1 * x64 * x11) + (-1 * x7 * x64) + (x59 * x15); const FLT x66 = (x65 * x38) + (x5 * x40) + (x62 * x39); const FLT x67 = x66 * x44; const FLT x68 = x21 * x29; const FLT x69 = x29 * x10; const FLT x70 = 2 * x3; const FLT x71 = x51 * (*err).Pose.AxisAngleRot[0]; const FLT x72 = x71 + (-1 * x69) + x70 + (-1 * x49 * (*err).Pose.AxisAngleRot[2]); const FLT x73 = x72 * x24; const FLT x74 = (x71 * x15) + (x70 * x15) + (-1 * x4 * x73) + (-1 * x7 * x73) + (-1 * x73 * x11) + (-1 * x69 * x15); const FLT x75 = (x0 * x40) + (x74 * x38) + (x72 * x39); const FLT x76 = x75 * x44; const FLT x77 = x6 * x35; const FLT x78 = x61 * x44; const FLT x79 = x62 * x46; const FLT x80 = x72 * x46; const FLT x81 = x70 * x44; const FLT x82 = x3 * x35; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x47 * x10) + (x41 * x45) + x23 + (-1 * x34 * x36)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x56 * x36) + (x57 * x45) + x48 + (-1 * x54 * x10)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x67 * x42) + x58 + (-1 * x65 * x36) + (-1 * x63 * x62)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x76 * x42) + (-1 * x74 * x36) + (-1 * x68) + (-1 * x72 * x63)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x77 * x34) + (-1 * x6 * x47) + (x78 * x41) + x68); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x6 * x54) + (x78 * x57) + (-1 * x58) + (-1 * x77 * x56)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x77 * x65) + (-1 * x6 * x79) + x48 + (x61 * x67)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x23 + (-1 * x74 * x77) + (x76 * x61) + (-1 * x6 * x80)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x82 * x34) + (x81 * x41) + x58 + (-1 * x3 * x47)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x81 * x57) + x68 + (-1 * x3 * x54) + (-1 * x82 * x56)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x3 * x79) + (-1 * x82 * x65) + (-1 * x23) + (x81 * x66)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x3 * x80) + (-1 * x82 * x74) + x48 + (x81 * x75)); } // Full version Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*err).Acc[0], (*err).Acc[1], (*err).Acc[2], (*err).IMUBias.AccBias[0], (*err).IMUBias.AccBias[1], (*err).IMUBias.AccBias[2], (*err).IMUBias.AccScale[0], (*err).IMUBias.AccScale[1], (*err).IMUBias.AccScale[2], (*err).IMUBias.GyroBias[0], (*err).IMUBias.GyroBias[1], (*err).IMUBias.GyroBias[2], (*err).IMUBias.IMUCorrection[0], (*err).IMUBias.IMUCorrection[1], (*err).IMUBias.IMUCorrection[2], (*err).Pose.AxisAngleRot[0], (*err).Pose.AxisAngleRot[1], (*err).Pose.AxisAngleRot[2], (*err).Pose.Pos[0], (*err).Pose.Pos[1], (*err).Pose.Pos[2], (*err).Velocity.AxisAngleRot[0], (*err).Velocity.AxisAngleRot[1], (*err).Velocity.AxisAngleRot[2], (*err).Velocity.Pos[0], (*err).Velocity.Pos[1], (*err).Velocity.Pos[2]] static inline void SurviveObsErrorStateErrorModelNoFlip_jac_err(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x3 = (x2 * (*_x0).Pose.Rot[1]) + (-1 * x1 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x0 * (*_x0).Pose.Rot[0]); const FLT x4 = x3 * x3; const FLT x5 = (-1 * x0 * (*_x0).Pose.Rot[1]) + (x2 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x6 = x5 * x5; const FLT x7 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x0 * (*_x0).Pose.Rot[3]) + (-1 * x2 * (*_x0).Pose.Rot[2]); const FLT x8 = x7 * x7; const FLT x9 = (*_x0).Pose.Rot[1] + (x1 * (*_x0).Pose.Rot[0]) + (-1 * x2 * (*_x0).Pose.Rot[3]) + (x0 * (*_x0).Pose.Rot[2]); const FLT x10 = x9 * x9; const FLT x11 = x8 + x4 + x10 + x6; const FLT x12 = sqrt(x11); const FLT x13 = 1. / x12; const FLT x14 = 1. / x11; const FLT x15 = 1e-10 + (x6 * x14) + (x4 * x14) + (x14 * x10); const FLT x16 = sqrt(x15); const FLT x17 = atan2(x16, x7 * x13); const FLT x18 = 1. / x16; const FLT x19 = 1.0 * x13 * x18 * x17; const FLT x20 = x19 * (*_x0).Pose.Rot[0]; const FLT x21 = 1. / (x11 * x11); const FLT x22 = 1.0 * x7; const FLT x23 = 1.0 * x3; const FLT x24 = x23 * (*_x0).Pose.Rot[2]; const FLT x25 = 1.0 * x9; const FLT x26 = x25 * (*_x0).Pose.Rot[0]; const FLT x27 = 1.0 * x5; const FLT x28 = x27 * (*_x0).Pose.Rot[3]; const FLT x29 = x28 + x26 + (-1 * x22 * (*_x0).Pose.Rot[1]) + (-1 * x24); const FLT x30 = x21 * x29; const FLT x31 = (-1 * x4 * x30) + (x28 * x14) + (-1 * x6 * x30) + (x26 * x14) + (-1 * x24 * x14) + (-1 * x30 * x10); const FLT x32 = 1.0/2.0 * (1. / x7); const FLT x33 = x32 * x12 * x18; const FLT x34 = x32 * x13 * x16; const FLT x35 = 0.5 * (1. / x8) * x12 * x16; const FLT x36 = (x35 * (*_x0).Pose.Rot[1]) + (x31 * x33) + (x34 * x29); const FLT x37 = (1. / (x11 * sqrt(x11))) * x18; const FLT x38 = 2 * x8 * x37 * (1. / (x15 + (x8 * x14))); const FLT x39 = x36 * x38; const FLT x40 = (1. / (x15 * sqrt(x15))) * x13; const FLT x41 = x40 * x31; const FLT x42 = x9 * x17; const FLT x43 = x37 * x29; const FLT x44 = x19 * (*_x0).Pose.Rot[3]; const FLT x45 = x25 * (*_x0).Pose.Rot[3]; const FLT x46 = x23 * (*_x0).Pose.Rot[1]; const FLT x47 = x27 * (*_x0).Pose.Rot[0]; const FLT x48 = x47 + (-1 * x22 * (*_x0).Pose.Rot[2]) + (-1 * x45) + x46; const FLT x49 = x48 * x21; const FLT x50 = (x47 * x14) + (-1 * x6 * x49) + (-1 * x4 * x49) + (x46 * x14) + (-1 * x45 * x14) + (-1 * x49 * x10); const FLT x51 = (x35 * (*_x0).Pose.Rot[2]) + (x50 * x33) + (x48 * x34); const FLT x52 = x51 * x38; const FLT x53 = x50 * x40; const FLT x54 = x48 * x37; const FLT x55 = x19 * (*_x0).Pose.Rot[2]; const FLT x56 = x25 * (*_x0).Pose.Rot[2]; const FLT x57 = x23 * (*_x0).Pose.Rot[0]; const FLT x58 = x27 * (*_x0).Pose.Rot[1]; const FLT x59 = (-1 * x58) + x57 + (-1 * x22 * (*_x0).Pose.Rot[3]) + x56; const FLT x60 = x59 * x37; const FLT x61 = x59 * x21; const FLT x62 = (-1 * x61 * x10) + (-1 * x4 * x61) + (-1 * x58 * x14) + (-1 * x6 * x61) + (x57 * x14) + (x56 * x14); const FLT x63 = (x35 * (*_x0).Pose.Rot[3]) + (x62 * x33) + (x59 * x34); const FLT x64 = x63 * x38; const FLT x65 = x62 * x40; const FLT x66 = x5 * x17; const FLT x67 = x5 * x38; const FLT x68 = x19 * (*_x0).Pose.Rot[1]; const FLT x69 = x3 * x17; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x20 + (-1 * x42 * x43) + (-1 * x41 * x42) + (x9 * x39)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x54 * x42) + (-1 * x44) + (-1 * x53 * x42) + (x9 * x52)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x65 * x42) + (x9 * x64) + x55 + (-1 * x60 * x42)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x67 * x36) + (-1 * x66 * x41) + x44 + (-1 * x66 * x43)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x66 * x53) + (x67 * x51) + (-1 * x66 * x54) + x20); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x65 * x66) + (-1 * x60 * x66) + (-1 * x68) + (x63 * x67)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x69 * x41) + (x3 * x39) + (-1 * x55) + (-1 * x69 * x43)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x69 * x53) + (x3 * x52) + x68 + (-1 * x69 * x54)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x65 * x69) + x20 + (-1 * x60 * x69) + (x3 * x64)); } // Full version Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*err).Acc[0], (*err).Acc[1], (*err).Acc[2], (*err).IMUBias.AccBias[0], (*err).IMUBias.AccBias[1], (*err).IMUBias.AccBias[2], (*err).IMUBias.AccScale[0], (*err).IMUBias.AccScale[1], (*err).IMUBias.AccScale[2], (*err).IMUBias.GyroBias[0], (*err).IMUBias.GyroBias[1], (*err).IMUBias.GyroBias[2], (*err).IMUBias.IMUCorrection[0], (*err).IMUBias.IMUCorrection[1], (*err).IMUBias.IMUCorrection[2], (*err).Pose.AxisAngleRot[0], (*err).Pose.AxisAngleRot[1], (*err).Pose.AxisAngleRot[2], (*err).Pose.Pos[0], (*err).Pose.Pos[1], (*err).Pose.Pos[2], (*err).Velocity.AxisAngleRot[0], (*err).Velocity.AxisAngleRot[1], (*err).Velocity.AxisAngleRot[2], (*err).Velocity.Pos[0], (*err).Velocity.Pos[1], (*err).Velocity.Pos[2]] // Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorStateErrorModelNoFlip_jac_Z(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), -1); } // Full version Jacobian of SurviveObsErrorStateErrorModelNoFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorStateErrorModelFlip(SurviveAxisAnglePose* out, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x3 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x4 = (x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[0]); const FLT x5 = x4 * x4; const FLT x6 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = x6 * x6; const FLT x8 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x9 = x3 * x3; const FLT x10 = x9 + (x8 * x8) + x5 + x7; const FLT x11 = 1. / sqrt(x10); const FLT x12 = 1. / x10; const FLT x13 = sqrt(1e-10 + (x7 * x12) + (x5 * x12) + (x9 * x12)); const FLT x14 = 2 * (1. / x13) * x11 * atan2(x13, x8 * x11); const FLT x15 = (x3 * x14) + (-1 * (*Z).AxisAngleRot[0]); const FLT x16 = (x4 * x14) + (-1 * (*Z).AxisAngleRot[2]); const FLT x17 = (x6 * x14) + (-1 * (*Z).AxisAngleRot[1]); const FLT x18 = sqrt(1e-10 + (x17 * x17) + (x16 * x16) + (x15 * x15)); const FLT x19 = (1. / x18) * (-6.28318530717959 + x18); out->Pos[0]=(*_x0).Pose.Pos[0] + (-1 * (*Z).Pos[0]) + (*err).Pose.Pos[0]; out->Pos[1]=(*_x0).Pose.Pos[1] + (-1 * (*Z).Pos[1]) + (*err).Pose.Pos[1]; out->Pos[2]=(*_x0).Pose.Pos[2] + (-1 * (*Z).Pos[2]) + (*err).Pose.Pos[2]; out->AxisAngleRot[0]=x15 * x19; out->AxisAngleRot[1]=x19 * x17; out->AxisAngleRot[2]=x19 * x16; } // Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveObsErrorStateErrorModelFlip_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x3 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x4 = 1.0 * x3; const FLT x5 = x4 * (*err).Pose.AxisAngleRot[0]; const FLT x6 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x7 = (x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[0]); const FLT x8 = 1.0 * x7; const FLT x9 = x8 * (*err).Pose.AxisAngleRot[2]; const FLT x10 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x11 = 1.0 * x10; const FLT x12 = x11 * (*err).Pose.AxisAngleRot[1]; const FLT x13 = x12 + x5 + x9 + (2 * x6); const FLT x14 = x7 * x7; const FLT x15 = x10 * x10; const FLT x16 = x6 * x6; const FLT x17 = x3 * x3; const FLT x18 = x17 + x14 + x16 + x15; const FLT x19 = 1. / (x18 * sqrt(x18)); const FLT x20 = 1. / x18; const FLT x21 = 1e-10 + (x20 * x15) + (x20 * x14) + (x20 * x17); const FLT x22 = sqrt(x21); const FLT x23 = 1. / x22; const FLT x24 = sqrt(x18); const FLT x25 = 1. / x24; const FLT x26 = atan2(x22, x6 * x25); const FLT x27 = x23 * x26; const FLT x28 = x27 * x19; const FLT x29 = x28 * x10; const FLT x30 = x25 * x27; const FLT x31 = 1.0 * x30; const FLT x32 = x31 * (*err).Pose.AxisAngleRot[1]; const FLT x33 = 1. / (x18 * x18); const FLT x34 = x33 * x15; const FLT x35 = x33 * x14; const FLT x36 = x33 * x17; const FLT x37 = (x20 * x12) + (x9 * x20) + (-1 * x34 * x13) + (-1 * x35 * x13) + (x5 * x20) + (-1 * x36 * x13); const FLT x38 = x25 * (1. / (x21 * sqrt(x21))) * x26; const FLT x39 = x38 * x10; const FLT x40 = 1.0/2.0 * (1. / x6); const FLT x41 = x40 * x24 * x23; const FLT x42 = x40 * x25 * x22; const FLT x43 = x24 * x22 * (1. / x16); const FLT x44 = (x41 * x37) + (-1 * x43) + (x42 * x13); const FLT x45 = 2 * x10; const FLT x46 = x23 * x19 * x16 * (1. / (x21 + (x20 * x16))); const FLT x47 = x45 * x46; const FLT x48 = (x44 * x47) + (-1 * x37 * x39) + (-1 * x29 * x13) + x32; const FLT x49 = (x45 * x30) + (-1 * (*Z).AxisAngleRot[1]); const FLT x50 = 2 * x49; const FLT x51 = x31 * (*err).Pose.AxisAngleRot[2]; const FLT x52 = x7 * x28; const FLT x53 = 2 * x7; const FLT x54 = x53 * x46; const FLT x55 = x7 * x38; const FLT x56 = (-1 * x55 * x37) + x51 + (x54 * x44) + (-1 * x52 * x13); const FLT x57 = (x53 * x30) + (-1 * (*Z).AxisAngleRot[2]); const FLT x58 = 2 * x57; const FLT x59 = x31 * (*err).Pose.AxisAngleRot[0]; const FLT x60 = x3 * x38; const FLT x61 = 2 * x3; const FLT x62 = x61 * x46; const FLT x63 = x3 * x28; const FLT x64 = (-1 * x63 * x13) + (x62 * x44) + x59 + (-1 * x60 * x37); const FLT x65 = (x61 * x30) + (-1 * (*Z).AxisAngleRot[0]); const FLT x66 = 2 * x65; const FLT x67 = (x64 * x66) + (x50 * x48) + (x58 * x56); const FLT x68 = 1e-10 + (x49 * x49) + (x57 * x57) + (x65 * x65); const FLT x69 = sqrt(x68); const FLT x70 = -6.28318530717959 + x69; const FLT x71 = 1.0/2.0 * x70 * (1. / (x68 * sqrt(x68))); const FLT x72 = x71 * x65; const FLT x73 = x70 * (1. / x69); const FLT x74 = 1.0/2.0 * (1. / x68); const FLT x75 = x74 * x67; const FLT x76 = 1.0 * x6; const FLT x77 = x8 * (*err).Pose.AxisAngleRot[1]; const FLT x78 = x11 * (*err).Pose.AxisAngleRot[2]; const FLT x79 = (-1 * x78) + x77 + x61 + (-1 * x76 * (*err).Pose.AxisAngleRot[0]); const FLT x80 = (-1 * x79 * x36) + (-1 * x79 * x34) + (x61 * x20) + (-1 * x79 * x35) + (x77 * x20) + (-1 * x78 * x20); const FLT x81 = (x2 * x43) + (x80 * x41) + (x79 * x42); const FLT x82 = (x81 * x47) + (-1 * x79 * x29) + (-1 * x51) + (-1 * x80 * x39); const FLT x83 = (-1 * x79 * x52) + (x81 * x54) + x32 + (-1 * x80 * x55); const FLT x84 = 2 * x30; const FLT x85 = (-1 * x80 * x60) + (x81 * x62) + x84 + (-1 * x79 * x63); const FLT x86 = (x85 * x66) + (x82 * x50) + (x83 * x58); const FLT x87 = x74 * x65; const FLT x88 = x4 * (*err).Pose.AxisAngleRot[2]; const FLT x89 = x8 * (*err).Pose.AxisAngleRot[0]; const FLT x90 = (-1 * x89) + x88 + x45 + (-1 * x76 * (*err).Pose.AxisAngleRot[1]); const FLT x91 = (-1 * x90 * x34) + (x45 * x20) + (-1 * x90 * x36) + (-1 * x90 * x35) + (x88 * x20) + (-1 * x89 * x20); const FLT x92 = (x0 * x43) + (x91 * x41) + (x90 * x42); const FLT x93 = (-1 * x91 * x39) + (-1 * x90 * x29) + x84 + (x92 * x47); const FLT x94 = (-1 * x52 * x90) + (-1 * x59) + (-1 * x55 * x91) + (x54 * x92); const FLT x95 = (-1 * x60 * x91) + (x62 * x92) + x51 + (-1 * x63 * x90); const FLT x96 = (x66 * x95) + (x50 * x93) + (x58 * x94); const FLT x97 = x4 * (*err).Pose.AxisAngleRot[1]; const FLT x98 = x11 * (*err).Pose.AxisAngleRot[0]; const FLT x99 = x98 + x53 + (-1 * x97) + (-1 * x76 * (*err).Pose.AxisAngleRot[2]); const FLT x100 = (-1 * x99 * x35) + (-1 * x99 * x34) + (x98 * x20) + (x53 * x20) + (-1 * x99 * x36) + (-1 * x97 * x20); const FLT x101 = (x1 * x43) + (x41 * x100) + (x99 * x42); const FLT x102 = (x62 * x101) + (-1 * x60 * x100) + (-1 * x32) + (-1 * x63 * x99); const FLT x103 = (-1 * x39 * x100) + (x47 * x101) + x59 + (-1 * x99 * x29); const FLT x104 = (-1 * x52 * x99) + x84 + (-1 * x55 * x100) + (x54 * x101); const FLT x105 = (x50 * x103) + (x66 * x102) + (x58 * x104); const FLT x106 = x71 * x105; const FLT x107 = x71 * x67; const FLT x108 = x74 * x49; const FLT x109 = x86 * x71; const FLT x110 = x71 * x96; const FLT x111 = x74 * x57; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (x75 * x65) + (-1 * x72 * x67) + (x73 * x64)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x86 * x72) + (x86 * x87) + (x85 * x73)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x87 * x96) + (-1 * x72 * x96) + (x73 * x95)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x87 * x105) + (x73 * x102) + (-1 * x65 * x106)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x49 * x107) + (x73 * x48) + (x75 * x49)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x49 * x109) + (x82 * x73) + (x86 * x108)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x49 * x110) + (x73 * x93) + (x96 * x108)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x49 * x106) + (x73 * x103) + (x108 * x105)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (x75 * x57) + (-1 * x57 * x107) + (x73 * x56)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x86 * x111) + (-1 * x57 * x109) + (x83 * x73)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x96 * x111) + (-1 * x57 * x110) + (x73 * x94)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x105 * x111) + (-1 * x57 * x106) + (x73 * x104)); } // Full version Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] // Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*err).Acc[0], (*err).Acc[1], (*err).Acc[2], (*err).IMUBias.AccBias[0], (*err).IMUBias.AccBias[1], (*err).IMUBias.AccBias[2], (*err).IMUBias.AccScale[0], (*err).IMUBias.AccScale[1], (*err).IMUBias.AccScale[2], (*err).IMUBias.GyroBias[0], (*err).IMUBias.GyroBias[1], (*err).IMUBias.GyroBias[2], (*err).IMUBias.IMUCorrection[0], (*err).IMUBias.IMUCorrection[1], (*err).IMUBias.IMUCorrection[2], (*err).Pose.AxisAngleRot[0], (*err).Pose.AxisAngleRot[1], (*err).Pose.AxisAngleRot[2], (*err).Pose.Pos[0], (*err).Pose.Pos[1], (*err).Pose.Pos[2], (*err).Velocity.AxisAngleRot[0], (*err).Velocity.AxisAngleRot[1], (*err).Velocity.AxisAngleRot[2], (*err).Velocity.Pos[0], (*err).Velocity.Pos[1], (*err).Velocity.Pos[2]] static inline void SurviveObsErrorStateErrorModelFlip_jac_err(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x3 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x4 = (x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[0]); const FLT x5 = x4 * x4; const FLT x6 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = x6 * x6; const FLT x8 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x9 = x8 * x8; const FLT x10 = x3 * x3; const FLT x11 = x10 + x9 + x5 + x7; const FLT x12 = 1. / x11; const FLT x13 = 1e-10 + (x7 * x12) + (x5 * x12) + (x12 * x10); const FLT x14 = sqrt(x13); const FLT x15 = 1. / x14; const FLT x16 = sqrt(x11); const FLT x17 = 1. / x16; const FLT x18 = atan2(x14, x8 * x17); const FLT x19 = x18 * x17; const FLT x20 = x15 * x19; const FLT x21 = 2 * x20; const FLT x22 = (x3 * x21) + (-1 * (*Z).AxisAngleRot[0]); const FLT x23 = 1.0 * x20; const FLT x24 = x23 * (*_x0).Pose.Rot[3]; const FLT x25 = 1.0 * x8; const FLT x26 = 1.0 * x4; const FLT x27 = x26 * (*_x0).Pose.Rot[2]; const FLT x28 = 1.0 * x3; const FLT x29 = x28 * (*_x0).Pose.Rot[0]; const FLT x30 = 1.0 * x6; const FLT x31 = x30 * (*_x0).Pose.Rot[3]; const FLT x32 = x31 + x29 + (-1 * x25 * (*_x0).Pose.Rot[1]) + (-1 * x27); const FLT x33 = x15 * (1. / (x11 * sqrt(x11))); const FLT x34 = x33 * x18; const FLT x35 = x32 * x34; const FLT x36 = 1. / (x11 * x11); const FLT x37 = x32 * x36; const FLT x38 = (x31 * x12) + (-1 * x27 * x12) + (-1 * x5 * x37) + (-1 * x7 * x37) + (x29 * x12) + (-1 * x37 * x10); const FLT x39 = 1.0/2.0 * (1. / x8); const FLT x40 = x39 * x15 * x16; const FLT x41 = x39 * x14 * x17; const FLT x42 = 0.5 * (1. / x9) * x14 * x16; const FLT x43 = 2 * x9 * x33 * (1. / (x13 + (x9 * x12))); const FLT x44 = x43 * ((x42 * (*_x0).Pose.Rot[1]) + (x40 * x38) + (x41 * x32)); const FLT x45 = (1. / (x13 * sqrt(x13))) * x19; const FLT x46 = x6 * x45; const FLT x47 = (-1 * x46 * x38) + (x6 * x44) + x24 + (-1 * x6 * x35); const FLT x48 = (x6 * x21) + (-1 * (*Z).AxisAngleRot[1]); const FLT x49 = 2 * x48; const FLT x50 = x23 * (*_x0).Pose.Rot[2]; const FLT x51 = x4 * x45; const FLT x52 = (-1 * x51 * x38) + (x4 * x44) + (-1 * x50) + (-1 * x4 * x35); const FLT x53 = (x4 * x21) + (-1 * (*Z).AxisAngleRot[2]); const FLT x54 = 2 * x53; const FLT x55 = 1.0 * (*_x0).Pose.Rot[0]; const FLT x56 = x55 * x20; const FLT x57 = x3 * x45; const FLT x58 = (-1 * x3 * x35) + x56 + (-1 * x57 * x38) + (x3 * x44); const FLT x59 = 2 * x22; const FLT x60 = (x58 * x59) + (x47 * x49) + (x54 * x52); const FLT x61 = 1e-10 + (x48 * x48) + (x53 * x53) + (x22 * x22); const FLT x62 = sqrt(x61); const FLT x63 = -6.28318530717959 + x62; const FLT x64 = 1.0/2.0 * x63 * (1. / (x61 * sqrt(x61))); const FLT x65 = x60 * x64; const FLT x66 = x63 * (1. / x62); const FLT x67 = 1.0/2.0 * (1. / x61); const FLT x68 = x60 * x67; const FLT x69 = x23 * (*_x0).Pose.Rot[1]; const FLT x70 = x28 * (*_x0).Pose.Rot[3]; const FLT x71 = x26 * (*_x0).Pose.Rot[1]; const FLT x72 = x6 * x55; const FLT x73 = x72 + (-1 * x25 * (*_x0).Pose.Rot[2]) + (-1 * x70) + x71; const FLT x74 = x73 * x34; const FLT x75 = x73 * x36; const FLT x76 = (x72 * x12) + (-1 * x5 * x75) + (-1 * x7 * x75) + (-1 * x70 * x12) + (-1 * x75 * x10) + (x71 * x12); const FLT x77 = x43 * ((x42 * (*_x0).Pose.Rot[2]) + (x76 * x40) + (x73 * x41)); const FLT x78 = (x4 * x77) + x69 + (-1 * x76 * x51) + (-1 * x4 * x74); const FLT x79 = (-1 * x76 * x46) + (-1 * x6 * x74) + (x6 * x77) + x56; const FLT x80 = (-1 * x3 * x74) + (-1 * x76 * x57) + (-1 * x24) + (x3 * x77); const FLT x81 = (x80 * x59) + (x78 * x54) + (x79 * x49); const FLT x82 = x81 * x64; const FLT x83 = x81 * x67; const FLT x84 = x28 * (*_x0).Pose.Rot[2]; const FLT x85 = x4 * x55; const FLT x86 = x30 * (*_x0).Pose.Rot[1]; const FLT x87 = (-1 * x86) + x85 + (-1 * x25 * (*_x0).Pose.Rot[3]) + x84; const FLT x88 = x87 * x36; const FLT x89 = (-1 * x88 * x10) + (-1 * x5 * x88) + (-1 * x7 * x88) + (-1 * x86 * x12) + (x84 * x12) + (x85 * x12); const FLT x90 = x43 * ((x89 * x40) + (x42 * (*_x0).Pose.Rot[3]) + (x87 * x41)); const FLT x91 = x87 * x34; const FLT x92 = (-1 * x89 * x46) + (-1 * x6 * x91) + (-1 * x69) + (x6 * x90); const FLT x93 = (-1 * x89 * x51) + (-1 * x4 * x91) + x56 + (x4 * x90); const FLT x94 = (-1 * x89 * x57) + (x3 * x90) + x50 + (-1 * x3 * x91); const FLT x95 = (x59 * x94) + (x92 * x49) + (x54 * x93); const FLT x96 = x64 * x95; const FLT x97 = x67 * x95; const FLT x98 = x64 * x48; cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x68 * x22) + (-1 * x65 * x22) + (x66 * x58)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (x83 * x22) + (-1 * x82 * x22) + (x80 * x66)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (x97 * x22) + (-1 * x96 * x22) + (x66 * x94)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x60 * x98) + (x66 * x47) + (x68 * x48)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x81 * x98) + (x79 * x66) + (x83 * x48)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x96 * x48) + (x66 * x92) + (x97 * x48)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x68 * x53) + (-1 * x65 * x53) + (x66 * x52)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (x83 * x53) + (-1 * x82 * x53) + (x78 * x66)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (x53 * x97) + (-1 * x53 * x96) + (x66 * x93)); } // Full version Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*err).Acc[0], (*err).Acc[1], (*err).Acc[2], (*err).IMUBias.AccBias[0], (*err).IMUBias.AccBias[1], (*err).IMUBias.AccBias[2], (*err).IMUBias.AccScale[0], (*err).IMUBias.AccScale[1], (*err).IMUBias.AccScale[2], (*err).IMUBias.GyroBias[0], (*err).IMUBias.GyroBias[1], (*err).IMUBias.GyroBias[2], (*err).IMUBias.IMUCorrection[0], (*err).IMUBias.IMUCorrection[1], (*err).IMUBias.IMUCorrection[2], (*err).Pose.AxisAngleRot[0], (*err).Pose.AxisAngleRot[1], (*err).Pose.AxisAngleRot[2], (*err).Pose.Pos[0], (*err).Pose.Pos[1], (*err).Pose.Pos[2], (*err).Velocity.AxisAngleRot[0], (*err).Velocity.AxisAngleRot[1], (*err).Velocity.AxisAngleRot[2], (*err).Velocity.Pos[0], (*err).Velocity.Pos[1], (*err).Velocity.Pos[2]] // Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline void SurviveObsErrorStateErrorModelFlip_jac_Z(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* err, const SurviveAxisAnglePose* Z) { const FLT x0 = 0.5 * (*err).Pose.AxisAngleRot[1]; const FLT x1 = 0.5 * (*err).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*err).Pose.AxisAngleRot[0]; const FLT x3 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x0 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x4 = (x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[3] + (x1 * (*_x0).Pose.Rot[0]); const FLT x5 = x4 * x4; const FLT x6 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x7 = x6 * x6; const FLT x8 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]) + (-1 * x0 * (*_x0).Pose.Rot[2]); const FLT x9 = x3 * x3; const FLT x10 = x9 + (x8 * x8) + x5 + x7; const FLT x11 = 1. / sqrt(x10); const FLT x12 = 1. / x10; const FLT x13 = sqrt(1e-10 + (x7 * x12) + (x5 * x12) + (x9 * x12)); const FLT x14 = 2 * (1. / x13) * x11 * atan2(x13, x8 * x11); const FLT x15 = (x3 * x14) + (-1 * (*Z).AxisAngleRot[0]); const FLT x16 = x15 * x15; const FLT x17 = (x4 * x14) + (-1 * (*Z).AxisAngleRot[2]); const FLT x18 = x17 * x17; const FLT x19 = (x6 * x14) + (-1 * (*Z).AxisAngleRot[1]); const FLT x20 = x19 * x19; const FLT x21 = 1e-10 + x18 + x20 + x16; const FLT x22 = sqrt(x21); const FLT x23 = -6.28318530717959 + x22; const FLT x24 = x23 * (1. / (x21 * sqrt(x21))); const FLT x25 = -1 * (1. / x22) * x23; const FLT x26 = 1. / x21; const FLT x27 = x24 * x19; const FLT x28 = x26 * x19; const FLT x29 = (-1 * x28 * x15) + (x27 * x15); const FLT x30 = x15 * x17; const FLT x31 = (-1 * x30 * x26) + (x30 * x24); const FLT x32 = (-1 * x28 * x17) + (x27 * x17); cnSetZero(Hx); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[0])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[1])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, Pos[2])/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (-1 * x26 * x16) + (x24 * x16) + x25); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), x29); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), x31); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), x29); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (x24 * x20) + x25 + (-1 * x20 * x26)); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), x32); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), x31); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), x32); cnMatrixOptionalSet(Hx, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (-1 * x26 * x18) + (x24 * x18) + x25); } // Full version Jacobian of SurviveObsErrorStateErrorModelFlip wrt [(*Z).AxisAngleRot[0], (*Z).AxisAngleRot[1], (*Z).AxisAngleRot[2], (*Z).Pos[0], (*Z).Pos[1], (*Z).Pos[2]] static inline FLT SurviveKalmanModel_LightMeas_x_gen1(const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x11 * x10; const FLT x15 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[1]) + (x13 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x14 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]) + (-1 * x13 * (*_x0).Pose.Rot[1]); const FLT x20 = (-1 * x19 * sensor_pt[1]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[0]); const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[1]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (x13 * (*_x0).Pose.Rot[3]); const FLT x22 = (x19 * sensor_pt[0]) + (-1 * x21 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x23 = (2 * ((x22 * x21) + (-1 * x20 * x17))) + sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x24 = (-1 * x17 * sensor_pt[0]) + (x18 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x25 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x26 = (*_x0).Pose.Pos[0] + (2 * ((x24 * x17) + (-1 * x22 * x19))) + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x27 = (-1 * x26 * (*lh_p).Rot[2]) + (x23 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[1]); const FLT x28 = (-1 * x25 * (*lh_p).Rot[3]) + (x23 * (*lh_p).Rot[2]) + (x26 * (*lh_p).Rot[0]); const FLT x29 = x25 + (*lh_p).Pos[1] + (2 * ((x28 * (*lh_p).Rot[3]) + (-1 * x27 * (*lh_p).Rot[1]))); const FLT x30 = (-1 * x23 * (*lh_p).Rot[1]) + (x25 * (*lh_p).Rot[0]) + (x26 * (*lh_p).Rot[3]); const FLT x31 = x23 + (2 * ((x30 * (*lh_p).Rot[1]) + (-1 * x28 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x32 = x26 + (2 * ((x27 * (*lh_p).Rot[2]) + (-1 * x30 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x33 = -1 * x31; const FLT x34 = (-1 * (*bsc0).phase) + (-1 * asin((1. / sqrt((x32 * x32) + (x31 * x31))) * x29 * (*bsc0).tilt)) + (-1 * atan2(x32, x33)); return x34 + (-1 * cos(1.5707963267949 + x34 + (*bsc0).gibpha) * (*bsc0).gibmag) + ((atan2(x29, x33) * atan2(x29, x33)) * (*bsc0).curve); } // Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = x0 * (*lh_p).Rot[1]; const FLT x2 = x1 * (*lh_p).Rot[2]; const FLT x3 = x0 * (*lh_p).Rot[3]; const FLT x4 = x3 * (*lh_p).Rot[0]; const FLT x5 = x4 + x2; const FLT x6 = 1.0/2.0 * x0; const FLT x7 = dt * dt; const FLT x8 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x9 = x8 * x7; const FLT x10 = (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x11 = x7 * x10; const FLT x12 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x13 = x7 * x12; const FLT x14 = 1e-10 + x9 + x11 + x13; const FLT x15 = sqrt(x14); const FLT x16 = 0.5 * x15; const FLT x17 = sin(x16); const FLT x18 = x17 * x17; const FLT x19 = 1. / x14; const FLT x20 = x19 * x18; const FLT x21 = cos(x16); const FLT x22 = (x20 * x13) + (x20 * x11) + (x9 * x20) + (x21 * x21); const FLT x23 = 1. / sqrt(x22); const FLT x24 = (1. / x15) * x17; const FLT x25 = dt * x24; const FLT x26 = x25 * x23; const FLT x27 = x26 * (*_x0).Pose.Rot[0]; const FLT x28 = x23 * x21; const FLT x29 = x28 * (*_x0).Pose.Rot[2]; const FLT x30 = x26 * (*_x0).Pose.Rot[1]; const FLT x31 = x23 * (*_x0).Pose.Rot[3]; const FLT x32 = x31 * x25; const FLT x33 = (-1 * x32 * (*_x0).Velocity.AxisAngleRot[0]) + (x30 * (*_x0).Velocity.AxisAngleRot[2]) + (x27 * (*_x0).Velocity.AxisAngleRot[1]) + x29; const FLT x34 = x26 * (*_x0).Pose.Rot[2]; const FLT x35 = x28 * (*_x0).Pose.Rot[0]; const FLT x36 = (-1 * x30 * (*_x0).Velocity.AxisAngleRot[0]) + x35 + (-1 * x32 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x34 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x37 = x28 * (*_x0).Pose.Rot[3]; const FLT x38 = (x34 * (*_x0).Velocity.AxisAngleRot[0]) + x37 + (x27 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x30 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x39 = (-1 * x38 * sensor_pt[1]) + (x33 * sensor_pt[2]) + (x36 * sensor_pt[0]); const FLT x40 = x28 * (*_x0).Pose.Rot[1]; const FLT x41 = (x27 * (*_x0).Velocity.AxisAngleRot[0]) + x40 + (-1 * x34 * (*_x0).Velocity.AxisAngleRot[2]) + (x32 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x42 = (-1 * x41 * sensor_pt[2]) + (x38 * sensor_pt[0]) + (x36 * sensor_pt[1]); const FLT x43 = (2 * ((x41 * x42) + (-1 * x33 * x39))) + sensor_pt[2] + (x6 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x44 = (-1 * x33 * sensor_pt[0]) + (x36 * sensor_pt[2]) + (x41 * sensor_pt[1]); const FLT x45 = (2 * ((x44 * x33) + (-1 * x42 * x38))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x6 * (*_x0).Acc[0]); const FLT x46 = sensor_pt[1] + (2 * ((x38 * x39) + (-1 * x41 * x44))) + (*_x0).Pose.Pos[1] + (dt * (*_x0).Velocity.Pos[1]) + (x6 * (*_x0).Acc[1]); const FLT x47 = (-1 * x46 * (*lh_p).Rot[3]) + (x43 * (*lh_p).Rot[2]) + (x45 * (*lh_p).Rot[0]); const FLT x48 = (x46 * (*lh_p).Rot[0]) + (-1 * x43 * (*lh_p).Rot[1]) + (x45 * (*lh_p).Rot[3]); const FLT x49 = (2 * ((x48 * (*lh_p).Rot[1]) + (-1 * x47 * (*lh_p).Rot[2]))) + x43 + (*lh_p).Pos[2]; const FLT x50 = 1. / x49; const FLT x51 = x0 * (*lh_p).Rot[2] * (*lh_p).Rot[0]; const FLT x52 = x1 * (*lh_p).Rot[3]; const FLT x53 = x52 + (-1 * x51); const FLT x54 = x49 * x49; const FLT x55 = 1. / x54; const FLT x56 = (x43 * (*lh_p).Rot[0]) + (-1 * x45 * (*lh_p).Rot[2]) + (x46 * (*lh_p).Rot[1]); const FLT x57 = (*lh_p).Pos[1] + x46 + (2 * ((x47 * (*lh_p).Rot[3]) + (-1 * x56 * (*lh_p).Rot[1]))); const FLT x58 = x57 * x55; const FLT x59 = x57 * x57; const FLT x60 = -1 * x49; const FLT x61 = 2 * (1. / (x54 + x59)) * x54 * atan2(x57, x60) * (*bsc0).curve; const FLT x62 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x63 = -1 * x0 * x62; const FLT x64 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x65 = -1 * x0 * x64; const FLT x66 = x65 + x6 + x63; const FLT x67 = x45 + (2 * ((x56 * (*lh_p).Rot[2]) + (-1 * x48 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x68 = x67 * x55; const FLT x69 = x54 + (x67 * x67); const FLT x70 = 1. / x69; const FLT x71 = x70 * x54; const FLT x72 = 1. / sqrt(1 + (-1 * x70 * x59 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x73 = 2 * x67; const FLT x74 = 2 * x49; const FLT x75 = 1.0/2.0 * (1. / (x69 * sqrt(x69))) * x57 * (*bsc0).tilt; const FLT x76 = (1. / sqrt(x69)) * (*bsc0).tilt; const FLT x77 = (-1 * x72 * ((x5 * x76) + (-1 * ((x74 * x53) + (x73 * x66)) * x75))) + (-1 * ((x68 * x53) + (-1 * x66 * x50)) * x71); const FLT x78 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * asin(x76 * x57)) + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * atan2(x67, x60))); const FLT x79 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x80 = (-1 * x0 * x79) + x6; const FLT x81 = x80 + x65; const FLT x82 = x3 * (*lh_p).Rot[2]; const FLT x83 = x1 * (*lh_p).Rot[0]; const FLT x84 = x83 + x82; const FLT x85 = x2 + (-1 * x4); const FLT x86 = (-1 * x72 * ((x81 * x76) + (-1 * ((x84 * x74) + (x85 * x73)) * x75))) + (-1 * ((x84 * x68) + (-1 * x85 * x50)) * x71); const FLT x87 = x82 + (-1 * x83); const FLT x88 = x80 + x63; const FLT x89 = x51 + x52; const FLT x90 = (-1 * x72 * ((x87 * x76) + (-1 * ((x88 * x74) + (x89 * x73)) * x75))) + (-1 * ((x88 * x68) + (-1 * x89 * x50)) * x71); const FLT x91 = 2 * (*lh_p).Rot[1]; const FLT x92 = x91 * (*lh_p).Rot[2]; const FLT x93 = 2 * (*lh_p).Rot[3]; const FLT x94 = x93 * (*lh_p).Rot[0]; const FLT x95 = x94 + x92; const FLT x96 = 2 * (*lh_p).Rot[2]; const FLT x97 = x96 * (*lh_p).Rot[0]; const FLT x98 = x91 * (*lh_p).Rot[3]; const FLT x99 = x98 + (-1 * x97); const FLT x100 = 2 * x62; const FLT x101 = -1 * x100; const FLT x102 = 2 * x64; const FLT x103 = 1 + (-1 * x102); const FLT x104 = x103 + x101; const FLT x105 = (-1 * x72 * ((x76 * x95) + (-1 * x75 * ((x74 * x99) + (x73 * x104))))) + (-1 * x71 * ((x68 * x99) + (-1 * x50 * x104))); const FLT x106 = 2 * x79; const FLT x107 = -1 * x106; const FLT x108 = x103 + x107; const FLT x109 = x93 * (*lh_p).Rot[2]; const FLT x110 = x91 * (*lh_p).Rot[0]; const FLT x111 = x110 + x109; const FLT x112 = x92 + (-1 * x94); const FLT x113 = (-1 * x72 * ((x76 * x108) + (-1 * ((x74 * x111) + (x73 * x112)) * x75))) + (-1 * ((x68 * x111) + (-1 * x50 * x112)) * x71); const FLT x114 = x109 + (-1 * x110); const FLT x115 = 1 + x107 + x101; const FLT x116 = x97 + x98; const FLT x117 = (-1 * x72 * ((x76 * x114) + (-1 * ((x74 * x115) + (x73 * x116)) * x75))) + (-1 * ((x68 * x115) + (-1 * x50 * x116)) * x71); const FLT x118 = x26 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x119 = -1 * x118 * sensor_pt[2]; const FLT x120 = x26 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x121 = x120 * sensor_pt[0]; const FLT x122 = x28 * sensor_pt[1]; const FLT x123 = x122 + x119 + x121; const FLT x124 = 2 * x38; const FLT x125 = 2 * x42; const FLT x126 = -1 * x120 * x125; const FLT x127 = x118 * sensor_pt[1]; const FLT x128 = x26 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x129 = -1 * x128 * sensor_pt[0]; const FLT x130 = x28 * sensor_pt[2]; const FLT x131 = x130 + x129; const FLT x132 = x131 + x127; const FLT x133 = 2 * x33; const FLT x134 = 2 * x44; const FLT x135 = x128 * x134; const FLT x136 = x135 + (-1 * x124 * x123) + (x133 * x132) + x126; const FLT x137 = x128 * sensor_pt[2]; const FLT x138 = x28 * sensor_pt[0]; const FLT x139 = -1 * x120 * sensor_pt[1]; const FLT x140 = x139 + x138; const FLT x141 = x140 + x137; const FLT x142 = x118 * x125; const FLT x143 = 2 * x39; const FLT x144 = -1 * x128 * x143; const FLT x145 = 2 * x41; const FLT x146 = x144 + (-1 * x133 * x141) + (x123 * x145) + x142; const FLT x147 = x120 * x143; const FLT x148 = -1 * x118 * x134; const FLT x149 = x148 + x147 + (-1 * x132 * x145) + (x124 * x141); const FLT x150 = (x149 * (*lh_p).Rot[1]) + (-1 * x136 * (*lh_p).Rot[2]) + (x146 * (*lh_p).Rot[0]); const FLT x151 = (-1 * x149 * (*lh_p).Rot[3]) + (x136 * (*lh_p).Rot[0]) + (x146 * (*lh_p).Rot[2]); const FLT x152 = x149 + (-1 * x91 * x150) + (x93 * x151); const FLT x153 = (x136 * (*lh_p).Rot[3]) + (-1 * x146 * (*lh_p).Rot[1]) + (x149 * (*lh_p).Rot[0]); const FLT x154 = x146 + (-1 * x96 * x151) + (x91 * x153); const FLT x155 = x136 + (-1 * x93 * x153) + (x96 * x150); const FLT x156 = (-1 * x72 * ((x76 * x152) + (-1 * ((x74 * x154) + (x73 * x155)) * x75))) + (-1 * ((x68 * x154) + (-1 * x50 * x155)) * x71); const FLT x157 = x120 * x134; const FLT x158 = -1 * x127; const FLT x159 = x158 + (-1 * x130) + x129; const FLT x160 = x128 * x125; const FLT x161 = (-1 * x121) + x119; const FLT x162 = x161 + x122; const FLT x163 = (x162 * x133) + x160 + x157 + (-1 * x124 * x159); const FLT x164 = x128 * sensor_pt[1]; const FLT x165 = x120 * sensor_pt[2]; const FLT x166 = x118 * sensor_pt[0]; const FLT x167 = (-1 * x166) + x164 + x165; const FLT x168 = x28 * x125; const FLT x169 = (-1 * x147) + x168 + (-1 * x167 * x133) + (x145 * x159); const FLT x170 = x28 * x134; const FLT x171 = x144 + (-1 * x170) + (-1 * x162 * x145) + (x124 * x167); const FLT x172 = (x171 * (*lh_p).Rot[1]) + (-1 * x163 * (*lh_p).Rot[2]) + (x169 * (*lh_p).Rot[0]); const FLT x173 = 2 * ((-1 * x171 * (*lh_p).Rot[3]) + (x163 * (*lh_p).Rot[0]) + (x169 * (*lh_p).Rot[2])); const FLT x174 = x171 + (-1 * x91 * x172) + (x173 * (*lh_p).Rot[3]); const FLT x175 = (x163 * (*lh_p).Rot[3]) + (-1 * x169 * (*lh_p).Rot[1]) + (x171 * (*lh_p).Rot[0]); const FLT x176 = x169 + (-1 * x173 * (*lh_p).Rot[2]) + (x91 * x175); const FLT x177 = x163 + (-1 * x93 * x175) + (x96 * x172); const FLT x178 = (-1 * x72 * ((x76 * x174) + (-1 * ((x74 * x176) + (x73 * x177)) * x75))) + (-1 * ((x68 * x176) + (-1 * x50 * x177)) * x71); const FLT x179 = -1 * x137; const FLT x180 = x139 + (-1 * x138) + x179; const FLT x181 = x131 + x158; const FLT x182 = x118 * x143; const FLT x183 = x182 + (x124 * x181) + (-1 * x180 * x145) + x157; const FLT x184 = x28 * x143; const FLT x185 = 2 * ((-1 * x164) + x165 + x166); const FLT x186 = x126 + (x41 * x185) + (-1 * x181 * x133) + (-1 * x184); const FLT x187 = (x180 * x133) + x170 + (-1 * x142) + (-1 * x38 * x185); const FLT x188 = (x187 * (*lh_p).Rot[3]) + (x183 * (*lh_p).Rot[0]) + (-1 * x186 * (*lh_p).Rot[1]); const FLT x189 = (x183 * (*lh_p).Rot[1]) + (x186 * (*lh_p).Rot[0]) + (-1 * x187 * (*lh_p).Rot[2]); const FLT x190 = (-1 * x93 * x188) + x187 + (x96 * x189); const FLT x191 = (x187 * (*lh_p).Rot[0]) + (-1 * x183 * (*lh_p).Rot[3]) + (x186 * (*lh_p).Rot[2]); const FLT x192 = x186 + (-1 * x96 * x191) + (x91 * x188); const FLT x193 = (-1 * x91 * x189) + x183 + (x93 * x191); const FLT x194 = (-1 * x72 * ((x76 * x193) + (-1 * ((x74 * x192) + (x73 * x190)) * x75))) + (-1 * ((x68 * x192) + (-1 * x50 * x190)) * x71); const FLT x195 = x161 + (-1 * x122); const FLT x196 = x140 + x179; const FLT x197 = x160 + (x196 * x145) + (-1 * x195 * x133) + x182; const FLT x198 = 2 * (x164 + x166 + (-1 * x165)); const FLT x199 = (-1 * x135) + x184 + (-1 * x41 * x198) + (x124 * x195); const FLT x200 = (-1 * x168) + x148 + (x33 * x198) + (-1 * x124 * x196); const FLT x201 = (-1 * x197 * (*lh_p).Rot[1]) + (x200 * (*lh_p).Rot[3]) + (x199 * (*lh_p).Rot[0]); const FLT x202 = (x199 * (*lh_p).Rot[1]) + (-1 * x200 * (*lh_p).Rot[2]) + (x197 * (*lh_p).Rot[0]); const FLT x203 = (-1 * x93 * x201) + x200 + (x96 * x202); const FLT x204 = (x200 * (*lh_p).Rot[0]) + (-1 * x199 * (*lh_p).Rot[3]) + (x197 * (*lh_p).Rot[2]); const FLT x205 = x197 + (-1 * x96 * x204) + (x91 * x201); const FLT x206 = x199 + (x93 * x204) + (-1 * x91 * x202); const FLT x207 = (-1 * x72 * ((x76 * x206) + (-1 * ((x74 * x205) + (x73 * x203)) * x75))) + (-1 * ((x68 * x205) + (-1 * x50 * x203)) * x71); const FLT x208 = -1 * x32; const FLT x209 = x23 * (*_x0).Pose.Rot[2]; const FLT x210 = x7 * x24; const FLT x211 = 0.5 * x210; const FLT x212 = x211 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x213 = 0.5 * x19; const FLT x214 = dt * dt * dt; const FLT x215 = x12 * x214; const FLT x216 = x213 * x215; const FLT x217 = 1.0/2.0 * (1. / (x22 * sqrt(x22))); const FLT x218 = x25 * x217; const FLT x219 = x218 * (*_x0).Pose.Rot[3]; const FLT x220 = dt * dt * dt * dt; const FLT x221 = x220 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x222 = (1. / (x14 * x14)) * x18; const FLT x223 = 2 * x222; const FLT x224 = 1.0 * x21; const FLT x225 = (1. / (x14 * sqrt(x14))) * x17; const FLT x226 = x224 * x225; const FLT x227 = x220 * x226; const FLT x228 = 2 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x229 = x222 * x228; const FLT x230 = x8 * x220; const FLT x231 = x210 * x224; const FLT x232 = x7 * x20; const FLT x233 = x10 * x220; const FLT x234 = x233 * x226; const FLT x235 = (-1 * x233 * x229) + (x234 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x223 * x221) + (x232 * x228) + (x8 * x227 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x230 * x229) + (x221 * x226) + (-1 * x231 * (*_x0).Velocity.AxisAngleRot[0]); const FLT x236 = x235 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x237 = x21 * x217; const FLT x238 = x237 * x235; const FLT x239 = x235 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x240 = x218 * x239; const FLT x241 = x23 * (*_x0).Pose.Rot[1]; const FLT x242 = x214 * x225; const FLT x243 = x242 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x244 = x243 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x245 = x241 * x244; const FLT x246 = x214 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x247 = x213 * x246 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x248 = x40 * x247; const FLT x249 = x218 * (*_x0).Pose.Rot[1]; const FLT x250 = x235 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x251 = x12 * x242; const FLT x252 = x214 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x253 = x35 * x213; const FLT x254 = x253 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x255 = x23 * (*_x0).Pose.Rot[0]; const FLT x256 = x255 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x257 = x242 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x258 = (-1 * x256 * x257) + (x252 * x254); const FLT x259 = (x31 * x251) + x258 + (-1 * x250 * x249) + x248 + (-1 * x245) + (-1 * x212 * x209) + (-1 * x240 * (*_x0).Pose.Rot[0]) + (-1 * x37 * x216) + (x219 * x236) + x208 + (-1 * x238 * (*_x0).Pose.Rot[2]); const FLT x260 = x213 * x252; const FLT x261 = x260 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x262 = x37 * x261; const FLT x263 = x257 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x264 = x31 * x263; const FLT x265 = x29 * x247; const FLT x266 = x209 * x244; const FLT x267 = x218 * (*_x0).Pose.Rot[2]; const FLT x268 = x218 * (*_x0).Pose.Rot[0]; const FLT x269 = (-1 * x236 * x268) + x27 + (-1 * x264) + (x215 * x253) + (-1 * x238 * (*_x0).Pose.Rot[1]) + x266 + (-1 * x219 * x239) + x262 + (-1 * x212 * x241) + (-1 * x265) + (x267 * x250) + (-1 * x251 * x255); const FLT x270 = x31 * x211; const FLT x271 = x255 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x272 = (x254 * x246) + (-1 * x271 * x243); const FLT x273 = (-1 * x40 * x261) + (x263 * x241); const FLT x274 = x272 + x273 + (-1 * x236 * x267) + (x29 * x216) + (-1 * x270 * (*_x0).Velocity.AxisAngleRot[0]) + x34 + (-1 * x209 * x251) + (-1 * x238 * (*_x0).Pose.Rot[3]) + (-1 * x268 * x250) + (x239 * x249); const FLT x275 = -1 * x30; const FLT x276 = x29 * x260; const FLT x277 = x276 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x278 = x209 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x279 = x278 * x257; const FLT x280 = (x31 * x244) + (-1 * x37 * x247); const FLT x281 = x280 + (x251 * x241) + (-1 * x40 * x216) + (-1 * x211 * x271) + (x219 * x250) + (-1 * x277) + (x240 * (*_x0).Pose.Rot[2]) + x275 + (-1 * x238 * (*_x0).Pose.Rot[0]) + (x236 * x249) + x279; const FLT x282 = (x281 * sensor_pt[1]) + (-1 * x269 * sensor_pt[2]) + (x274 * sensor_pt[0]); const FLT x283 = (x269 * sensor_pt[1]) + (-1 * x259 * sensor_pt[0]) + (x281 * sensor_pt[2]); const FLT x284 = (x283 * x133) + (-1 * x274 * x125) + (x259 * x134) + (-1 * x282 * x124); const FLT x285 = (-1 * x274 * sensor_pt[1]) + (x281 * sensor_pt[0]) + (x259 * sensor_pt[2]); const FLT x286 = 2 * x269; const FLT x287 = (x282 * x145) + (-1 * x285 * x133) + (x42 * x286) + (-1 * x259 * x143); const FLT x288 = (x285 * x124) + (-1 * x44 * x286) + (-1 * x283 * x145) + (x274 * x143); const FLT x289 = (x288 * (*lh_p).Rot[1]) + (-1 * x284 * (*lh_p).Rot[2]) + (x287 * (*lh_p).Rot[0]); const FLT x290 = (x284 * (*lh_p).Rot[0]) + (x287 * (*lh_p).Rot[2]) + (-1 * x288 * (*lh_p).Rot[3]); const FLT x291 = x288 + (-1 * x91 * x289) + (x93 * x290); const FLT x292 = (x284 * (*lh_p).Rot[3]) + (-1 * x287 * (*lh_p).Rot[1]) + (x288 * (*lh_p).Rot[0]); const FLT x293 = (-1 * x96 * x290) + x287 + (x91 * x292); const FLT x294 = x284 + (-1 * x93 * x292) + (x96 * x289); const FLT x295 = (-1 * x72 * ((x76 * x291) + (-1 * ((x74 * x293) + (x73 * x294)) * x75))) + (-1 * ((x68 * x293) + (-1 * x50 * x294)) * x71); const FLT x296 = x220 * x223; const FLT x297 = x296 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x298 = 2 * x232; const FLT x299 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x300 = (x299 * x227) + (-1 * x10 * x297) + (-1 * x231 * (*_x0).Velocity.AxisAngleRot[1]) + (x234 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x12 * x297) + (x298 * (*_x0).Velocity.AxisAngleRot[1]) + (x12 * x227 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x296 * x299); const FLT x301 = x300 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x302 = x218 * x301; const FLT x303 = x8 * x242; const FLT x304 = x300 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x305 = x300 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x306 = x8 * x214; const FLT x307 = x213 * x306; const FLT x308 = x237 * x300; const FLT x309 = x211 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x310 = (x278 * x243) + (-1 * x276 * (*_x0).Velocity.AxisAngleRot[2]); const FLT x311 = (-1 * x241 * x309) + x32 + (-1 * x31 * x303) + (x37 * x307) + (-1 * x302 * (*_x0).Pose.Rot[0]) + (-1 * x308 * (*_x0).Pose.Rot[1]) + (-1 * x219 * x305) + x258 + x310 + (x267 * x304); const FLT x312 = (x252 * x253 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x256 * x243); const FLT x313 = x277 + (-1 * x270 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x279) + (-1 * x268 * x304) + (x249 * x305) + x312 + (-1 * x302 * (*_x0).Pose.Rot[2]) + x275 + (-1 * x40 * x307) + (x241 * x303) + (-1 * x308 * (*_x0).Pose.Rot[3]); const FLT x314 = -1 * x34; const FLT x315 = x260 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x316 = x37 * x315; const FLT x317 = x243 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x318 = x31 * x317; const FLT x319 = x273 + x318 + (-1 * x308 * (*_x0).Pose.Rot[0]) + (-1 * x211 * x256) + (x219 * x304) + (x267 * x305) + x314 + (-1 * x316) + (x249 * x301) + (x209 * x303) + (-1 * x29 * x307); const FLT x320 = (x319 * sensor_pt[1]) + (-1 * x311 * sensor_pt[2]) + (x313 * sensor_pt[0]); const FLT x321 = x241 * x317; const FLT x322 = x40 * x315; const FLT x323 = x264 + x27 + (-1 * x308 * (*_x0).Pose.Rot[2]) + (x302 * (*_x0).Pose.Rot[3]) + (-1 * x262) + (-1 * x255 * x303) + (-1 * x249 * x304) + (x253 * x306) + (-1 * x268 * x305) + (-1 * x209 * x309) + (-1 * x321) + x322; const FLT x324 = (x311 * sensor_pt[1]) + (-1 * x323 * sensor_pt[0]) + (x319 * sensor_pt[2]); const FLT x325 = (-1 * x313 * x125) + (x323 * x134) + (-1 * x320 * x124) + (x324 * x133); const FLT x326 = (-1 * x313 * sensor_pt[1]) + (x319 * sensor_pt[0]) + (x323 * sensor_pt[2]); const FLT x327 = (x320 * x145) + (-1 * x323 * x143) + (-1 * x326 * x133) + (x311 * x125); const FLT x328 = (x313 * x143) + (x326 * x124) + (-1 * x324 * x145) + (-1 * x311 * x134); const FLT x329 = (x328 * (*lh_p).Rot[1]) + (-1 * x325 * (*lh_p).Rot[2]) + (x327 * (*lh_p).Rot[0]); const FLT x330 = (-1 * x328 * (*lh_p).Rot[3]) + (x325 * (*lh_p).Rot[0]) + (x327 * (*lh_p).Rot[2]); const FLT x331 = x328 + (-1 * x91 * x329) + (x93 * x330); const FLT x332 = (x325 * (*lh_p).Rot[3]) + (-1 * x327 * (*lh_p).Rot[1]) + (x328 * (*lh_p).Rot[0]); const FLT x333 = x327 + (-1 * x96 * x330) + (x91 * x332); const FLT x334 = (-1 * x93 * x332) + x325 + (x96 * x329); const FLT x335 = (-1 * x72 * ((x76 * x331) + (-1 * ((x74 * x333) + (x73 * x334)) * x75))) + (-1 * ((x68 * x333) + (-1 * x50 * x334)) * x71); const FLT x336 = x296 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x337 = x224 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x338 = x225 * x337; const FLT x339 = (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x340 = (-1 * x296 * x339) + (x298 * (*_x0).Velocity.AxisAngleRot[2]) + (x12 * x220 * x338) + (x230 * x338) + (-1 * x8 * x336) + (-1 * x12 * x336) + (x227 * x339) + (-1 * x210 * x337); const FLT x341 = x340 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x342 = x10 * x214; const FLT x343 = x225 * x342; const FLT x344 = x23 * x343; const FLT x345 = x340 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x346 = x340 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x347 = x237 * x340; const FLT x348 = x211 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x349 = x213 * x342; const FLT x350 = x272 + (-1 * x29 * x349) + (-1 * x241 * x348) + (-1 * x268 * x346) + (-1 * x219 * x345) + (x267 * x341) + (-1 * x318) + (-1 * x347 * (*_x0).Pose.Rot[1]) + x316 + x314 + (x344 * (*_x0).Pose.Rot[2]); const FLT x351 = x267 * x340; const FLT x352 = (-1 * x266) + x27 + (-1 * x351 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x347 * (*_x0).Pose.Rot[3]) + (-1 * x268 * x341) + (-1 * x322) + x321 + (x253 * x342) + (x249 * x345) + x265 + (-1 * x344 * (*_x0).Pose.Rot[0]) + (-1 * x270 * (*_x0).Velocity.AxisAngleRot[2]); const FLT x353 = (-1 * x255 * x348) + (-1 * x347 * (*_x0).Pose.Rot[0]) + x310 + (x219 * x341) + (-1 * x248) + (-1 * x37 * x349) + (x351 * (*_x0).Velocity.AxisAngleRot[1]) + (x249 * x346) + x245 + x208 + (x31 * x343); const FLT x354 = (x353 * sensor_pt[1]) + (-1 * x350 * sensor_pt[2]) + (x352 * sensor_pt[0]); const FLT x355 = x280 + (-1 * x249 * x341) + (-1 * x241 * x343) + x312 + (-1 * x209 * x348) + (x219 * x346) + (-1 * x268 * x345) + (-1 * x347 * (*_x0).Pose.Rot[2]) + x30 + (x40 * x349); const FLT x356 = (x350 * sensor_pt[1]) + (-1 * x355 * sensor_pt[0]) + (x353 * sensor_pt[2]); const FLT x357 = (x356 * x133) + (-1 * x354 * x124) + (-1 * x352 * x125) + (x355 * x134); const FLT x358 = (x353 * sensor_pt[0]) + (-1 * x352 * sensor_pt[1]) + (x355 * sensor_pt[2]); const FLT x359 = 2 * x350; const FLT x360 = (x42 * x359) + (-1 * x358 * x133) + (-1 * x355 * x143) + (x354 * x145); const FLT x361 = (-1 * x44 * x359) + (x358 * x124) + (-1 * x356 * x145) + (x352 * x143); const FLT x362 = (-1 * x357 * (*lh_p).Rot[2]) + (x361 * (*lh_p).Rot[1]) + (x360 * (*lh_p).Rot[0]); const FLT x363 = (-1 * x361 * (*lh_p).Rot[3]) + (x357 * (*lh_p).Rot[0]) + (x360 * (*lh_p).Rot[2]); const FLT x364 = x361 + (-1 * x91 * x362) + (x93 * x363); const FLT x365 = (x357 * (*lh_p).Rot[3]) + (-1 * x360 * (*lh_p).Rot[1]) + (x361 * (*lh_p).Rot[0]); const FLT x366 = x360 + (x91 * x365) + (-1 * x96 * x363); const FLT x367 = x357 + (-1 * x93 * x365) + (x96 * x362); const FLT x368 = (-1 * x72 * ((x76 * x364) + (-1 * ((x74 * x366) + (x73 * x367)) * x75))) + (-1 * ((x68 * x366) + (-1 * x50 * x367)) * x71); const FLT x369 = dt * x92; const FLT x370 = dt * x94; const FLT x371 = x370 + x369; const FLT x372 = dt * x97; const FLT x373 = dt * x98; const FLT x374 = x373 + (-1 * x372); const FLT x375 = -1 * dt * x102; const FLT x376 = (-1 * dt * x100) + dt; const FLT x377 = x376 + x375; const FLT x378 = (-1 * x72 * ((x76 * x371) + (-1 * ((x74 * x374) + (x73 * x377)) * x75))) + (-1 * ((x68 * x374) + (-1 * x50 * x377)) * x71); const FLT x379 = -1 * dt * x106; const FLT x380 = x379 + x375 + dt; const FLT x381 = dt * x109; const FLT x382 = dt * x110; const FLT x383 = x382 + x381; const FLT x384 = x369 + (-1 * x370); const FLT x385 = (-1 * x72 * ((x76 * x380) + (-1 * ((x74 * x383) + (x73 * x384)) * x75))) + (-1 * ((x68 * x383) + (-1 * x50 * x384)) * x71); const FLT x386 = x381 + (-1 * x382); const FLT x387 = x376 + x379; const FLT x388 = x372 + x373; const FLT x389 = (-1 * x72 * ((x76 * x386) + (-1 * ((x74 * x387) + (x73 * x388)) * x75))) + (-1 * ((x68 * x387) + (-1 * x50 * x388)) * x71); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x77 + (x61 * ((x53 * x58) + (-1 * x5 * x50))) + (x78 * x77)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), (((x84 * x58) + (-1 * x81 * x50)) * x61) + x86 + (x86 * x78)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), (((x88 * x58) + (-1 * x87 * x50)) * x61) + x90 + (x78 * x90)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x105 + (((x58 * x99) + (-1 * x50 * x95)) * x61) + (x78 * x105)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x113 + (((x58 * x111) + (-1 * x50 * x108)) * x61) + (x78 * x113)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x117 + (((x58 * x115) + (-1 * x50 * x114)) * x61) + (x78 * x117)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x156 + (((x58 * x154) + (-1 * x50 * x152)) * x61) + (x78 * x156)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (((x58 * x176) + (-1 * x50 * x174)) * x61) + x178 + (x78 * x178)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x194 + (x78 * x194) + (((x58 * x192) + (-1 * x50 * x193)) * x61)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x207 + (x78 * x207) + (((x58 * x205) + (-1 * x50 * x206)) * x61)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x295 + (((x58 * x293) + (-1 * x50 * x291)) * x61) + (x78 * x295)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x335 + (((x58 * x333) + (-1 * x50 * x331)) * x61) + (x78 * x335)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x368 + (((x58 * x366) + (-1 * x50 * x364)) * x61) + (x78 * x368)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x378 + (((x58 * x374) + (-1 * x50 * x371)) * x61) + (x78 * x378)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x385 + (((x58 * x383) + (-1 * x50 * x380)) * x61) + (x78 * x385)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), (((x58 * x387) + (-1 * x50 * x386)) * x61) + x389 + (x78 * x389)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen1_jac_x0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * dt; const FLT x1 = x0 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x2 = x0 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x3 = x0 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x4 = 1e-10 + x3 + x1 + x2; const FLT x5 = sqrt(x4); const FLT x6 = 0.5 * x5; const FLT x7 = cos(x6); const FLT x8 = sin(x6); const FLT x9 = (1. / x4) * (x8 * x8); const FLT x10 = 1. / sqrt((x2 * x9) + (x1 * x9) + (x3 * x9) + (x7 * x7)); const FLT x11 = x7 * x10; const FLT x12 = x11 * (*_x0).Pose.Rot[2]; const FLT x13 = (1. / x5) * x8 * dt * x10; const FLT x14 = x13 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x15 = x14 * (*_x0).Pose.Rot[0]; const FLT x16 = x13 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = x16 * (*_x0).Pose.Rot[3]; const FLT x18 = x13 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x19 = x18 * (*_x0).Pose.Rot[1]; const FLT x20 = (-1 * x19) + x17 + (-1 * x12) + (-1 * x15); const FLT x21 = x18 * (*_x0).Pose.Rot[2]; const FLT x22 = x14 * (*_x0).Pose.Rot[3]; const FLT x23 = x11 * (*_x0).Pose.Rot[1]; const FLT x24 = x16 * (*_x0).Pose.Rot[0]; const FLT x25 = x23 + x24 + (-1 * x21) + x22; const FLT x26 = 2 * x25; const FLT x27 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x11 * (*_x0).Pose.Rot[0]) + (-1 * x18 * (*_x0).Pose.Rot[3]) + (-1 * x14 * (*_x0).Pose.Rot[2]); const FLT x28 = x18 * (*_x0).Pose.Rot[0]; const FLT x29 = x14 * (*_x0).Pose.Rot[1]; const FLT x30 = x11 * (*_x0).Pose.Rot[3]; const FLT x31 = x16 * (*_x0).Pose.Rot[2]; const FLT x32 = x30 + x28 + x31 + (-1 * x29); const FLT x33 = 2 * x32; const FLT x34 = x33 * x27; const FLT x35 = x34 + (-1 * x20 * x26); const FLT x36 = (-1 * x17) + x19 + x15 + x12; const FLT x37 = 2 * x36; const FLT x38 = x37 * x27; const FLT x39 = (x33 * x25) + (-1 * x38); const FLT x40 = 1 + (x37 * x20) + (-2 * (x32 * x32)); const FLT x41 = (x40 * (*lh_p).Rot[0]) + (-1 * x35 * (*lh_p).Rot[3]) + (x39 * (*lh_p).Rot[2]); const FLT x42 = 2 * (*lh_p).Rot[3]; const FLT x43 = (x35 * (*lh_p).Rot[1]) + (-1 * x40 * (*lh_p).Rot[2]) + (x39 * (*lh_p).Rot[0]); const FLT x44 = 2 * (*lh_p).Rot[1]; const FLT x45 = x35 + (x41 * x42) + (-1 * x43 * x44); const FLT x46 = 1.0/2.0 * dt * fabs(dt); const FLT x47 = (-1 * x32 * sensor_pt[1]) + (x36 * sensor_pt[2]) + (x27 * sensor_pt[0]); const FLT x48 = (-1 * x25 * sensor_pt[2]) + (x32 * sensor_pt[0]) + (x27 * sensor_pt[1]); const FLT x49 = (2 * ((x48 * x25) + (-1 * x47 * x36))) + sensor_pt[2] + (x46 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x50 = (-1 * x36 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x25 * sensor_pt[1]); const FLT x51 = (2 * ((x50 * x36) + (-1 * x48 * x32))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x46 * (*_x0).Acc[0]); const FLT x52 = (2 * ((x47 * x32) + (-1 * x50 * x25))) + (dt * (*_x0).Velocity.Pos[1]) + (*_x0).Pose.Pos[1] + sensor_pt[1] + (x46 * (*_x0).Acc[1]); const FLT x53 = (-1 * x52 * (*lh_p).Rot[3]) + (x49 * (*lh_p).Rot[2]) + (x51 * (*lh_p).Rot[0]); const FLT x54 = (-1 * x49 * (*lh_p).Rot[1]) + (x52 * (*lh_p).Rot[0]) + (x51 * (*lh_p).Rot[3]); const FLT x55 = x49 + (2 * ((x54 * (*lh_p).Rot[1]) + (-1 * x53 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x56 = 1. / x55; const FLT x57 = (x40 * (*lh_p).Rot[3]) + (-1 * x39 * (*lh_p).Rot[1]) + (x35 * (*lh_p).Rot[0]); const FLT x58 = 2 * (*lh_p).Rot[2]; const FLT x59 = x39 + (x57 * x44) + (-1 * x58 * x41); const FLT x60 = x55 * x55; const FLT x61 = 1. / x60; const FLT x62 = (-1 * x51 * (*lh_p).Rot[2]) + (x49 * (*lh_p).Rot[0]) + (x52 * (*lh_p).Rot[1]); const FLT x63 = x52 + (*lh_p).Pos[1] + (2 * ((x53 * (*lh_p).Rot[3]) + (-1 * x62 * (*lh_p).Rot[1]))); const FLT x64 = x63 * x61; const FLT x65 = x63 * x63; const FLT x66 = -1 * x55; const FLT x67 = 2 * (1. / (x60 + x65)) * x60 * atan2(x63, x66) * (*bsc0).curve; const FLT x68 = (x58 * x43) + x40 + (-1 * x57 * x42); const FLT x69 = x51 + (2 * ((x62 * (*lh_p).Rot[2]) + (-1 * x54 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x70 = x61 * x69; const FLT x71 = x60 + (x69 * x69); const FLT x72 = 1. / x71; const FLT x73 = x72 * x60; const FLT x74 = 1. / sqrt(1 + (-1 * x72 * x65 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x75 = 2 * x69; const FLT x76 = 2 * x55; const FLT x77 = 1.0/2.0 * (1. / (x71 * sqrt(x71))) * x63 * (*bsc0).tilt; const FLT x78 = (1. / sqrt(x71)) * (*bsc0).tilt; const FLT x79 = (-1 * x74 * ((x78 * x45) + (-1 * ((x76 * x59) + (x75 * x68)) * x77))) + (-1 * ((x70 * x59) + (-1 * x68 * x56)) * x73); const FLT x80 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x78 * x63)) + (-1 * atan2(x69, x66))); const FLT x81 = (-1 * x30) + (-1 * x31) + x29 + (-1 * x28); const FLT x82 = x26 * x27; const FLT x83 = x82 + (-1 * x81 * x37); const FLT x84 = 1 + (x81 * x33) + (-2 * (x25 * x25)); const FLT x85 = (x37 * x25) + (-1 * x34); const FLT x86 = 2 * ((x85 * (*lh_p).Rot[0]) + (x83 * (*lh_p).Rot[2]) + (-1 * x84 * (*lh_p).Rot[3])); const FLT x87 = 2 * ((x84 * (*lh_p).Rot[1]) + (x83 * (*lh_p).Rot[0]) + (-1 * x85 * (*lh_p).Rot[2])); const FLT x88 = x84 + (x86 * (*lh_p).Rot[3]) + (-1 * x87 * (*lh_p).Rot[1]); const FLT x89 = 2 * ((x85 * (*lh_p).Rot[3]) + (-1 * x83 * (*lh_p).Rot[1]) + (x84 * (*lh_p).Rot[0])); const FLT x90 = x83 + (x89 * (*lh_p).Rot[1]) + (-1 * x86 * (*lh_p).Rot[2]); const FLT x91 = x85 + (x87 * (*lh_p).Rot[2]) + (-1 * x89 * (*lh_p).Rot[3]); const FLT x92 = (-1 * x74 * ((x88 * x78) + (-1 * ((x76 * x90) + (x75 * x91)) * x77))) + (-1 * ((x70 * x90) + (-1 * x56 * x91)) * x73); const FLT x93 = (x32 * x37) + (-1 * x82); const FLT x94 = x21 + (-1 * x24) + (-1 * x22) + (-1 * x23); const FLT x95 = 1 + (x94 * x26) + (-2 * (x36 * x36)); const FLT x96 = x38 + (-1 * x94 * x33); const FLT x97 = (x96 * (*lh_p).Rot[0]) + (-1 * x93 * (*lh_p).Rot[3]) + (x95 * (*lh_p).Rot[2]); const FLT x98 = (x93 * (*lh_p).Rot[1]) + (-1 * x96 * (*lh_p).Rot[2]) + (x95 * (*lh_p).Rot[0]); const FLT x99 = x93 + (x97 * x42) + (-1 * x98 * x44); const FLT x100 = (x96 * (*lh_p).Rot[3]) + (-1 * x95 * (*lh_p).Rot[1]) + (x93 * (*lh_p).Rot[0]); const FLT x101 = x95 + (x44 * x100) + (-1 * x58 * x97); const FLT x102 = x96 + (x58 * x98) + (-1 * x42 * x100); const FLT x103 = (-1 * x74 * ((x78 * x99) + (-1 * ((x76 * x101) + (x75 * x102)) * x77))) + (-1 * ((x70 * x101) + (-1 * x56 * x102)) * x73); cnMatrixOptionalSet(Hx, 0, 0, x79 + (((x64 * x59) + (-1 * x56 * x45)) * x67) + (x80 * x79)); cnMatrixOptionalSet(Hx, 0, 1, x92 + (((x64 * x90) + (-1 * x88 * x56)) * x67) + (x80 * x92)); cnMatrixOptionalSet(Hx, 0, 2, x103 + (x67 * ((x64 * x101) + (-1 * x56 * x99))) + (x80 * x103)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen1_jac_sensor_pt(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * (*_x0).Velocity.Pos[2]; const FLT x1 = 1.0/2.0 * dt * fabs(dt); const FLT x2 = x1 * (*_x0).Acc[2]; const FLT x3 = dt * dt; const FLT x4 = x3 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x5 = x3 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x6 = x3 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x7 = 1e-10 + x4 + x5 + x6; const FLT x8 = sqrt(x7); const FLT x9 = 0.5 * x8; const FLT x10 = sin(x9); const FLT x11 = (1. / x7) * (x10 * x10); const FLT x12 = cos(x9); const FLT x13 = 1. / sqrt((x6 * x11) + (x5 * x11) + (x4 * x11) + (x12 * x12)); const FLT x14 = (1. / x8) * dt * x13 * x10; const FLT x15 = x14 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x16 = x13 * x12; const FLT x17 = x14 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x14 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = (-1 * x18 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]); const FLT x20 = x14 * (*_x0).Pose.Rot[2]; const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (-1 * x18 * (*_x0).Pose.Rot[1]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x20 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x22 = (x20 * (*_x0).Velocity.AxisAngleRot[0]) + (x16 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[1]); const FLT x23 = (x19 * sensor_pt[2]) + (-1 * x22 * sensor_pt[1]) + (x21 * sensor_pt[0]); const FLT x24 = (-1 * x20 * (*_x0).Velocity.AxisAngleRot[2]) + (x18 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[3]); const FLT x25 = (x22 * sensor_pt[0]) + (-1 * x24 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x26 = 2 * ((x24 * x25) + (-1 * x23 * x19)); const FLT x27 = sensor_pt[2] + x2 + x0 + x26 + (*_x0).Pose.Pos[2]; const FLT x28 = x27 * (*lh_p).Rot[2]; const FLT x29 = dt * (*_x0).Velocity.Pos[0]; const FLT x30 = x1 * (*_x0).Acc[0]; const FLT x31 = (-1 * x19 * sensor_pt[0]) + (x21 * sensor_pt[2]) + (x24 * sensor_pt[1]); const FLT x32 = 2 * ((x31 * x19) + (-1 * x25 * x22)); const FLT x33 = x32 + (*_x0).Pose.Pos[0] + x29 + sensor_pt[0] + x30; const FLT x34 = x33 * (*lh_p).Rot[0]; const FLT x35 = x1 * (*_x0).Acc[1]; const FLT x36 = dt * (*_x0).Velocity.Pos[1]; const FLT x37 = 2 * ((x22 * x23) + (-1 * x31 * x24)); const FLT x38 = x37 + x36 + (*_x0).Pose.Pos[1] + sensor_pt[1] + x35; const FLT x39 = x38 * (*lh_p).Rot[3]; const FLT x40 = (-1 * x39) + x28 + x34; const FLT x41 = x38 * (*lh_p).Rot[0]; const FLT x42 = x33 * (*lh_p).Rot[3]; const FLT x43 = x27 * (*lh_p).Rot[1]; const FLT x44 = (-1 * x43) + x41 + x42; const FLT x45 = (2 * ((x44 * (*lh_p).Rot[1]) + (-1 * x40 * (*lh_p).Rot[2]))) + x27 + (*lh_p).Pos[2]; const FLT x46 = x27 * (*lh_p).Rot[0]; const FLT x47 = x38 * (*lh_p).Rot[1]; const FLT x48 = x33 * (*lh_p).Rot[2]; const FLT x49 = (-1 * x48) + x46 + x47; const FLT x50 = x33 + (2 * ((x49 * (*lh_p).Rot[2]) + (-1 * x44 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x51 = x45 * x45; const FLT x52 = x51 + (x50 * x50); const FLT x53 = 1. / x52; const FLT x54 = (*lh_p).Pos[1] + x38 + (2 * ((x40 * (*lh_p).Rot[3]) + (-1 * x49 * (*lh_p).Rot[1]))); const FLT x55 = x54 * x54; const FLT x56 = 1. / sqrt(1 + (-1 * x53 * x55 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x57 = x54 * (1. / (x52 * sqrt(x52))) * (*bsc0).tilt; const FLT x58 = x57 * x56; const FLT x59 = (x50 * x58) + (x53 * x45); const FLT x60 = (1. / sqrt(x52)) * (*bsc0).tilt; const FLT x61 = -1 * x45; const FLT x62 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x60 * x54)) + (-1 * atan2(x50, x61))); const FLT x63 = 2 * x45; const FLT x64 = (1. / (x51 + x55)) * atan2(x54, x61) * (*bsc0).curve; const FLT x65 = x60 * x56; const FLT x66 = 2 * x64; const FLT x67 = (x58 * x45) + (-1 * x50 * x53); const FLT x68 = 1. / x45; const FLT x69 = 2 * x43; const FLT x70 = (2 * x42) + (-1 * x69); const FLT x71 = 2 * x48; const FLT x72 = (2 * x47) + (-1 * x71); const FLT x73 = 1. / x51; const FLT x74 = x73 * x54; const FLT x75 = x66 * x51; const FLT x76 = 2 * x39; const FLT x77 = (2 * x28) + (-1 * x76); const FLT x78 = x73 * x50; const FLT x79 = x53 * x51; const FLT x80 = 2 * x50; const FLT x81 = 1.0/2.0 * x57; const FLT x82 = (-1 * x56 * ((x70 * x60) + (-1 * ((x72 * x63) + (x80 * x77)) * x81))) + (-1 * ((x72 * x78) + (-1 * x77 * x68)) * x79); const FLT x83 = 2 * x46; const FLT x84 = (-1 * x83) + (-4 * x47) + x71; const FLT x85 = (-1 * sensor_pt[2]) + (-1 * x2) + (-1 * x26) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x0); const FLT x86 = 2 * (*lh_p).Rot[1]; const FLT x87 = 2 * x41; const FLT x88 = x70 + (x85 * x86) + x87; const FLT x89 = 2 * (*lh_p).Rot[3]; const FLT x90 = 2 * (*lh_p).Rot[2]; const FLT x91 = (x90 * x38) + (-1 * x89 * x85); const FLT x92 = (-1 * x56 * ((x84 * x60) + (-1 * ((x88 * x63) + (x80 * x91)) * x81))) + (-1 * ((x88 * x78) + (-1 * x68 * x91)) * x79); const FLT x93 = 2 * ((-1 * (*_x0).Pose.Pos[0]) + (-1 * x32) + (-1 * x29) + (-1 * x30) + (-1 * sensor_pt[0])); const FLT x94 = (x89 * x27) + (-1 * x93 * (*lh_p).Rot[1]); const FLT x95 = 2 * x34; const FLT x96 = (-1 * x95) + (-4 * x28) + x76; const FLT x97 = x72 + x83 + (x93 * (*lh_p).Rot[2]); const FLT x98 = (-1 * x56 * ((x60 * x94) + (-1 * ((x63 * x96) + (x80 * x97)) * x81))) + (-1 * ((x78 * x96) + (-1 * x68 * x97)) * x79); const FLT x99 = (-1 * x37) + (-1 * x36) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * x35) + (-1 * sensor_pt[1]); const FLT x100 = x77 + (x89 * x99) + x95; const FLT x101 = (x86 * x33) + (-1 * x90 * x99); const FLT x102 = (-1 * x87) + x69 + (-4 * x42); const FLT x103 = (-1 * x56 * ((x60 * x100) + (-1 * ((x63 * x101) + (x80 * x102)) * x81))) + (-1 * ((x78 * x101) + (-1 * x68 * x102)) * x79); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), x59 + (x62 * x59)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x65) + (-1 * x63 * x64) + (-1 * x62 * x65)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x67 + (x66 * x54) + (x62 * x67)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x82 + (((x72 * x74) + (-1 * x70 * x68)) * x75) + (x82 * x62)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x92 + (((x88 * x74) + (-1 * x84 * x68)) * x75) + (x62 * x92)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x98 + (((x74 * x96) + (-1 * x68 * x94)) * x75) + (x62 * x98)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), x103 + (((x74 * x101) + (-1 * x68 * x100)) * x75) + (x62 * x103)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen1_jac_lh_p(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x11 * x10; const FLT x15 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[1]) + (x13 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x14 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]) + (-1 * x13 * (*_x0).Pose.Rot[1]); const FLT x20 = (-1 * x19 * sensor_pt[1]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[0]); const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[1]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (x13 * (*_x0).Pose.Rot[3]); const FLT x22 = (x19 * sensor_pt[0]) + (-1 * x21 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x23 = (2 * ((x22 * x21) + (-1 * x20 * x17))) + sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x24 = (-1 * x17 * sensor_pt[0]) + (x18 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x25 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x26 = (*_x0).Pose.Pos[0] + (2 * ((x24 * x17) + (-1 * x22 * x19))) + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x27 = (-1 * x26 * (*lh_p).Rot[2]) + (x23 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[1]); const FLT x28 = (-1 * x25 * (*lh_p).Rot[3]) + (x23 * (*lh_p).Rot[2]) + (x26 * (*lh_p).Rot[0]); const FLT x29 = x25 + (*lh_p).Pos[1] + (2 * ((x28 * (*lh_p).Rot[3]) + (-1 * x27 * (*lh_p).Rot[1]))); const FLT x30 = (-1 * x23 * (*lh_p).Rot[1]) + (x25 * (*lh_p).Rot[0]) + (x26 * (*lh_p).Rot[3]); const FLT x31 = x23 + (2 * ((x30 * (*lh_p).Rot[1]) + (-1 * x28 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x32 = -1 * x31; const FLT x33 = x26 + (2 * ((x27 * (*lh_p).Rot[2]) + (-1 * x30 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x34 = (x33 * x33) + (x31 * x31); const FLT x35 = (1. / sqrt(x34)) * x29; const FLT x36 = 1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x35 * (*bsc0).tilt)) + (-1 * atan2(x33, x32)); const FLT x37 = sin(x36) * (*bsc0).gibmag; const FLT x38 = x35 * (1. / sqrt(1 + (-1 * (1. / x34) * (x29 * x29) * ((*bsc0).tilt * (*bsc0).tilt)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), atan2(x29, x32) * atan2(x29, x32)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * cos(x36)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x37); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1 + (-1 * x37)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x38 * x37) + (-1 * x38)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_x_gen1_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen1_jac_bsc0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanModel_LightMeas_y_gen1(const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x14 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x15 = x11 * x10; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (-1 * x13 * (*_x0).Pose.Rot[3]) + (-1 * x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (x15 * (*_x0).Pose.Rot[1]) + (x16 * (*_x0).Pose.Rot[0]) + (-1 * x13 * (*_x0).Pose.Rot[2]) + (x14 * (*_x0).Pose.Rot[3]); const FLT x19 = (x13 * (*_x0).Pose.Rot[1]) + (-1 * x16 * (*_x0).Pose.Rot[3]) + (x14 * (*_x0).Pose.Rot[0]) + (x15 * (*_x0).Pose.Rot[2]); const FLT x20 = (-1 * x19 * sensor_pt[0]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x21 = (x16 * (*_x0).Pose.Rot[2]) + (x15 * (*_x0).Pose.Rot[3]) + (x13 * (*_x0).Pose.Rot[0]) + (-1 * x14 * (*_x0).Pose.Rot[1]); const FLT x22 = (-1 * x21 * sensor_pt[1]) + (x19 * sensor_pt[2]) + (x17 * sensor_pt[0]); const FLT x23 = (dt * (*_x0).Velocity.Pos[1]) + (2 * ((x22 * x21) + (-1 * x20 * x18))) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x24 = (-1 * x18 * sensor_pt[2]) + (x21 * sensor_pt[0]) + (x17 * sensor_pt[1]); const FLT x25 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x26 = sensor_pt[2] + (2 * ((x24 * x18) + (-1 * x22 * x19))) + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x27 = (-1 * x26 * (*lh_p).Rot[1]) + (x23 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[3]); const FLT x28 = (-1 * x25 * (*lh_p).Rot[2]) + (x26 * (*lh_p).Rot[0]) + (x23 * (*lh_p).Rot[1]); const FLT x29 = (2 * ((x28 * (*lh_p).Rot[2]) + (-1 * x27 * (*lh_p).Rot[3]))) + x25 + (*lh_p).Pos[0]; const FLT x30 = (-1 * x23 * (*lh_p).Rot[3]) + (x26 * (*lh_p).Rot[2]) + (x25 * (*lh_p).Rot[0]); const FLT x31 = x26 + (2 * ((x27 * (*lh_p).Rot[1]) + (-1 * x30 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x32 = -1 * x31; const FLT x33 = x23 + (*lh_p).Pos[1] + (2 * ((x30 * (*lh_p).Rot[3]) + (-1 * x28 * (*lh_p).Rot[1]))); const FLT x34 = (-1 * (*bsc0).phase) + (-1 * asin((1. / sqrt((x33 * x33) + (x31 * x31))) * x29 * (*bsc0).tilt)) + (-1 * atan2(-1 * x33, x32)); return ((atan2(x29, x32) * atan2(x29, x32)) * (*bsc0).curve) + x34 + (-1 * cos(1.5707963267949 + x34 + (*bsc0).gibpha) * (*bsc0).gibmag); } // Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x1 = dt * fabs(dt); const FLT x2 = -1 * x0 * x1; const FLT x3 = 1.0/2.0 * x1; const FLT x4 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x5 = (-1 * x1 * x4) + x3; const FLT x6 = x5 + x2; const FLT x7 = dt * dt; const FLT x8 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x9 = x8 * x7; const FLT x10 = (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x11 = x7 * x10; const FLT x12 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x13 = x7 * x12; const FLT x14 = 1e-10 + x9 + x11 + x13; const FLT x15 = sqrt(x14); const FLT x16 = 0.5 * x15; const FLT x17 = sin(x16); const FLT x18 = x17 * x17; const FLT x19 = 1. / x14; const FLT x20 = x19 * x18; const FLT x21 = cos(x16); const FLT x22 = (x20 * x13) + (x20 * x11) + (x9 * x20) + (x21 * x21); const FLT x23 = 1. / sqrt(x22); const FLT x24 = x23 * x17; const FLT x25 = 1. / x15; const FLT x26 = dt * x25; const FLT x27 = x24 * x26; const FLT x28 = x27 * (*_x0).Pose.Rot[0]; const FLT x29 = x23 * x21; const FLT x30 = x29 * (*_x0).Pose.Rot[2]; const FLT x31 = x27 * (*_x0).Pose.Rot[1]; const FLT x32 = x24 * (*_x0).Pose.Rot[3]; const FLT x33 = x32 * x26; const FLT x34 = (-1 * x33 * (*_x0).Velocity.AxisAngleRot[0]) + (x31 * (*_x0).Velocity.AxisAngleRot[2]) + (x28 * (*_x0).Velocity.AxisAngleRot[1]) + x30; const FLT x35 = x27 * (*_x0).Pose.Rot[2]; const FLT x36 = x29 * (*_x0).Pose.Rot[0]; const FLT x37 = (-1 * x31 * (*_x0).Velocity.AxisAngleRot[0]) + x36 + (-1 * x33 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x35 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x38 = x29 * (*_x0).Pose.Rot[3]; const FLT x39 = (x35 * (*_x0).Velocity.AxisAngleRot[0]) + x38 + (x28 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x31 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x40 = (-1 * x39 * sensor_pt[1]) + (x34 * sensor_pt[2]) + (x37 * sensor_pt[0]); const FLT x41 = x29 * (*_x0).Pose.Rot[1]; const FLT x42 = (x28 * (*_x0).Velocity.AxisAngleRot[0]) + x41 + (-1 * x35 * (*_x0).Velocity.AxisAngleRot[2]) + (x33 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x43 = (-1 * x42 * sensor_pt[2]) + (x39 * sensor_pt[0]) + (x37 * sensor_pt[1]); const FLT x44 = sensor_pt[2] + (x3 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (2 * ((x42 * x43) + (-1 * x40 * x34))) + (*_x0).Pose.Pos[2]; const FLT x45 = (-1 * x34 * sensor_pt[0]) + (x37 * sensor_pt[2]) + (x42 * sensor_pt[1]); const FLT x46 = (*_x0).Pose.Pos[0] + sensor_pt[0] + (2 * ((x45 * x34) + (-1 * x43 * x39))) + (dt * (*_x0).Velocity.Pos[0]) + (x3 * (*_x0).Acc[0]); const FLT x47 = (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (2 * ((x40 * x39) + (-1 * x42 * x45))) + (*_x0).Pose.Pos[1] + (x3 * (*_x0).Acc[1]); const FLT x48 = (x44 * (*lh_p).Rot[2]) + (-1 * x47 * (*lh_p).Rot[3]) + (x46 * (*lh_p).Rot[0]); const FLT x49 = (-1 * x44 * (*lh_p).Rot[1]) + (x47 * (*lh_p).Rot[0]) + (x46 * (*lh_p).Rot[3]); const FLT x50 = x44 + (2 * ((x49 * (*lh_p).Rot[1]) + (-1 * x48 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x51 = 1. / x50; const FLT x52 = x1 * (*lh_p).Rot[0]; const FLT x53 = x52 * (*lh_p).Rot[2]; const FLT x54 = x1 * (*lh_p).Rot[3]; const FLT x55 = x54 * (*lh_p).Rot[1]; const FLT x56 = x55 + (-1 * x53); const FLT x57 = x50 * x50; const FLT x58 = 1. / x57; const FLT x59 = (x44 * (*lh_p).Rot[0]) + (-1 * x46 * (*lh_p).Rot[2]) + (x47 * (*lh_p).Rot[1]); const FLT x60 = x46 + (2 * ((x59 * (*lh_p).Rot[2]) + (-1 * x49 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x61 = x60 * x58; const FLT x62 = -1 * x50; const FLT x63 = x60 * x60; const FLT x64 = 2 * (1. / (x57 + x63)) * x57 * atan2(x60, x62) * (*bsc0).curve; const FLT x65 = x1 * (*lh_p).Rot[1] * (*lh_p).Rot[2]; const FLT x66 = x54 * (*lh_p).Rot[0]; const FLT x67 = x66 + x65; const FLT x68 = (*lh_p).Pos[1] + x47 + (2 * ((x48 * (*lh_p).Rot[3]) + (-1 * x59 * (*lh_p).Rot[1]))); const FLT x69 = x68 * x58; const FLT x70 = (x68 * x68) + x57; const FLT x71 = 1. / x70; const FLT x72 = x71 * x57; const FLT x73 = 1. / sqrt(1 + (-1 * x71 * x63 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x74 = (1. / sqrt(x70)) * (*bsc0).tilt; const FLT x75 = 2 * x68; const FLT x76 = 2 * x50; const FLT x77 = 1.0/2.0 * (1. / (x70 * sqrt(x70))) * x60 * (*bsc0).tilt; const FLT x78 = (-1 * x73 * ((-1 * ((x76 * x56) + (x75 * x67)) * x77) + (x6 * x74))) + (-1 * ((-1 * x69 * x56) + (x67 * x51)) * x72); const FLT x79 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x74 * x60)) + (-1 * atan2(-1 * x68, x62))); const FLT x80 = x65 + (-1 * x66); const FLT x81 = x54 * (*lh_p).Rot[2]; const FLT x82 = x52 * (*lh_p).Rot[1]; const FLT x83 = x82 + x81; const FLT x84 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x85 = -1 * x1 * x84; const FLT x86 = x85 + x2 + x3; const FLT x87 = (-1 * x73 * ((-1 * ((x83 * x76) + (x86 * x75)) * x77) + (x80 * x74))) + (-1 * ((-1 * x83 * x69) + (x86 * x51)) * x72); const FLT x88 = x53 + x55; const FLT x89 = x5 + x85; const FLT x90 = x81 + (-1 * x82); const FLT x91 = (-1 * x73 * ((-1 * ((x89 * x76) + (x75 * x90)) * x77) + (x88 * x74))) + (-1 * ((-1 * x89 * x69) + (x51 * x90)) * x72); const FLT x92 = 2 * x4; const FLT x93 = -1 * x92; const FLT x94 = 2 * x0; const FLT x95 = 1 + (-1 * x94); const FLT x96 = x95 + x93; const FLT x97 = 2 * (*lh_p).Rot[2]; const FLT x98 = x97 * (*lh_p).Rot[0]; const FLT x99 = 2 * (*lh_p).Rot[1]; const FLT x100 = x99 * (*lh_p).Rot[3]; const FLT x101 = x100 + (-1 * x98); const FLT x102 = x97 * (*lh_p).Rot[1]; const FLT x103 = 2 * (*lh_p).Rot[3]; const FLT x104 = x103 * (*lh_p).Rot[0]; const FLT x105 = x104 + x102; const FLT x106 = (-1 * x73 * ((-1 * ((x76 * x101) + (x75 * x105)) * x77) + (x74 * x96))) + (-1 * ((-1 * x69 * x101) + (x51 * x105)) * x72); const FLT x107 = x102 + (-1 * x104); const FLT x108 = x97 * (*lh_p).Rot[3]; const FLT x109 = x99 * (*lh_p).Rot[0]; const FLT x110 = x109 + x108; const FLT x111 = 2 * x84; const FLT x112 = -1 * x111; const FLT x113 = x95 + x112; const FLT x114 = (-1 * x73 * ((-1 * ((x76 * x110) + (x75 * x113)) * x77) + (x74 * x107))) + (-1 * ((-1 * x69 * x110) + (x51 * x113)) * x72); const FLT x115 = x98 + x100; const FLT x116 = 1 + x112 + x93; const FLT x117 = x108 + (-1 * x109); const FLT x118 = (-1 * x73 * ((-1 * ((x76 * x116) + (x75 * x117)) * x77) + (x74 * x115))) + (-1 * ((-1 * x69 * x116) + (x51 * x117)) * x72); const FLT x119 = x27 * sensor_pt[2]; const FLT x120 = x119 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x121 = x29 * sensor_pt[0]; const FLT x122 = x27 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x123 = -1 * x122 * sensor_pt[1]; const FLT x124 = x123 + x121; const FLT x125 = x124 + x120; const FLT x126 = 2 * x34; const FLT x127 = 2 * x43; const FLT x128 = x27 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x129 = x128 * x127; const FLT x130 = 2 * x40; const FLT x131 = x27 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x132 = -1 * x130 * x131; const FLT x133 = x122 * sensor_pt[0]; const FLT x134 = -1 * x119 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x135 = x29 * sensor_pt[1]; const FLT x136 = x135 + x134; const FLT x137 = x136 + x133; const FLT x138 = 2 * x42; const FLT x139 = (x137 * x138) + x132 + (-1 * x126 * x125) + x129; const FLT x140 = -1 * x131 * sensor_pt[0]; const FLT x141 = x29 * sensor_pt[2]; const FLT x142 = x128 * sensor_pt[1]; const FLT x143 = x142 + x140 + x141; const FLT x144 = 2 * x39; const FLT x145 = x122 * x130; const FLT x146 = 2 * x45; const FLT x147 = -1 * x128 * x146; const FLT x148 = x145 + (-1 * x138 * x143) + x147 + (x125 * x144); const FLT x149 = -1 * x122 * x127; const FLT x150 = x131 * x146; const FLT x151 = x150 + (-1 * x137 * x144) + (x126 * x143) + x149; const FLT x152 = (x151 * (*lh_p).Rot[3]) + (-1 * x139 * (*lh_p).Rot[1]) + (x148 * (*lh_p).Rot[0]); const FLT x153 = (x148 * (*lh_p).Rot[1]) + (-1 * x151 * (*lh_p).Rot[2]) + (x139 * (*lh_p).Rot[0]); const FLT x154 = x151 + (-1 * x103 * x152) + (x97 * x153); const FLT x155 = (x151 * (*lh_p).Rot[0]) + (-1 * x148 * (*lh_p).Rot[3]) + (x139 * (*lh_p).Rot[2]); const FLT x156 = x139 + (-1 * x97 * x155) + (x99 * x152); const FLT x157 = (-1 * x99 * x153) + x148 + (x103 * x155); const FLT x158 = (-1 * x73 * ((-1 * ((x76 * x156) + (x75 * x157)) * x77) + (x74 * x154))) + (-1 * ((-1 * x69 * x156) + (x51 * x157)) * x72); const FLT x159 = x131 * sensor_pt[1]; const FLT x160 = x119 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x161 = x128 * sensor_pt[0]; const FLT x162 = (-1 * x161) + x159 + x160; const FLT x163 = (-1 * x142) + x140; const FLT x164 = x163 + (-1 * x141); const FLT x165 = x29 * x127; const FLT x166 = (-1 * x126 * x162) + (-1 * x145) + x165 + (x164 * x138); const FLT x167 = -1 * x133; const FLT x168 = x136 + x167; const FLT x169 = x29 * x146; const FLT x170 = (-1 * x169) + x132 + (-1 * x168 * x138) + (x162 * x144); const FLT x171 = x122 * x146; const FLT x172 = x127 * x131; const FLT x173 = (x126 * x168) + x172 + x171 + (-1 * x164 * x144); const FLT x174 = (x173 * (*lh_p).Rot[3]) + (-1 * x166 * (*lh_p).Rot[1]) + (x170 * (*lh_p).Rot[0]); const FLT x175 = 2 * ((-1 * x173 * (*lh_p).Rot[2]) + (x170 * (*lh_p).Rot[1]) + (x166 * (*lh_p).Rot[0])); const FLT x176 = x173 + (-1 * x103 * x174) + (x175 * (*lh_p).Rot[2]); const FLT x177 = 2 * ((x173 * (*lh_p).Rot[0]) + (-1 * x170 * (*lh_p).Rot[3]) + (x166 * (*lh_p).Rot[2])); const FLT x178 = x166 + (-1 * x177 * (*lh_p).Rot[2]) + (x99 * x174); const FLT x179 = x170 + (-1 * x175 * (*lh_p).Rot[1]) + (x177 * (*lh_p).Rot[3]); const FLT x180 = (-1 * x73 * ((-1 * ((x76 * x178) + (x75 * x179)) * x77) + (x74 * x176))) + (-1 * ((-1 * x69 * x178) + (x51 * x179)) * x72); const FLT x181 = -1 * x120; const FLT x182 = x123 + (-1 * x121) + x181; const FLT x183 = x163 + x141; const FLT x184 = x128 * x130; const FLT x185 = (-1 * x182 * x138) + x184 + (x183 * x144) + x171; const FLT x186 = x29 * x130; const FLT x187 = (-1 * x159) + x160 + x161; const FLT x188 = x149 + (x187 * x138) + (-1 * x126 * x183) + (-1 * x186); const FLT x189 = (x126 * x182) + x169 + (-1 * x129) + (-1 * x187 * x144); const FLT x190 = (x189 * (*lh_p).Rot[3]) + (x185 * (*lh_p).Rot[0]) + (-1 * x188 * (*lh_p).Rot[1]); const FLT x191 = (x185 * (*lh_p).Rot[1]) + (x188 * (*lh_p).Rot[0]) + (-1 * x189 * (*lh_p).Rot[2]); const FLT x192 = x189 + (-1 * x103 * x190) + (x97 * x191); const FLT x193 = (-1 * x185 * (*lh_p).Rot[3]) + (x189 * (*lh_p).Rot[0]) + (x188 * (*lh_p).Rot[2]); const FLT x194 = x188 + (-1 * x97 * x193) + (x99 * x190); const FLT x195 = x185 + (-1 * x99 * x191) + (x103 * x193); const FLT x196 = (-1 * x73 * ((-1 * ((x76 * x194) + (x75 * x195)) * x77) + (x74 * x192))) + (-1 * ((-1 * x69 * x194) + (x51 * x195)) * x72); const FLT x197 = x167 + (-1 * x135) + x134; const FLT x198 = x124 + x181; const FLT x199 = (x198 * x138) + (-1 * x126 * x197) + x172 + x184; const FLT x200 = x161 + x159 + (-1 * x160); const FLT x201 = (-1 * x150) + x186 + (-1 * x200 * x138) + (x197 * x144); const FLT x202 = x147 + (x200 * x126) + (-1 * x165) + (-1 * x198 * x144); const FLT x203 = (x202 * (*lh_p).Rot[3]) + (-1 * x199 * (*lh_p).Rot[1]) + (x201 * (*lh_p).Rot[0]); const FLT x204 = (x201 * (*lh_p).Rot[1]) + (-1 * x202 * (*lh_p).Rot[2]) + (x199 * (*lh_p).Rot[0]); const FLT x205 = x202 + (-1 * x203 * x103) + (x97 * x204); const FLT x206 = (x202 * (*lh_p).Rot[0]) + (-1 * x201 * (*lh_p).Rot[3]) + (x199 * (*lh_p).Rot[2]); const FLT x207 = (-1 * x97 * x206) + x199 + (x99 * x203); const FLT x208 = x201 + (x206 * x103) + (-1 * x99 * x204); const FLT x209 = (-1 * x73 * ((-1 * ((x76 * x207) + (x75 * x208)) * x77) + (x74 * x205))) + (-1 * ((-1 * x69 * x207) + (x51 * x208)) * x72); const FLT x210 = dt * dt * dt; const FLT x211 = 0.5 * x19 * x210; const FLT x212 = x12 * x211; const FLT x213 = 1.0/2.0 * (1. / (x22 * sqrt(x22))); const FLT x214 = x26 * x17 * x213; const FLT x215 = x214 * (*_x0).Pose.Rot[0]; const FLT x216 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x217 = dt * dt * dt * dt; const FLT x218 = 2 * (1. / (x14 * x14)) * x18; const FLT x219 = x218 * x217; const FLT x220 = x8 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x221 = 1. / (x14 * sqrt(x14)); const FLT x222 = 1.0 * x21 * x17; const FLT x223 = x221 * x222; const FLT x224 = x217 * x223; const FLT x225 = x25 * x222; const FLT x226 = x7 * x225; const FLT x227 = 2 * x20; const FLT x228 = x7 * x227; const FLT x229 = x10 * x224; const FLT x230 = x10 * x219; const FLT x231 = (-1 * x230 * (*_x0).Velocity.AxisAngleRot[0]) + (x229 * (*_x0).Velocity.AxisAngleRot[0]) + (x220 * x224) + (-1 * x219 * x216) + (-1 * x219 * x220) + (x216 * x224) + (x228 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x226 * (*_x0).Velocity.AxisAngleRot[0]); const FLT x232 = x231 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x233 = x210 * x221; const FLT x234 = x12 * x233; const FLT x235 = x24 * (*_x0).Pose.Rot[2]; const FLT x236 = x21 * x213; const FLT x237 = x231 * x236; const FLT x238 = x231 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x239 = x214 * x238; const FLT x240 = 0.5 * x25; const FLT x241 = x7 * x240; const FLT x242 = x241 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x243 = x214 * (*_x0).Pose.Rot[2]; const FLT x244 = x231 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x245 = x211 * (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x246 = x24 * (*_x0).Pose.Rot[0]; const FLT x247 = x233 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x248 = x247 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x249 = (-1 * x246 * x248) + (x36 * x245); const FLT x250 = x24 * (*_x0).Pose.Rot[1]; const FLT x251 = x247 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x252 = x211 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x253 = x252 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x254 = (-1 * x41 * x253) + (x250 * x251); const FLT x255 = x249 + (-1 * x243 * x244) + x254 + (-1 * x32 * x242) + x35 + (-1 * x215 * x232) + (x30 * x212) + (-1 * x234 * x235) + (-1 * x237 * (*_x0).Pose.Rot[3]) + (x239 * (*_x0).Pose.Rot[1]); const FLT x256 = -1 * x33; const FLT x257 = x214 * (*_x0).Pose.Rot[3]; const FLT x258 = x250 * x248; const FLT x259 = x41 * x245; const FLT x260 = x214 * (*_x0).Pose.Rot[1]; const FLT x261 = (-1 * x251 * x246) + (x36 * x253); const FLT x262 = x261 + (-1 * x232 * x260) + (-1 * x258) + (-1 * x239 * (*_x0).Pose.Rot[0]) + x256 + (-1 * x235 * x242) + (-1 * x237 * (*_x0).Pose.Rot[2]) + (-1 * x38 * x212) + (x32 * x234) + x259 + (x257 * x244); const FLT x263 = x30 * x253; const FLT x264 = x235 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x265 = x264 * x247; const FLT x266 = -1 * x31; const FLT x267 = (-1 * x38 * x245) + (x32 * x248); const FLT x268 = x267 + (x234 * x250) + (-1 * x41 * x212) + (-1 * x237 * (*_x0).Pose.Rot[0]) + x266 + (x239 * (*_x0).Pose.Rot[2]) + (-1 * x263) + (x232 * x257) + (x260 * x244) + (-1 * x242 * x246) + x265; const FLT x269 = (x268 * sensor_pt[0]) + (-1 * x255 * sensor_pt[1]) + (x262 * sensor_pt[2]); const FLT x270 = x38 * x253; const FLT x271 = x32 * x251; const FLT x272 = x30 * x245; const FLT x273 = x235 * x248; const FLT x274 = (-1 * x215 * x244) + x28 + (-1 * x234 * x246) + (-1 * x272) + (x232 * x243) + x270 + (-1 * x271) + (-1 * x250 * x242) + x273 + (-1 * x237 * (*_x0).Pose.Rot[1]) + (-1 * x238 * x257) + (x36 * x212); const FLT x275 = (x268 * sensor_pt[1]) + (-1 * x274 * sensor_pt[2]) + (x255 * sensor_pt[0]); const FLT x276 = 2 * x274; const FLT x277 = (x43 * x276) + (x275 * x138) + (-1 * x269 * x126) + (-1 * x262 * x130); const FLT x278 = (x274 * sensor_pt[1]) + (-1 * x262 * sensor_pt[0]) + (x268 * sensor_pt[2]); const FLT x279 = (x269 * x144) + (-1 * x278 * x138) + (-1 * x45 * x276) + (x255 * x130); const FLT x280 = (x278 * x126) + (x262 * x146) + (-1 * x255 * x127) + (-1 * x275 * x144); const FLT x281 = 2 * ((x280 * (*lh_p).Rot[3]) + (-1 * x277 * (*lh_p).Rot[1]) + (x279 * (*lh_p).Rot[0])); const FLT x282 = 2 * ((x279 * (*lh_p).Rot[1]) + (-1 * x280 * (*lh_p).Rot[2]) + (x277 * (*lh_p).Rot[0])); const FLT x283 = (-1 * x281 * (*lh_p).Rot[3]) + x280 + (x282 * (*lh_p).Rot[2]); const FLT x284 = 2 * ((x280 * (*lh_p).Rot[0]) + (x277 * (*lh_p).Rot[2]) + (-1 * x279 * (*lh_p).Rot[3])); const FLT x285 = x277 + (-1 * x284 * (*lh_p).Rot[2]) + (x281 * (*lh_p).Rot[1]); const FLT x286 = x279 + (-1 * x282 * (*lh_p).Rot[1]) + (x284 * (*lh_p).Rot[3]); const FLT x287 = (-1 * x73 * ((-1 * ((x76 * x285) + (x75 * x286)) * x77) + (x74 * x283))) + (-1 * ((-1 * x69 * x285) + (x51 * x286)) * x72); const FLT x288 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x289 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x290 = (-1 * x230 * (*_x0).Velocity.AxisAngleRot[1]) + (x229 * (*_x0).Velocity.AxisAngleRot[1]) + (x289 * x224) + (-1 * x226 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x219 * x288) + (x288 * x224) + (x228 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x219 * x289); const FLT x291 = x214 * x290; const FLT x292 = x291 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x293 = x291 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x294 = x8 * x233; const FLT x295 = x236 * x290; const FLT x296 = x291 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x297 = x8 * x211; const FLT x298 = x241 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x299 = x246 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x300 = x233 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x301 = x252 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x302 = (x36 * x301) + (-1 * x299 * x300); const FLT x303 = x263 + (-1 * x32 * x298) + x302 + (-1 * x265) + (-1 * x296 * (*_x0).Pose.Rot[0]) + (-1 * x292 * (*_x0).Pose.Rot[2]) + (x294 * x250) + (x293 * (*_x0).Pose.Rot[1]) + (-1 * x41 * x297) + x266 + (-1 * x295 * (*_x0).Pose.Rot[3]); const FLT x304 = x300 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x305 = x250 * x304; const FLT x306 = x41 * x301; const FLT x307 = (-1 * x293 * (*_x0).Pose.Rot[0]) + (-1 * x264 * x241) + (-1 * x294 * x246) + (-1 * x270) + (x292 * (*_x0).Pose.Rot[3]) + (-1 * x295 * (*_x0).Pose.Rot[2]) + x28 + (-1 * x305) + (-1 * x296 * (*_x0).Pose.Rot[1]) + x271 + (x36 * x297) + x306; const FLT x308 = -1 * x35; const FLT x309 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Pose.Rot[2]; const FLT x310 = x38 * x301; const FLT x311 = x32 * x304; const FLT x312 = x254 + (x235 * x294) + (-1 * x299 * x241) + x311 + (x291 * x309) + (-1 * x295 * (*_x0).Pose.Rot[0]) + x308 + (-1 * x30 * x297) + (-1 * x310) + (x296 * (*_x0).Pose.Rot[3]) + (x292 * (*_x0).Pose.Rot[1]); const FLT x313 = (x312 * sensor_pt[0]) + (-1 * x303 * sensor_pt[1]) + (x307 * sensor_pt[2]); const FLT x314 = (-1 * x30 * x301) + (x264 * x300); const FLT x315 = (-1 * x295 * (*_x0).Pose.Rot[1]) + (x38 * x297) + x33 + x314 + (-1 * x293 * (*_x0).Pose.Rot[3]) + x261 + (-1 * x298 * x250) + (x296 * (*_x0).Pose.Rot[2]) + (-1 * x292 * (*_x0).Pose.Rot[0]) + (-1 * x32 * x294); const FLT x316 = (x312 * sensor_pt[1]) + (-1 * x315 * sensor_pt[2]) + (x303 * sensor_pt[0]); const FLT x317 = (x316 * x138) + (-1 * x307 * x130) + (-1 * x313 * x126) + (x315 * x127); const FLT x318 = (x315 * sensor_pt[1]) + (-1 * x307 * sensor_pt[0]) + (x312 * sensor_pt[2]); const FLT x319 = (x303 * x130) + (x313 * x144) + (-1 * x318 * x138) + (-1 * x315 * x146); const FLT x320 = (-1 * x316 * x144) + (-1 * x303 * x127) + (x307 * x146) + (x318 * x126); const FLT x321 = (-1 * x317 * (*lh_p).Rot[1]) + (x320 * (*lh_p).Rot[3]) + (x319 * (*lh_p).Rot[0]); const FLT x322 = (x319 * (*lh_p).Rot[1]) + (-1 * x320 * (*lh_p).Rot[2]) + (x317 * (*lh_p).Rot[0]); const FLT x323 = x320 + (-1 * x321 * x103) + (x97 * x322); const FLT x324 = (-1 * x319 * (*lh_p).Rot[3]) + (x320 * (*lh_p).Rot[0]) + (x317 * (*lh_p).Rot[2]); const FLT x325 = (-1 * x97 * x324) + x317 + (x99 * x321); const FLT x326 = x319 + (-1 * x99 * x322) + (x324 * x103); const FLT x327 = (-1 * x73 * ((-1 * ((x76 * x325) + (x75 * x326)) * x77) + (x74 * x323))) + (-1 * ((-1 * x69 * x325) + (x51 * x326)) * x72); const FLT x328 = x10 * x211; const FLT x329 = x219 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x330 = x224 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x331 = x217 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x332 = x7 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x333 = (-1 * x218 * x331) + (x227 * x332) + (x12 * x330) + (-1 * x8 * x329) + (-1 * x12 * x329) + (x223 * x331) + (x8 * x330) + (-1 * x225 * x332); const FLT x334 = x333 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x335 = x333 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x336 = x214 * x335; const FLT x337 = x10 * x233; const FLT x338 = x236 * x333; const FLT x339 = x333 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x340 = x240 * x332; const FLT x341 = (-1 * x273) + (-1 * x336 * (*_x0).Pose.Rot[2]) + (-1 * x215 * x339) + (x260 * x334) + (-1 * x306) + x305 + x28 + (-1 * x338 * (*_x0).Pose.Rot[3]) + (-1 * x246 * x337) + (x36 * x328) + (-1 * x32 * x340) + x272; const FLT x342 = x267 + x31 + (-1 * x250 * x337) + (x257 * x335) + (-1 * x260 * x339) + (-1 * x215 * x334) + (-1 * x338 * (*_x0).Pose.Rot[2]) + x302 + (-1 * x235 * x340) + (x41 * x328); const FLT x343 = x214 * x333; const FLT x344 = x314 + (-1 * x338 * (*_x0).Pose.Rot[0]) + (x257 * x339) + (-1 * x246 * x340) + (x32 * x337) + (x260 * x335) + (-1 * x38 * x328) + x256 + x258 + (-1 * x259) + (x309 * x343); const FLT x345 = (x344 * sensor_pt[0]) + (-1 * x341 * sensor_pt[1]) + (x342 * sensor_pt[2]); const FLT x346 = x249 + (-1 * x250 * x340) + (-1 * x338 * (*_x0).Pose.Rot[1]) + (-1 * x336 * (*_x0).Pose.Rot[0]) + (-1 * x311) + (-1 * x30 * x328) + x310 + (-1 * x257 * x334) + (x235 * x337) + (x343 * (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Pose.Rot[2]) + x308; const FLT x347 = (-1 * x346 * sensor_pt[2]) + (x344 * sensor_pt[1]) + (x341 * sensor_pt[0]); const FLT x348 = 2 * x342; const FLT x349 = 2 * x346; const FLT x350 = (x43 * x349) + (-1 * x40 * x348) + (-1 * x345 * x126) + (x347 * x138); const FLT x351 = (x346 * sensor_pt[1]) + (-1 * x342 * sensor_pt[0]) + (x344 * sensor_pt[2]); const FLT x352 = 2 * x341; const FLT x353 = (-1 * x45 * x349) + (x345 * x144) + (-1 * x351 * x138) + (x40 * x352); const FLT x354 = (x351 * x126) + (-1 * x43 * x352) + (-1 * x347 * x144) + (x45 * x348); const FLT x355 = (x354 * (*lh_p).Rot[3]) + (-1 * x350 * (*lh_p).Rot[1]) + (x353 * (*lh_p).Rot[0]); const FLT x356 = (-1 * x354 * (*lh_p).Rot[2]) + (x353 * (*lh_p).Rot[1]) + (x350 * (*lh_p).Rot[0]); const FLT x357 = x354 + (-1 * x355 * x103) + (x97 * x356); const FLT x358 = (x354 * (*lh_p).Rot[0]) + (-1 * x353 * (*lh_p).Rot[3]) + (x350 * (*lh_p).Rot[2]); const FLT x359 = x350 + (x99 * x355) + (-1 * x97 * x358); const FLT x360 = x353 + (-1 * x99 * x356) + (x358 * x103); const FLT x361 = (-1 * x73 * ((-1 * ((x76 * x359) + (x75 * x360)) * x77) + (x74 * x357))) + (-1 * ((-1 * x69 * x359) + (x51 * x360)) * x72); const FLT x362 = -1 * dt * x92; const FLT x363 = (-1 * dt * x94) + dt; const FLT x364 = x363 + x362; const FLT x365 = dt * x98; const FLT x366 = dt * x100; const FLT x367 = x366 + (-1 * x365); const FLT x368 = dt * x102; const FLT x369 = dt * x104; const FLT x370 = x369 + x368; const FLT x371 = (-1 * x73 * ((-1 * ((x76 * x367) + (x75 * x370)) * x77) + (x74 * x364))) + (-1 * ((-1 * x69 * x367) + (x51 * x370)) * x72); const FLT x372 = x368 + (-1 * x369); const FLT x373 = dt * x108; const FLT x374 = dt * x109; const FLT x375 = x374 + x373; const FLT x376 = -1 * dt * x111; const FLT x377 = x363 + x376; const FLT x378 = (-1 * x73 * ((-1 * ((x76 * x375) + (x75 * x377)) * x77) + (x74 * x372))) + (-1 * ((-1 * x69 * x375) + (x51 * x377)) * x72); const FLT x379 = x365 + x366; const FLT x380 = x362 + dt + x376; const FLT x381 = x373 + (-1 * x374); const FLT x382 = (-1 * x73 * ((-1 * ((x76 * x380) + (x75 * x381)) * x77) + (x74 * x379))) + (-1 * ((-1 * x69 * x380) + (x51 * x381)) * x72); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x78 + (x64 * ((x61 * x56) + (-1 * x6 * x51))) + (x79 * x78)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x87 + (((x83 * x61) + (-1 * x80 * x51)) * x64) + (x87 * x79)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x91 + (((x89 * x61) + (-1 * x88 * x51)) * x64) + (x79 * x91)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x106 + (x64 * ((x61 * x101) + (-1 * x51 * x96))) + (x79 * x106)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x114 + (((x61 * x110) + (-1 * x51 * x107)) * x64) + (x79 * x114)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x118 + (((x61 * x116) + (-1 * x51 * x115)) * x64) + (x79 * x118)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x158 + (((x61 * x156) + (-1 * x51 * x154)) * x64) + (x79 * x158)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x180 + (((x61 * x178) + (-1 * x51 * x176)) * x64) + (x79 * x180)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (((x61 * x194) + (-1 * x51 * x192)) * x64) + x196 + (x79 * x196)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x209 + (((x61 * x207) + (-1 * x51 * x205)) * x64) + (x79 * x209)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x287 + (((x61 * x285) + (-1 * x51 * x283)) * x64) + (x79 * x287)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x327 + (((x61 * x325) + (-1 * x51 * x323)) * x64) + (x79 * x327)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x361 + (((x61 * x359) + (-1 * x51 * x357)) * x64) + (x79 * x361)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x371 + (((x61 * x367) + (-1 * x51 * x364)) * x64) + (x79 * x371)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x378 + (((x61 * x375) + (-1 * x51 * x372)) * x64) + (x79 * x378)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x382 + (((x61 * x380) + (-1 * x51 * x379)) * x64) + (x79 * x382)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen1_jac_x0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * dt; const FLT x1 = x0 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x2 = x0 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x3 = x0 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x4 = 1e-10 + x1 + x2 + x3; const FLT x5 = sqrt(x4); const FLT x6 = 0.5 * x5; const FLT x7 = sin(x6); const FLT x8 = (1. / x4) * (x7 * x7); const FLT x9 = cos(x6); const FLT x10 = 1. / sqrt((x3 * x8) + (x1 * x8) + (x2 * x8) + (x9 * x9)); const FLT x11 = (1. / x5) * x7 * dt * x10; const FLT x12 = x11 * (*_x0).Pose.Rot[0]; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x14 = x11 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x15 = x14 * (*_x0).Pose.Rot[1]; const FLT x16 = x9 * x10; const FLT x17 = x16 * (*_x0).Pose.Rot[3]; const FLT x18 = x11 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = x18 * (*_x0).Pose.Rot[2]; const FLT x20 = x17 + x19 + x13 + (-1 * x15); const FLT x21 = x16 * (*_x0).Pose.Rot[2]; const FLT x22 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x23 = x18 * (*_x0).Pose.Rot[3]; const FLT x24 = x11 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x25 = x24 * (*_x0).Pose.Rot[1]; const FLT x26 = (-1 * x25) + x23 + (-1 * x21) + (-1 * x22); const FLT x27 = (-1 * x23) + x25 + x22 + x21; const FLT x28 = 2 * x27; const FLT x29 = 1 + (x28 * x26) + (-2 * (x20 * x20)); const FLT x30 = (x16 * (*_x0).Pose.Rot[0]) + (-1 * x24 * (*_x0).Pose.Rot[3]) + (-1 * x18 * (*_x0).Pose.Rot[1]) + (-1 * x14 * (*_x0).Pose.Rot[2]); const FLT x31 = 2 * x30; const FLT x32 = x31 * x27; const FLT x33 = x24 * (*_x0).Pose.Rot[2]; const FLT x34 = x14 * (*_x0).Pose.Rot[3]; const FLT x35 = x16 * (*_x0).Pose.Rot[1]; const FLT x36 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x37 = x36 + x35 + (-1 * x33) + x34; const FLT x38 = 2 * x20; const FLT x39 = (x38 * x37) + (-1 * x32); const FLT x40 = 2 * x37; const FLT x41 = x31 * x20; const FLT x42 = x41 + (-1 * x40 * x26); const FLT x43 = (x42 * (*lh_p).Rot[1]) + (-1 * x29 * (*lh_p).Rot[2]) + (x39 * (*lh_p).Rot[0]); const FLT x44 = 2 * (*lh_p).Rot[2]; const FLT x45 = (x29 * (*lh_p).Rot[3]) + (-1 * x39 * (*lh_p).Rot[1]) + (x42 * (*lh_p).Rot[0]); const FLT x46 = 2 * (*lh_p).Rot[3]; const FLT x47 = x29 + (x43 * x44) + (-1 * x45 * x46); const FLT x48 = 1.0/2.0 * dt * fabs(dt); const FLT x49 = (-1 * x20 * sensor_pt[1]) + (x27 * sensor_pt[2]) + (x30 * sensor_pt[0]); const FLT x50 = (-1 * x37 * sensor_pt[2]) + (x20 * sensor_pt[0]) + (x30 * sensor_pt[1]); const FLT x51 = sensor_pt[2] + (x48 * (*_x0).Acc[2]) + (2 * ((x50 * x37) + (-1 * x49 * x27))) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x52 = (-1 * x27 * sensor_pt[0]) + (x30 * sensor_pt[2]) + (x37 * sensor_pt[1]); const FLT x53 = (*_x0).Pose.Pos[0] + sensor_pt[0] + (2 * ((x52 * x27) + (-1 * x50 * x20))) + (dt * (*_x0).Velocity.Pos[0]) + (x48 * (*_x0).Acc[0]); const FLT x54 = (2 * ((x49 * x20) + (-1 * x52 * x37))) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (dt * (*_x0).Velocity.Pos[1]) + (x48 * (*_x0).Acc[1]); const FLT x55 = (-1 * x54 * (*lh_p).Rot[3]) + (x51 * (*lh_p).Rot[2]) + (x53 * (*lh_p).Rot[0]); const FLT x56 = (-1 * x51 * (*lh_p).Rot[1]) + (x54 * (*lh_p).Rot[0]) + (x53 * (*lh_p).Rot[3]); const FLT x57 = (2 * ((x56 * (*lh_p).Rot[1]) + (-1 * x55 * (*lh_p).Rot[2]))) + x51 + (*lh_p).Pos[2]; const FLT x58 = 1. / x57; const FLT x59 = 2 * (*lh_p).Rot[1]; const FLT x60 = (x29 * (*lh_p).Rot[0]) + (-1 * x42 * (*lh_p).Rot[3]) + (x39 * (*lh_p).Rot[2]); const FLT x61 = x39 + (x59 * x45) + (-1 * x60 * x44); const FLT x62 = x57 * x57; const FLT x63 = 1. / x62; const FLT x64 = (-1 * x53 * (*lh_p).Rot[2]) + (x51 * (*lh_p).Rot[0]) + (x54 * (*lh_p).Rot[1]); const FLT x65 = x53 + (2 * ((x64 * (*lh_p).Rot[2]) + (-1 * x56 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x66 = x63 * x65; const FLT x67 = -1 * x57; const FLT x68 = x65 * x65; const FLT x69 = 2 * (1. / (x62 + x68)) * x62 * atan2(x65, x67) * (*bsc0).curve; const FLT x70 = x42 + (x60 * x46) + (-1 * x59 * x43); const FLT x71 = x54 + (*lh_p).Pos[1] + (2 * ((x55 * (*lh_p).Rot[3]) + (-1 * x64 * (*lh_p).Rot[1]))); const FLT x72 = x71 * x63; const FLT x73 = x62 + (x71 * x71); const FLT x74 = 1. / x73; const FLT x75 = x74 * x62; const FLT x76 = 1. / sqrt(1 + (-1 * x74 * x68 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x77 = (1. / sqrt(x73)) * (*bsc0).tilt; const FLT x78 = 2 * x71; const FLT x79 = 2 * x57; const FLT x80 = 1.0/2.0 * (1. / (x73 * sqrt(x73))) * x65 * (*bsc0).tilt; const FLT x81 = (-1 * x76 * ((-1 * ((x79 * x61) + (x70 * x78)) * x80) + (x77 * x47))) + (-1 * ((-1 * x72 * x61) + (x70 * x58)) * x75); const FLT x82 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x77 * x65)) + (-1 * atan2(-1 * x71, x67))); const FLT x83 = (-1 * x17) + x15 + (-1 * x19) + (-1 * x13); const FLT x84 = x31 * x37; const FLT x85 = x84 + (-1 * x83 * x28); const FLT x86 = (x40 * x27) + (-1 * x41); const FLT x87 = 1 + (x83 * x38) + (-2 * (x37 * x37)); const FLT x88 = 2 * ((x87 * (*lh_p).Rot[1]) + (x85 * (*lh_p).Rot[0]) + (-1 * x86 * (*lh_p).Rot[2])); const FLT x89 = 2 * ((x86 * (*lh_p).Rot[3]) + (-1 * x85 * (*lh_p).Rot[1]) + (x87 * (*lh_p).Rot[0])); const FLT x90 = x86 + (x88 * (*lh_p).Rot[2]) + (-1 * x89 * (*lh_p).Rot[3]); const FLT x91 = 2 * ((x86 * (*lh_p).Rot[0]) + (x85 * (*lh_p).Rot[2]) + (-1 * x87 * (*lh_p).Rot[3])); const FLT x92 = x85 + (x89 * (*lh_p).Rot[1]) + (-1 * x91 * (*lh_p).Rot[2]); const FLT x93 = (x91 * (*lh_p).Rot[3]) + x87 + (-1 * x88 * (*lh_p).Rot[1]); const FLT x94 = (-1 * x76 * ((-1 * ((x79 * x92) + (x78 * x93)) * x80) + (x77 * x90))) + (-1 * ((-1 * x72 * x92) + (x58 * x93)) * x75); const FLT x95 = x33 + (-1 * x36) + (-1 * x34) + (-1 * x35); const FLT x96 = x32 + (-1 * x95 * x38); const FLT x97 = 1 + (x95 * x40) + (-2 * (x27 * x27)); const FLT x98 = (x38 * x27) + (-1 * x84); const FLT x99 = (-1 * x96 * (*lh_p).Rot[2]) + (x98 * (*lh_p).Rot[1]) + (x97 * (*lh_p).Rot[0]); const FLT x100 = (x96 * (*lh_p).Rot[3]) + (-1 * x97 * (*lh_p).Rot[1]) + (x98 * (*lh_p).Rot[0]); const FLT x101 = x96 + (x99 * x44) + (-1 * x46 * x100); const FLT x102 = (x96 * (*lh_p).Rot[0]) + (-1 * x98 * (*lh_p).Rot[3]) + (x97 * (*lh_p).Rot[2]); const FLT x103 = x97 + (x59 * x100) + (-1 * x44 * x102); const FLT x104 = (x46 * x102) + x98 + (-1 * x59 * x99); const FLT x105 = (-1 * x76 * ((-1 * ((x79 * x103) + (x78 * x104)) * x80) + (x77 * x101))) + (-1 * ((-1 * x72 * x103) + (x58 * x104)) * x75); cnMatrixOptionalSet(Hx, 0, 0, x81 + (((x61 * x66) + (-1 * x58 * x47)) * x69) + (x81 * x82)); cnMatrixOptionalSet(Hx, 0, 1, (((x66 * x92) + (-1 * x58 * x90)) * x69) + x94 + (x82 * x94)); cnMatrixOptionalSet(Hx, 0, 2, x105 + (((x66 * x103) + (-1 * x58 * x101)) * x69) + (x82 * x105)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen1_jac_sensor_pt(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * (*_x0).Velocity.Pos[2]; const FLT x1 = 1.0/2.0 * dt * fabs(dt); const FLT x2 = x1 * (*_x0).Acc[2]; const FLT x3 = dt * dt; const FLT x4 = x3 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x5 = x3 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x6 = x3 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x7 = 1e-10 + x4 + x5 + x6; const FLT x8 = sqrt(x7); const FLT x9 = 0.5 * x8; const FLT x10 = sin(x9); const FLT x11 = (1. / x7) * (x10 * x10); const FLT x12 = cos(x9); const FLT x13 = 1. / sqrt((x6 * x11) + (x5 * x11) + (x4 * x11) + (x12 * x12)); const FLT x14 = (1. / x8) * dt * x13 * x10; const FLT x15 = x14 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x16 = x13 * x12; const FLT x17 = x14 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x14 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = (-1 * x18 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]); const FLT x20 = (x16 * (*_x0).Pose.Rot[0]) + (-1 * x18 * (*_x0).Pose.Rot[1]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x15 * (*_x0).Pose.Rot[2]); const FLT x21 = (x18 * (*_x0).Pose.Rot[2]) + (x16 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[1]); const FLT x22 = (-1 * x21 * sensor_pt[1]) + (x19 * sensor_pt[2]) + (x20 * sensor_pt[0]); const FLT x23 = (-1 * x17 * (*_x0).Pose.Rot[2]) + (x18 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[3]); const FLT x24 = (x21 * sensor_pt[0]) + (-1 * x23 * sensor_pt[2]) + (x20 * sensor_pt[1]); const FLT x25 = 2 * ((x24 * x23) + (-1 * x22 * x19)); const FLT x26 = x25 + sensor_pt[2] + x2 + x0 + (*_x0).Pose.Pos[2]; const FLT x27 = x26 * (*lh_p).Rot[2]; const FLT x28 = dt * (*_x0).Velocity.Pos[0]; const FLT x29 = x1 * (*_x0).Acc[0]; const FLT x30 = (x20 * sensor_pt[2]) + (-1 * x19 * sensor_pt[0]) + (x23 * sensor_pt[1]); const FLT x31 = 2 * ((x30 * x19) + (-1 * x24 * x21)); const FLT x32 = (*_x0).Pose.Pos[0] + sensor_pt[0] + x28 + x31 + x29; const FLT x33 = x32 * (*lh_p).Rot[0]; const FLT x34 = x1 * (*_x0).Acc[1]; const FLT x35 = dt * (*_x0).Velocity.Pos[1]; const FLT x36 = 2 * ((x22 * x21) + (-1 * x30 * x23)); const FLT x37 = x35 + sensor_pt[1] + x36 + (*_x0).Pose.Pos[1] + x34; const FLT x38 = x37 * (*lh_p).Rot[3]; const FLT x39 = (-1 * x38) + x27 + x33; const FLT x40 = x37 * (*lh_p).Rot[0]; const FLT x41 = x32 * (*lh_p).Rot[3]; const FLT x42 = x26 * (*lh_p).Rot[1]; const FLT x43 = (-1 * x42) + x40 + x41; const FLT x44 = x26 + (2 * ((x43 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x45 = x44 * x44; const FLT x46 = x26 * (*lh_p).Rot[0]; const FLT x47 = x37 * (*lh_p).Rot[1]; const FLT x48 = x32 * (*lh_p).Rot[2]; const FLT x49 = (-1 * x48) + x46 + x47; const FLT x50 = x37 + (*lh_p).Pos[1] + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x49 * (*lh_p).Rot[1]))); const FLT x51 = (x50 * x50) + x45; const FLT x52 = 1. / x51; const FLT x53 = x32 + (2 * ((x49 * (*lh_p).Rot[2]) + (-1 * x43 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x54 = x53 * x53; const FLT x55 = 1. / sqrt(1 + (-1 * x54 * x52 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x56 = (1. / sqrt(x51)) * (*bsc0).tilt; const FLT x57 = x56 * x55; const FLT x58 = 2 * x44; const FLT x59 = -1 * x44; const FLT x60 = (1. / (x45 + x54)) * atan2(x53, x59) * (*bsc0).curve; const FLT x61 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x53 * x56)) + (-1 * atan2(-1 * x50, x59))); const FLT x62 = x53 * (1. / (x51 * sqrt(x51))) * (*bsc0).tilt; const FLT x63 = x62 * x55; const FLT x64 = (x63 * x50) + (-1 * x52 * x44); const FLT x65 = 2 * x60; const FLT x66 = (x63 * x44) + (x50 * x52); const FLT x67 = 2 * x38; const FLT x68 = (2 * x27) + (-1 * x67); const FLT x69 = 1. / x44; const FLT x70 = 2 * x48; const FLT x71 = (2 * x47) + (-1 * x70); const FLT x72 = 1. / x45; const FLT x73 = x72 * x53; const FLT x74 = x65 * x45; const FLT x75 = 2 * x42; const FLT x76 = (2 * x41) + (-1 * x75); const FLT x77 = x72 * x50; const FLT x78 = x52 * x45; const FLT x79 = 2 * x50; const FLT x80 = 1.0/2.0 * x62; const FLT x81 = (-1 * x55 * ((-1 * ((x71 * x58) + (x79 * x76)) * x80) + (x68 * x56))) + (-1 * ((-1 * x71 * x77) + (x76 * x69)) * x78); const FLT x82 = (-1 * sensor_pt[2]) + (-1 * x2) + (-1 * x25) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x0); const FLT x83 = 2 * (*lh_p).Rot[3]; const FLT x84 = 2 * (*lh_p).Rot[2]; const FLT x85 = (x84 * x37) + (-1 * x82 * x83); const FLT x86 = 2 * (*lh_p).Rot[1]; const FLT x87 = 2 * x40; const FLT x88 = x76 + (x82 * x86) + x87; const FLT x89 = 2 * x46; const FLT x90 = (-4 * x47) + (-1 * x89) + x70; const FLT x91 = (-1 * x55 * ((-1 * ((x88 * x58) + (x79 * x90)) * x80) + (x85 * x56))) + (-1 * ((-1 * x88 * x77) + (x69 * x90)) * x78); const FLT x92 = (-1 * (*_x0).Pose.Pos[0]) + (-1 * x28) + (-1 * x31) + (-1 * x29) + (-1 * sensor_pt[0]); const FLT x93 = x71 + x89 + (x84 * x92); const FLT x94 = 2 * x33; const FLT x95 = (-1 * x94) + (-4 * x27) + x67; const FLT x96 = (x83 * x26) + (-1 * x86 * x92); const FLT x97 = (-1 * x55 * ((-1 * ((x58 * x95) + (x79 * x96)) * x80) + (x56 * x93))) + (-1 * ((-1 * x77 * x95) + (x69 * x96)) * x78); const FLT x98 = (-1 * x87) + x75 + (-4 * x41); const FLT x99 = (-1 * x36) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * x34) + (-1 * x35) + (-1 * sensor_pt[1]); const FLT x100 = (x86 * x32) + (-1 * x84 * x99); const FLT x101 = x68 + (x83 * x99) + x94; const FLT x102 = (-1 * x55 * ((-1 * ((x58 * x100) + (x79 * x101)) * x80) + (x56 * x98))) + (-1 * ((-1 * x77 * x100) + (x69 * x101)) * x78); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), (-1 * x61 * x57) + (-1 * x57) + (-1 * x60 * x58)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), x64 + (x64 * x61)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x66 + (x65 * x53) + (x61 * x66)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x81 + (((x71 * x73) + (-1 * x68 * x69)) * x74) + (x81 * x61)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (((x88 * x73) + (-1 * x85 * x69)) * x74) + x91 + (x61 * x91)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x97 + (((x73 * x95) + (-1 * x69 * x93)) * x74) + (x61 * x97)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x74 * ((x73 * x100) + (-1 * x69 * x98))) + x102 + (x61 * x102)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen1_jac_lh_p(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x14 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x15 = x11 * x10; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (-1 * x13 * (*_x0).Pose.Rot[3]) + (-1 * x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (x15 * (*_x0).Pose.Rot[1]) + (x16 * (*_x0).Pose.Rot[0]) + (-1 * x13 * (*_x0).Pose.Rot[2]) + (x14 * (*_x0).Pose.Rot[3]); const FLT x19 = (x13 * (*_x0).Pose.Rot[1]) + (-1 * x16 * (*_x0).Pose.Rot[3]) + (x14 * (*_x0).Pose.Rot[0]) + (x15 * (*_x0).Pose.Rot[2]); const FLT x20 = (-1 * x19 * sensor_pt[0]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x21 = (x16 * (*_x0).Pose.Rot[2]) + (x15 * (*_x0).Pose.Rot[3]) + (x13 * (*_x0).Pose.Rot[0]) + (-1 * x14 * (*_x0).Pose.Rot[1]); const FLT x22 = (-1 * x21 * sensor_pt[1]) + (x19 * sensor_pt[2]) + (x17 * sensor_pt[0]); const FLT x23 = (dt * (*_x0).Velocity.Pos[1]) + (2 * ((x22 * x21) + (-1 * x20 * x18))) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x24 = (-1 * x18 * sensor_pt[2]) + (x21 * sensor_pt[0]) + (x17 * sensor_pt[1]); const FLT x25 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x26 = sensor_pt[2] + (2 * ((x24 * x18) + (-1 * x22 * x19))) + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x27 = (-1 * x26 * (*lh_p).Rot[1]) + (x23 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[3]); const FLT x28 = (-1 * x25 * (*lh_p).Rot[2]) + (x26 * (*lh_p).Rot[0]) + (x23 * (*lh_p).Rot[1]); const FLT x29 = (2 * ((x28 * (*lh_p).Rot[2]) + (-1 * x27 * (*lh_p).Rot[3]))) + x25 + (*lh_p).Pos[0]; const FLT x30 = (-1 * x23 * (*lh_p).Rot[3]) + (x26 * (*lh_p).Rot[2]) + (x25 * (*lh_p).Rot[0]); const FLT x31 = x26 + (2 * ((x27 * (*lh_p).Rot[1]) + (-1 * x30 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x32 = -1 * x31; const FLT x33 = x23 + (*lh_p).Pos[1] + (2 * ((x30 * (*lh_p).Rot[3]) + (-1 * x28 * (*lh_p).Rot[1]))); const FLT x34 = (x33 * x33) + (x31 * x31); const FLT x35 = (1. / sqrt(x34)) * x29; const FLT x36 = 1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x35 * (*bsc0).tilt)) + (-1 * atan2(-1 * x33, x32)); const FLT x37 = sin(x36) * (*bsc0).gibmag; const FLT x38 = x35 * (1. / sqrt(1 + (-1 * (1. / x34) * (x29 * x29) * ((*bsc0).tilt * (*bsc0).tilt)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), atan2(x29, x32) * atan2(x29, x32)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * cos(x36)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x37); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1 + (-1 * x37)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x38 * x37) + (-1 * x38)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_y_gen1_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen1(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen1_jac_bsc0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanModel_LightMeas_x_gen2(const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x11 * x10; const FLT x15 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[1]) + (x13 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x14 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]) + (-1 * x13 * (*_x0).Pose.Rot[1]); const FLT x20 = (-1 * x19 * sensor_pt[1]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[0]); const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[1]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (x13 * (*_x0).Pose.Rot[3]); const FLT x22 = (x19 * sensor_pt[0]) + (-1 * x21 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x23 = (2 * ((x22 * x21) + (-1 * x20 * x17))) + sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x24 = (-1 * x17 * sensor_pt[0]) + (x18 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x25 = (*_x0).Pose.Pos[0] + (2 * ((x24 * x17) + (-1 * x22 * x19))) + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x26 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x27 = (-1 * x26 * (*lh_p).Rot[3]) + (x23 * (*lh_p).Rot[2]) + (x25 * (*lh_p).Rot[0]); const FLT x28 = (-1 * x23 * (*lh_p).Rot[1]) + (x26 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[3]); const FLT x29 = x23 + (2 * ((x28 * (*lh_p).Rot[1]) + (-1 * x27 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x30 = (-1 * x25 * (*lh_p).Rot[2]) + (x23 * (*lh_p).Rot[0]) + (x26 * (*lh_p).Rot[1]); const FLT x31 = (2 * ((x30 * (*lh_p).Rot[2]) + (-1 * x28 * (*lh_p).Rot[3]))) + x25 + (*lh_p).Pos[0]; const FLT x32 = atan2(-1 * x29, x31); const FLT x33 = (*lh_p).Pos[1] + x26 + (2 * ((x27 * (*lh_p).Rot[3]) + (-1 * x30 * (*lh_p).Rot[1]))); const FLT x34 = (x31 * x31) + (x29 * x29); const FLT x35 = 0.523598775598299 + (*bsc0).tilt; const FLT x36 = cos(x35); const FLT x37 = asin(x33 * (1. / x36) * (1. / sqrt(x34 + (x33 * x33)))); const FLT x38 = (1. / sqrt(x34)) * x33 * tan(x35); const FLT x39 = ((*bsc0).ogeemag * sin(x32 + (-1 * asin(x38)) + (*bsc0).ogeephase)) + (*bsc0).curve; const FLT x40 = 0.0028679863 + (x37 * (-8.0108022e-06 + (-8.0108022e-06 * x37))); const FLT x41 = 5.3685255e-06 + (x40 * x37); const FLT x42 = 0.0076069798 + (x41 * x37); const FLT x43 = (-1 * asin(x38 + (x42 * (x37 * x37) * x39 * (1. / (x36 + (-1 * x39 * sin(x35) * ((x37 * (x42 + (x37 * (x41 + (x37 * (x40 + (x37 * (-8.0108022e-06 + (-1.60216044e-05 * x37))))))))) + (x42 * x37)))))))) + x32; return -1.5707963267949 + x43 + (-1 * (*bsc0).phase) + (sin(x43 + (*bsc0).gibpha) * (*bsc0).gibmag); } // Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = x0 * (*lh_p).Rot[1] * (*lh_p).Rot[2]; const FLT x2 = x0 * (*lh_p).Rot[3]; const FLT x3 = x2 * (*lh_p).Rot[0]; const FLT x4 = x3 + x1; const FLT x5 = 1.0/2.0 * x0; const FLT x6 = dt * dt; const FLT x7 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x8 = x6 * x7; const FLT x9 = (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x10 = x6 * x9; const FLT x11 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x12 = x6 * x11; const FLT x13 = 1e-10 + x8 + x10 + x12; const FLT x14 = sqrt(x13); const FLT x15 = 0.5 * x14; const FLT x16 = sin(x15); const FLT x17 = x16 * x16; const FLT x18 = 1. / x13; const FLT x19 = x18 * x17; const FLT x20 = cos(x15); const FLT x21 = (x10 * x19) + (x8 * x19) + (x12 * x19) + (x20 * x20); const FLT x22 = 1. / sqrt(x21); const FLT x23 = x22 * x16; const FLT x24 = 1. / x14; const FLT x25 = dt * x24; const FLT x26 = x25 * x23; const FLT x27 = x26 * (*_x0).Pose.Rot[0]; const FLT x28 = x22 * x20; const FLT x29 = x28 * (*_x0).Pose.Rot[2]; const FLT x30 = x26 * (*_x0).Pose.Rot[1]; const FLT x31 = x23 * (*_x0).Pose.Rot[3]; const FLT x32 = x31 * x25; const FLT x33 = (-1 * x32 * (*_x0).Velocity.AxisAngleRot[0]) + (x30 * (*_x0).Velocity.AxisAngleRot[2]) + (x27 * (*_x0).Velocity.AxisAngleRot[1]) + x29; const FLT x34 = x26 * (*_x0).Pose.Rot[2]; const FLT x35 = x28 * (*_x0).Pose.Rot[0]; const FLT x36 = (-1 * x30 * (*_x0).Velocity.AxisAngleRot[0]) + x35 + (-1 * x32 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x34 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x37 = x28 * (*_x0).Pose.Rot[3]; const FLT x38 = (x34 * (*_x0).Velocity.AxisAngleRot[0]) + x37 + (x27 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x30 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x39 = (-1 * x38 * sensor_pt[1]) + (x33 * sensor_pt[2]) + (x36 * sensor_pt[0]); const FLT x40 = x28 * (*_x0).Pose.Rot[1]; const FLT x41 = (x27 * (*_x0).Velocity.AxisAngleRot[0]) + x40 + (-1 * x34 * (*_x0).Velocity.AxisAngleRot[2]) + (x32 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x42 = (-1 * x41 * sensor_pt[2]) + (x38 * sensor_pt[0]) + (x36 * sensor_pt[1]); const FLT x43 = (2 * ((x41 * x42) + (-1 * x33 * x39))) + sensor_pt[2] + (x5 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x44 = (-1 * x33 * sensor_pt[0]) + (x36 * sensor_pt[2]) + (x41 * sensor_pt[1]); const FLT x45 = (dt * (*_x0).Velocity.Pos[1]) + (2 * ((x38 * x39) + (-1 * x41 * x44))) + (*_x0).Pose.Pos[1] + sensor_pt[1] + (x5 * (*_x0).Acc[1]); const FLT x46 = (2 * ((x44 * x33) + (-1 * x42 * x38))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x5 * (*_x0).Acc[0]); const FLT x47 = (-1 * x46 * (*lh_p).Rot[2]) + (x43 * (*lh_p).Rot[0]) + (x45 * (*lh_p).Rot[1]); const FLT x48 = (-1 * x45 * (*lh_p).Rot[3]) + (x43 * (*lh_p).Rot[2]) + (x46 * (*lh_p).Rot[0]); const FLT x49 = x45 + (*lh_p).Pos[1] + (2 * ((x48 * (*lh_p).Rot[3]) + (-1 * x47 * (*lh_p).Rot[1]))); const FLT x50 = x49 * x49; const FLT x51 = (x45 * (*lh_p).Rot[0]) + (-1 * x43 * (*lh_p).Rot[1]) + (x46 * (*lh_p).Rot[3]); const FLT x52 = (2 * ((x51 * (*lh_p).Rot[1]) + (-1 * x48 * (*lh_p).Rot[2]))) + x43 + (*lh_p).Pos[2]; const FLT x53 = x46 + (2 * ((x47 * (*lh_p).Rot[2]) + (-1 * x51 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x54 = x53 * x53; const FLT x55 = x54 + (x52 * x52); const FLT x56 = x55 + x50; const FLT x57 = 0.523598775598299 + (*bsc0).tilt; const FLT x58 = cos(x57); const FLT x59 = 1. / x58; const FLT x60 = (1. / sqrt(x56)) * x59; const FLT x61 = 2 * x49; const FLT x62 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x63 = -1 * x0 * x62; const FLT x64 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x65 = x5 + (-1 * x0 * x64); const FLT x66 = x65 + x63; const FLT x67 = 2 * x53; const FLT x68 = x0 * (*lh_p).Rot[0]; const FLT x69 = x68 * (*lh_p).Rot[2]; const FLT x70 = x2 * (*lh_p).Rot[1]; const FLT x71 = x70 + (-1 * x69); const FLT x72 = 2 * x52; const FLT x73 = (x71 * x72) + (x67 * x66); const FLT x74 = 1.0/2.0 * x49; const FLT x75 = x74 * (1. / (x56 * sqrt(x56))) * x59; const FLT x76 = (-1 * x75 * (x73 + (x4 * x61))) + (x4 * x60); const FLT x77 = 1. / sqrt(1 + (-1 * x50 * (1. / (x58 * x58)) * (1. / x56))); const FLT x78 = atan2(-1 * x52, x53); const FLT x79 = tan(x57); const FLT x80 = x79 * (1. / sqrt(x55)); const FLT x81 = x80 * x49; const FLT x82 = (-1 * asin(x81)) + x78 + (*bsc0).ogeephase; const FLT x83 = (sin(x82) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x84 = asin(x60 * x49); const FLT x85 = 8.0108022e-06 * x84; const FLT x86 = -8.0108022e-06 + (-1 * x85); const FLT x87 = 0.0028679863 + (x84 * x86); const FLT x88 = 5.3685255e-06 + (x84 * x87); const FLT x89 = 0.0076069798 + (x88 * x84); const FLT x90 = x89 * x84; const FLT x91 = -8.0108022e-06 + (-1.60216044e-05 * x84); const FLT x92 = x87 + (x84 * x91); const FLT x93 = x88 + (x84 * x92); const FLT x94 = x89 + (x84 * x93); const FLT x95 = (x84 * x94) + x90; const FLT x96 = sin(x57); const FLT x97 = x83 * x96; const FLT x98 = x58 + (-1 * x97 * x95); const FLT x99 = 1. / x98; const FLT x100 = 2 * x83 * x90 * x99; const FLT x101 = x77 * x100; const FLT x102 = x87 * x77; const FLT x103 = x86 * x77; const FLT x104 = x76 * x103; const FLT x105 = x85 * x77; const FLT x106 = (x84 * ((-1 * x76 * x105) + x104)) + (x76 * x102); const FLT x107 = x88 * x77; const FLT x108 = (x76 * x107) + (x84 * x106); const FLT x109 = x84 * x84; const FLT x110 = x83 * x109; const FLT x111 = x99 * x110; const FLT x112 = 1. / x55; const FLT x113 = 1. / sqrt(1 + (-1 * (x79 * x79) * x50 * x112)); const FLT x114 = x79 * x74 * (1. / (x55 * sqrt(x55))); const FLT x115 = (x4 * x80) + (-1 * x73 * x114); const FLT x116 = (1. / x54) * x52; const FLT x117 = 1. / x53; const FLT x118 = x54 * x112; const FLT x119 = ((-1 * x71 * x117) + (x66 * x116)) * x118; const FLT x120 = x119 + (-1 * x113 * x115); const FLT x121 = cos(x82) * (*bsc0).ogeemag; const FLT x122 = x96 * x95; const FLT x123 = x122 * x121; const FLT x124 = x77 * x94; const FLT x125 = 2.40324066e-05 * x84; const FLT x126 = x77 * x125; const FLT x127 = x77 * x91; const FLT x128 = x77 * x92; const FLT x129 = x77 * x93; const FLT x130 = x89 * x77; const FLT x131 = x89 * (1. / (x98 * x98)) * x110; const FLT x132 = x89 * x99 * x109; const FLT x133 = x121 * x132; const FLT x134 = x81 + (x89 * x111); const FLT x135 = 1. / sqrt(1 + (-1 * (x134 * x134))); const FLT x136 = x119 + (-1 * x135 * (x115 + (-1 * x131 * ((-1 * x97 * ((x76 * x130) + (x84 * x108) + (x76 * x124) + (x84 * (x108 + (x84 * (x106 + (x84 * ((x76 * x127) + x104 + (-1 * x76 * x126))) + (x76 * x128))) + (x76 * x129))))) + (-1 * x120 * x123))) + (x76 * x101) + (x120 * x133) + (x108 * x111))); const FLT x137 = cos((-1 * asin(x134)) + (*bsc0).gibpha + x78) * (*bsc0).gibmag; const FLT x138 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x139 = -1 * x0 * x138; const FLT x140 = x65 + x139; const FLT x141 = x1 + (-1 * x3); const FLT x142 = x2 * (*lh_p).Rot[2]; const FLT x143 = x68 * (*lh_p).Rot[1]; const FLT x144 = x143 + x142; const FLT x145 = (x72 * x144) + (x67 * x141); const FLT x146 = (-1 * x75 * (x145 + (x61 * x140))) + (x60 * x140); const FLT x147 = x103 * x146; const FLT x148 = (x84 * ((-1 * x105 * x146) + x147)) + (x102 * x146); const FLT x149 = (x107 * x146) + (x84 * x148); const FLT x150 = (x80 * x140) + (-1 * x114 * x145); const FLT x151 = ((-1 * x117 * x144) + (x116 * x141)) * x118; const FLT x152 = x121 * (x151 + (-1 * x113 * x150)); const FLT x153 = x151 + (-1 * x135 * ((x132 * x152) + (x101 * x146) + x150 + (x111 * x149) + (-1 * x131 * ((-1 * x97 * ((x130 * x146) + (x84 * x149) + (x124 * x146) + (x84 * (x149 + (x84 * (x148 + (x84 * ((x127 * x146) + x147 + (-1 * x126 * x146))) + (x128 * x146))) + (x129 * x146))))) + (-1 * x122 * x152))))); const FLT x154 = x69 + x70; const FLT x155 = x63 + x5 + x139; const FLT x156 = (x72 * x155) + (x67 * x154); const FLT x157 = x142 + (-1 * x143); const FLT x158 = (x80 * x157) + (-1 * x114 * x156); const FLT x159 = ((-1 * x117 * x155) + (x116 * x154)) * x118; const FLT x160 = x159 + (-1 * x113 * x158); const FLT x161 = (-1 * x75 * (x156 + (x61 * x157))) + (x60 * x157); const FLT x162 = x103 * x161; const FLT x163 = (x84 * ((-1 * x105 * x161) + x162)) + (x102 * x161); const FLT x164 = (x107 * x161) + (x84 * x163); const FLT x165 = x159 + (-1 * x135 * ((x160 * x133) + (x101 * x161) + (-1 * x131 * ((-1 * x97 * ((x161 * x130) + (x124 * x161) + (x84 * (x164 + (x84 * (x163 + (x84 * ((x127 * x161) + x162 + (-1 * x126 * x161))) + (x128 * x161))) + (x129 * x161))) + (x84 * x164))) + (-1 * x123 * x160))) + x158 + (x111 * x164))); const FLT x166 = 2 * (*lh_p).Rot[1]; const FLT x167 = x166 * (*lh_p).Rot[2]; const FLT x168 = 2 * (*lh_p).Rot[3]; const FLT x169 = x168 * (*lh_p).Rot[0]; const FLT x170 = x169 + x167; const FLT x171 = 2 * x62; const FLT x172 = -1 * x171; const FLT x173 = 2 * x64; const FLT x174 = 1 + (-1 * x173); const FLT x175 = x174 + x172; const FLT x176 = 2 * (*lh_p).Rot[2]; const FLT x177 = x176 * (*lh_p).Rot[0]; const FLT x178 = x166 * (*lh_p).Rot[3]; const FLT x179 = x178 + (-1 * x177); const FLT x180 = (x72 * x179) + (x67 * x175); const FLT x181 = (-1 * x75 * (x180 + (x61 * x170))) + (x60 * x170); const FLT x182 = x103 * x181; const FLT x183 = (x84 * ((-1 * x105 * x181) + x182)) + (x102 * x181); const FLT x184 = (x107 * x181) + (x84 * x183); const FLT x185 = (x80 * x170) + (-1 * x114 * x180); const FLT x186 = ((-1 * x117 * x179) + (x116 * x175)) * x118; const FLT x187 = x186 + (-1 * x113 * x185); const FLT x188 = x186 + (-1 * x135 * (x185 + (x187 * x133) + (-1 * x131 * ((-1 * x97 * ((x181 * x130) + (x84 * x184) + (x124 * x181) + (x84 * ((x84 * (x183 + (x84 * ((x127 * x181) + (-1 * x126 * x181) + x182)) + (x128 * x181))) + x184 + (x129 * x181))))) + (-1 * x123 * x187))) + (x101 * x181) + (x111 * x184))); const FLT x189 = 2 * x138; const FLT x190 = -1 * x189; const FLT x191 = x174 + x190; const FLT x192 = x167 + (-1 * x169); const FLT x193 = x176 * (*lh_p).Rot[3]; const FLT x194 = x166 * (*lh_p).Rot[0]; const FLT x195 = x194 + x193; const FLT x196 = (x72 * x195) + (x67 * x192); const FLT x197 = (-1 * x75 * (x196 + (x61 * x191))) + (x60 * x191); const FLT x198 = (x80 * x191) + (-1 * x114 * x196); const FLT x199 = ((-1 * x117 * x195) + (x116 * x192)) * x118; const FLT x200 = x199 + (-1 * x113 * x198); const FLT x201 = x103 * x197; const FLT x202 = (x84 * ((-1 * x105 * x197) + x201)) + (x102 * x197); const FLT x203 = (x107 * x197) + (x84 * x202); const FLT x204 = x199 + (-1 * x135 * (x198 + (x203 * x111) + (x200 * x133) + (x101 * x197) + (-1 * x131 * ((-1 * x97 * ((x197 * x130) + (x124 * x197) + (x84 * (x203 + (x84 * (x202 + (x84 * ((x127 * x197) + x201 + (-1 * x126 * x197))) + (x128 * x197))) + (x129 * x197))) + (x84 * x203))) + (-1 * x200 * x123))))); const FLT x205 = x193 + (-1 * x194); const FLT x206 = x177 + x178; const FLT x207 = 1 + x190 + x172; const FLT x208 = (x72 * x207) + (x67 * x206); const FLT x209 = (-1 * x75 * (x208 + (x61 * x205))) + (x60 * x205); const FLT x210 = x209 * x103; const FLT x211 = (x84 * ((-1 * x209 * x105) + x210)) + (x209 * x102); const FLT x212 = (x209 * x107) + (x84 * x211); const FLT x213 = (x80 * x205) + (-1 * x208 * x114); const FLT x214 = ((-1 * x207 * x117) + (x206 * x116)) * x118; const FLT x215 = x214 + (-1 * x213 * x113); const FLT x216 = x214 + (-1 * x135 * (x213 + (x215 * x133) + (x209 * x101) + (x212 * x111) + (-1 * x131 * ((-1 * x97 * ((x209 * x130) + (x209 * x124) + (x84 * (x212 + (x84 * (x211 + (x84 * ((x209 * x127) + x210 + (-1 * x209 * x126))) + (x209 * x128))) + (x209 * x129))) + (x84 * x212))) + (-1 * x215 * x123))))); const FLT x217 = x26 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x218 = x217 * sensor_pt[2]; const FLT x219 = x28 * sensor_pt[0]; const FLT x220 = x26 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x221 = -1 * x220 * sensor_pt[1]; const FLT x222 = x221 + x219; const FLT x223 = x222 + x218; const FLT x224 = 2 * x33; const FLT x225 = 2 * x42; const FLT x226 = x26 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x227 = x225 * x226; const FLT x228 = 2 * x39; const FLT x229 = -1 * x217 * x228; const FLT x230 = -1 * x226 * sensor_pt[2]; const FLT x231 = x220 * sensor_pt[0]; const FLT x232 = x28 * sensor_pt[1]; const FLT x233 = x232 + x230 + x231; const FLT x234 = 2 * x41; const FLT x235 = x229 + (x234 * x233) + (-1 * x223 * x224) + x227; const FLT x236 = x226 * sensor_pt[1]; const FLT x237 = -1 * x217 * sensor_pt[0]; const FLT x238 = x28 * sensor_pt[2]; const FLT x239 = x238 + x237; const FLT x240 = x239 + x236; const FLT x241 = 2 * x38; const FLT x242 = x220 * x228; const FLT x243 = 2 * x44; const FLT x244 = -1 * x226 * x243; const FLT x245 = x244 + x242 + (-1 * x234 * x240) + (x223 * x241); const FLT x246 = -1 * x220 * x225; const FLT x247 = x217 * x243; const FLT x248 = x247 + (x224 * x240) + (-1 * x233 * x241) + x246; const FLT x249 = (x248 * (*lh_p).Rot[3]) + (-1 * x235 * (*lh_p).Rot[1]) + (x245 * (*lh_p).Rot[0]); const FLT x250 = (x245 * (*lh_p).Rot[1]) + (-1 * x248 * (*lh_p).Rot[2]) + (x235 * (*lh_p).Rot[0]); const FLT x251 = x248 + (-1 * x249 * x168) + (x250 * x176); const FLT x252 = (x248 * (*lh_p).Rot[0]) + (-1 * x245 * (*lh_p).Rot[3]) + (x235 * (*lh_p).Rot[2]); const FLT x253 = (-1 * x252 * x176) + x235 + (x249 * x166); const FLT x254 = (x72 * x253) + (x67 * x251); const FLT x255 = x245 + (-1 * x250 * x166) + (x252 * x168); const FLT x256 = (x80 * x255) + (-1 * x254 * x114); const FLT x257 = ((-1 * x253 * x117) + (x251 * x116)) * x118; const FLT x258 = x257 + (-1 * x256 * x113); const FLT x259 = (-1 * x75 * (x254 + (x61 * x255))) + (x60 * x255); const FLT x260 = x259 * x103; const FLT x261 = (x84 * ((-1 * x259 * x105) + x260)) + (x259 * x102); const FLT x262 = (x259 * x107) + (x84 * x261); const FLT x263 = x257 + (-1 * x135 * ((x258 * x133) + x256 + (x262 * x111) + (-1 * x131 * ((-1 * x97 * ((x259 * x130) + (x84 * x262) + (x259 * x124) + (x84 * (x262 + (x84 * ((x84 * ((x259 * x127) + (-1 * x259 * x126) + x260)) + x261 + (x259 * x128))) + (x259 * x129))))) + (-1 * x258 * x123))) + (x259 * x101))); const FLT x264 = x217 * sensor_pt[1]; const FLT x265 = x220 * sensor_pt[2]; const FLT x266 = x226 * sensor_pt[0]; const FLT x267 = (-1 * x266) + x264 + x265; const FLT x268 = -1 * x236; const FLT x269 = x268 + (-1 * x238) + x237; const FLT x270 = x28 * x225; const FLT x271 = (-1 * x242) + x270 + (-1 * x267 * x224) + (x234 * x269); const FLT x272 = (-1 * x231) + x230; const FLT x273 = x272 + x232; const FLT x274 = x28 * x243; const FLT x275 = x229 + (-1 * x274) + (-1 * x234 * x273) + (x267 * x241); const FLT x276 = x220 * x243; const FLT x277 = x217 * x225; const FLT x278 = (x273 * x224) + x277 + x276 + (-1 * x269 * x241); const FLT x279 = (x278 * (*lh_p).Rot[3]) + (-1 * x271 * (*lh_p).Rot[1]) + (x275 * (*lh_p).Rot[0]); const FLT x280 = (-1 * x278 * (*lh_p).Rot[2]) + (x275 * (*lh_p).Rot[1]) + (x271 * (*lh_p).Rot[0]); const FLT x281 = x278 + (-1 * x279 * x168) + (x280 * x176); const FLT x282 = (-1 * x275 * (*lh_p).Rot[3]) + (x278 * (*lh_p).Rot[0]) + (x271 * (*lh_p).Rot[2]); const FLT x283 = x271 + (-1 * x282 * x176) + (x279 * x166); const FLT x284 = (x72 * x283) + (x67 * x281); const FLT x285 = x275 + (-1 * x280 * x166) + (x282 * x168); const FLT x286 = (x80 * x285) + (-1 * x284 * x114); const FLT x287 = ((-1 * x283 * x117) + (x281 * x116)) * x118; const FLT x288 = x287 + (-1 * x286 * x113); const FLT x289 = (-1 * x75 * (x284 + (x61 * x285))) + (x60 * x285); const FLT x290 = x289 * x103; const FLT x291 = (x84 * ((-1 * x289 * x105) + x290)) + (x289 * x102); const FLT x292 = (x289 * x107) + (x84 * x291); const FLT x293 = x77 * x289; const FLT x294 = x287 + (-1 * x135 * (x286 + (x288 * x133) + (x293 * x100) + (-1 * x131 * ((-1 * x97 * ((x289 * x130) + (x84 * (x292 + (x84 * (x291 + (x84 * (x290 + (x289 * x127) + (-1 * x293 * x125))) + (x289 * x128))) + (x289 * x129))) + (x289 * x124) + (x84 * x292))) + (-1 * x288 * x123))) + (x292 * x111))); const FLT x295 = x239 + x268; const FLT x296 = x28 * x228; const FLT x297 = (-1 * x264) + x265 + x266; const FLT x298 = x246 + (x234 * x297) + (-1 * x295 * x224) + (-1 * x296); const FLT x299 = -1 * x218; const FLT x300 = x221 + (-1 * x219) + x299; const FLT x301 = (x224 * x300) + x274 + (-1 * x227) + (-1 * x297 * x241); const FLT x302 = x228 * x226; const FLT x303 = x302 + (x295 * x241) + (-1 * x234 * x300) + x276; const FLT x304 = (x303 * (*lh_p).Rot[1]) + (x298 * (*lh_p).Rot[0]) + (-1 * x301 * (*lh_p).Rot[2]); const FLT x305 = (x301 * (*lh_p).Rot[0]) + (-1 * x303 * (*lh_p).Rot[3]) + (x298 * (*lh_p).Rot[2]); const FLT x306 = x303 + (-1 * x304 * x166) + (x305 * x168); const FLT x307 = (x301 * (*lh_p).Rot[3]) + (x303 * (*lh_p).Rot[0]) + (-1 * x298 * (*lh_p).Rot[1]); const FLT x308 = (-1 * x307 * x168) + x301 + (x304 * x176); const FLT x309 = x298 + (-1 * x305 * x176) + (x307 * x166); const FLT x310 = (x72 * x309) + (x67 * x308); const FLT x311 = x77 * ((-1 * x75 * (x310 + (x61 * x306))) + (x60 * x306)); const FLT x312 = x86 * x311; const FLT x313 = (x84 * ((-1 * x85 * x311) + x312)) + (x87 * x311); const FLT x314 = (x88 * x311) + (x84 * x313); const FLT x315 = (x80 * x306) + (-1 * x310 * x114); const FLT x316 = ((-1 * x309 * x117) + (x308 * x116)) * x118; const FLT x317 = x316 + (-1 * x315 * x113); const FLT x318 = x316 + (-1 * x135 * (x315 + (-1 * x131 * ((-1 * x97 * ((x89 * x311) + (x84 * x314) + (x94 * x311) + (x84 * ((x84 * (x313 + (x84 * ((x91 * x311) + x312 + (-1 * x311 * x125))) + (x92 * x311))) + x314 + (x93 * x311))))) + (-1 * x317 * x123))) + (x314 * x111) + (x317 * x133) + (x311 * x100))); const FLT x319 = x272 + (-1 * x232); const FLT x320 = x222 + x299; const FLT x321 = (x234 * x320) + x277 + (-1 * x224 * x319) + x302; const FLT x322 = x264 + x266 + (-1 * x265); const FLT x323 = (-1 * x247) + x296 + (-1 * x234 * x322) + (x241 * x319); const FLT x324 = x244 + (-1 * x270) + (x224 * x322) + (-1 * x241 * x320); const FLT x325 = (x324 * (*lh_p).Rot[3]) + (-1 * x321 * (*lh_p).Rot[1]) + (x323 * (*lh_p).Rot[0]); const FLT x326 = (x323 * (*lh_p).Rot[1]) + (-1 * x324 * (*lh_p).Rot[2]) + (x321 * (*lh_p).Rot[0]); const FLT x327 = x324 + (-1 * x325 * x168) + (x326 * x176); const FLT x328 = (x324 * (*lh_p).Rot[0]) + (-1 * x323 * (*lh_p).Rot[3]) + (x321 * (*lh_p).Rot[2]); const FLT x329 = x321 + (-1 * x328 * x176) + (x325 * x166); const FLT x330 = (x72 * x329) + (x67 * x327); const FLT x331 = (x328 * x168) + x323 + (-1 * x326 * x166); const FLT x332 = (x80 * x331) + (-1 * x330 * x114); const FLT x333 = ((-1 * x329 * x117) + (x327 * x116)) * x118; const FLT x334 = x333 + (-1 * x332 * x113); const FLT x335 = (-1 * x75 * (x330 + (x61 * x331))) + (x60 * x331); const FLT x336 = x335 * x103; const FLT x337 = (x84 * ((-1 * x335 * x105) + x336)) + (x335 * x102); const FLT x338 = (x335 * x107) + (x84 * x337); const FLT x339 = x333 + (-1 * x135 * ((x334 * x133) + x332 + (x338 * x111) + (-1 * x131 * ((-1 * x97 * ((x84 * (x338 + (x84 * (x337 + (x84 * ((x335 * x127) + x336 + (-1 * x335 * x126))) + (x335 * x128))) + (x335 * x129))) + (x335 * x130) + (x84 * x338) + (x335 * x124))) + (-1 * x334 * x123))) + (x335 * x101))); const FLT x340 = -1 * x32; const FLT x341 = x23 * (*_x0).Pose.Rot[2]; const FLT x342 = x6 * x24; const FLT x343 = x342 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x344 = 0.5 * x343; const FLT x345 = dt * dt * dt; const FLT x346 = 0.5 * x18 * x345; const FLT x347 = x11 * x346; const FLT x348 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x349 = dt * dt * dt * dt; const FLT x350 = 2 * (1. / (x13 * x13)) * x17; const FLT x351 = x350 * x349; const FLT x352 = x7 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x353 = 1. / (x13 * sqrt(x13)); const FLT x354 = 1.0 * x20 * x16; const FLT x355 = x354 * x353; const FLT x356 = x355 * x349; const FLT x357 = 2 * x19; const FLT x358 = x6 * x357; const FLT x359 = x9 * x351; const FLT x360 = (-1 * x359 * (*_x0).Velocity.AxisAngleRot[0]) + (x9 * x356 * (*_x0).Velocity.AxisAngleRot[0]) + (x358 * (*_x0).Velocity.AxisAngleRot[0]) + (x352 * x356) + (-1 * x351 * x348) + (-1 * x354 * x343) + (-1 * x352 * x351) + (x356 * x348); const FLT x361 = 1.0/2.0 * (1. / (x21 * sqrt(x21))); const FLT x362 = x25 * x16 * x361; const FLT x363 = x362 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x364 = x363 * x360; const FLT x365 = x20 * x361; const FLT x366 = x365 * x360; const FLT x367 = x362 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x368 = x367 * x360; const FLT x369 = x23 * (*_x0).Pose.Rot[1]; const FLT x370 = x353 * x345; const FLT x371 = x370 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x372 = x371 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x373 = x372 * x369; const FLT x374 = x346 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x375 = x374 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x376 = x40 * x375; const FLT x377 = x362 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x378 = x377 * x360; const FLT x379 = x11 * x370; const FLT x380 = x374 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x381 = x23 * (*_x0).Pose.Rot[0]; const FLT x382 = x381 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x383 = x370 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x384 = (-1 * x383 * x382) + (x35 * x380); const FLT x385 = x384 + (x31 * x379) + x376 + (-1 * x373) + (-1 * x344 * x341) + (-1 * x37 * x347) + (-1 * x378 * (*_x0).Pose.Rot[1]) + (-1 * x368 * (*_x0).Pose.Rot[0]) + (x364 * (*_x0).Pose.Rot[3]) + x340 + (-1 * x366 * (*_x0).Pose.Rot[2]); const FLT x386 = x37 * x380; const FLT x387 = x31 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x388 = x383 * x387; const FLT x389 = x29 * x375; const FLT x390 = x372 * x341; const FLT x391 = x377 * (*_x0).Pose.Rot[2]; const FLT x392 = (-1 * x364 * (*_x0).Pose.Rot[0]) + (-1 * x366 * (*_x0).Pose.Rot[1]) + (-1 * x379 * x381) + x27 + x386 + (x391 * x360) + (-1 * x389) + (-1 * x388) + x390 + (-1 * x369 * x344) + (-1 * x368 * (*_x0).Pose.Rot[3]) + (x35 * x347); const FLT x393 = x363 * (*_x0).Pose.Rot[2]; const FLT x394 = x369 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x395 = (-1 * x40 * x380) + (x394 * x383); const FLT x396 = (-1 * x372 * x381) + (x35 * x375); const FLT x397 = x396 + (-1 * x393 * x360) + x395 + (-1 * x31 * x344) + x34 + (-1 * x378 * (*_x0).Pose.Rot[0]) + (-1 * x379 * x341) + (x29 * x347) + (x368 * (*_x0).Pose.Rot[1]) + (-1 * x366 * (*_x0).Pose.Rot[3]); const FLT x398 = x29 * x380; const FLT x399 = x341 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x400 = x399 * x383; const FLT x401 = -1 * x30; const FLT x402 = (-1 * x37 * x375) + (x31 * x372); const FLT x403 = (x379 * x369) + x401 + (-1 * x40 * x347) + (-1 * x366 * (*_x0).Pose.Rot[0]) + (-1 * x381 * x344) + (x368 * (*_x0).Pose.Rot[2]) + x402 + (x378 * (*_x0).Pose.Rot[3]) + (-1 * x398) + (x364 * (*_x0).Pose.Rot[1]) + x400; const FLT x404 = (-1 * x392 * sensor_pt[2]) + (x403 * sensor_pt[1]) + (x397 * sensor_pt[0]); const FLT x405 = (x392 * sensor_pt[1]) + (-1 * x385 * sensor_pt[0]) + (x403 * sensor_pt[2]); const FLT x406 = (x405 * x224) + (-1 * x225 * x397) + (x243 * x385) + (-1 * x404 * x241); const FLT x407 = 2 * ((x403 * sensor_pt[0]) + (-1 * x397 * sensor_pt[1]) + (x385 * sensor_pt[2])); const FLT x408 = (x404 * x234) + (-1 * x33 * x407) + (x225 * x392) + (-1 * x228 * x385); const FLT x409 = (x38 * x407) + (-1 * x243 * x392) + (-1 * x405 * x234) + (x228 * x397); const FLT x410 = (x409 * (*lh_p).Rot[1]) + (-1 * x406 * (*lh_p).Rot[2]) + (x408 * (*lh_p).Rot[0]); const FLT x411 = (x406 * (*lh_p).Rot[0]) + (x408 * (*lh_p).Rot[2]) + (-1 * x409 * (*lh_p).Rot[3]); const FLT x412 = x409 + (-1 * x410 * x166) + (x411 * x168); const FLT x413 = (x406 * (*lh_p).Rot[3]) + (-1 * x408 * (*lh_p).Rot[1]) + (x409 * (*lh_p).Rot[0]); const FLT x414 = x406 + (-1 * x413 * x168) + (x410 * x176); const FLT x415 = x408 + (-1 * x411 * x176) + (x413 * x166); const FLT x416 = (x72 * x415) + (x67 * x414); const FLT x417 = (-1 * x75 * (x416 + (x61 * x412))) + (x60 * x412); const FLT x418 = (x80 * x412) + (-1 * x416 * x114); const FLT x419 = ((-1 * x415 * x117) + (x414 * x116)) * x118; const FLT x420 = x419 + (-1 * x418 * x113); const FLT x421 = x417 * x103; const FLT x422 = (x84 * ((-1 * x417 * x105) + x421)) + (x417 * x102); const FLT x423 = (x417 * x107) + (x84 * x422); const FLT x424 = x419 + (-1 * x135 * (x418 + (x420 * x133) + (x423 * x111) + (x417 * x101) + (-1 * x131 * ((-1 * x97 * ((x84 * x423) + (x417 * x130) + (x417 * x124) + (x84 * (x423 + (x84 * (x422 + (x84 * ((x417 * x127) + x421 + (-1 * x417 * x126))) + (x417 * x128))) + (x417 * x129))))) + (-1 * x420 * x123))))); const FLT x425 = x11 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x426 = x354 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x427 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x428 = (x427 * x356) + (-1 * x359 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x426 * x342) + (x9 * x426 * x353 * x349) + (-1 * x425 * x351) + (-1 * x427 * x351) + (x425 * x356) + (x358 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x429 = x363 * (*_x0).Pose.Rot[0]; const FLT x430 = x7 * x370; const FLT x431 = x428 * (*_x0).Pose.Rot[3]; const FLT x432 = x7 * x346; const FLT x433 = x428 * x365; const FLT x434 = 0.5 * x342; const FLT x435 = x346 * (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x436 = (-1 * x29 * x435) + (x371 * x399); const FLT x437 = x384 + x436 + (-1 * x433 * (*_x0).Pose.Rot[1]) + (-1 * x434 * x394) + x32 + (-1 * x429 * x428) + (x37 * x432) + (-1 * x31 * x430) + (-1 * x431 * x367) + (x428 * x391); const FLT x438 = x428 * x367; const FLT x439 = x23 * x430; const FLT x440 = x428 * x377; const FLT x441 = (x35 * x435) + (-1 * x371 * x382); const FLT x442 = (-1 * x434 * x387) + x441 + x398 + (-1 * x400) + (-1 * x440 * (*_x0).Pose.Rot[0]) + (-1 * x40 * x432) + x401 + (-1 * x428 * x393) + (x439 * (*_x0).Pose.Rot[1]) + (x438 * (*_x0).Pose.Rot[1]) + (-1 * x433 * (*_x0).Pose.Rot[3]); const FLT x443 = x37 * x435; const FLT x444 = x363 * (*_x0).Pose.Rot[1]; const FLT x445 = x371 * x387; const FLT x446 = -1 * x34; const FLT x447 = x395 + (x439 * (*_x0).Pose.Rot[2]) + (-1 * x434 * x382) + (-1 * x433 * (*_x0).Pose.Rot[0]) + (-1 * x443) + (-1 * x29 * x432) + x445 + (x438 * (*_x0).Pose.Rot[2]) + x446 + (x428 * x444) + (x431 * x377); const FLT x448 = (x447 * sensor_pt[1]) + (-1 * x437 * sensor_pt[2]) + (x442 * sensor_pt[0]); const FLT x449 = x371 * x394; const FLT x450 = x40 * x435; const FLT x451 = x27 + (-1 * x438 * (*_x0).Pose.Rot[0]) + (x35 * x432) + (-1 * x430 * x381) + (x431 * x363) + (-1 * x440 * (*_x0).Pose.Rot[1]) + x388 + (-1 * x434 * x399) + (-1 * x386) + (-1 * x433 * (*_x0).Pose.Rot[2]) + (-1 * x449) + x450; const FLT x452 = (x437 * sensor_pt[1]) + (-1 * x451 * sensor_pt[0]) + (x447 * sensor_pt[2]); const FLT x453 = (-1 * x442 * x225) + (x451 * x243) + (-1 * x448 * x241) + (x452 * x224); const FLT x454 = 2 * ((-1 * x442 * sensor_pt[1]) + (x447 * sensor_pt[0]) + (x451 * sensor_pt[2])); const FLT x455 = (x448 * x234) + (-1 * x451 * x228) + (-1 * x33 * x454) + (x437 * x225); const FLT x456 = (x38 * x454) + (-1 * x452 * x234) + (x442 * x228) + (-1 * x437 * x243); const FLT x457 = (x456 * (*lh_p).Rot[1]) + (-1 * x453 * (*lh_p).Rot[2]) + (x455 * (*lh_p).Rot[0]); const FLT x458 = (x453 * (*lh_p).Rot[0]) + (-1 * x456 * (*lh_p).Rot[3]) + (x455 * (*lh_p).Rot[2]); const FLT x459 = x456 + (-1 * x457 * x166) + (x458 * x168); const FLT x460 = (x453 * (*lh_p).Rot[3]) + (-1 * x455 * (*lh_p).Rot[1]) + (x456 * (*lh_p).Rot[0]); const FLT x461 = x453 + (-1 * x460 * x168) + (x457 * x176); const FLT x462 = x455 + (-1 * x458 * x176) + (x460 * x166); const FLT x463 = (x72 * x462) + (x67 * x461); const FLT x464 = (-1 * x75 * (x463 + (x61 * x459))) + (x60 * x459); const FLT x465 = x464 * x103; const FLT x466 = (x84 * ((-1 * x464 * x105) + x465)) + (x464 * x102); const FLT x467 = (x464 * x107) + (x84 * x466); const FLT x468 = (x80 * x459) + (-1 * x463 * x114); const FLT x469 = ((-1 * x462 * x117) + (x461 * x116)) * x118; const FLT x470 = x469 + (-1 * x468 * x113); const FLT x471 = x469 + (-1 * x135 * (x468 + (x464 * x101) + (x467 * x111) + (x470 * x133) + (-1 * x131 * ((-1 * x97 * ((x84 * x467) + (x464 * x124) + (x464 * x130) + (x84 * (x467 + (x84 * (x466 + (x84 * ((x464 * x127) + x465 + (-1 * x464 * x126))) + (x464 * x128))) + (x464 * x129))))) + (-1 * x470 * x123))))); const FLT x472 = x351 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x473 = x356 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x474 = x349 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x475 = x6 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x476 = x24 * x475; const FLT x477 = (-1 * x7 * x472) + (x475 * x357) + (x7 * x473) + (-1 * x11 * x472) + (x11 * x473) + (x474 * x355) + (-1 * x474 * x350) + (-1 * x476 * x354); const FLT x478 = x9 * x370; const FLT x479 = x477 * (*_x0).Pose.Rot[3]; const FLT x480 = x477 * x365; const FLT x481 = 0.5 * x476; const FLT x482 = x9 * x346; const FLT x483 = x396 + (-1 * x29 * x482) + (-1 * x477 * x429) + (-1 * x479 * x367) + x446 + (x477 * x391) + (-1 * x481 * x369) + x443 + (-1 * x480 * (*_x0).Pose.Rot[1]) + (-1 * x445) + (x478 * x341); const FLT x484 = x477 * x367; const FLT x485 = x477 * x377; const FLT x486 = (-1 * x390) + (-1 * x477 * x393) + (x35 * x482) + x449 + (x484 * (*_x0).Pose.Rot[1]) + x27 + (-1 * x480 * (*_x0).Pose.Rot[3]) + x389 + (-1 * x478 * x381) + (-1 * x450) + (-1 * x485 * (*_x0).Pose.Rot[0]) + (-1 * x31 * x481); const FLT x487 = x436 + (-1 * x480 * (*_x0).Pose.Rot[0]) + x340 + (-1 * x376) + (x485 * (*_x0).Pose.Rot[3]) + (-1 * x481 * x381) + (-1 * x37 * x482) + (x31 * x478) + x373 + (x477 * x444) + (x484 * (*_x0).Pose.Rot[2]); const FLT x488 = (x487 * sensor_pt[1]) + (-1 * x483 * sensor_pt[2]) + (x486 * sensor_pt[0]); const FLT x489 = x441 + x402 + (-1 * x485 * (*_x0).Pose.Rot[1]) + (-1 * x481 * x341) + (x479 * x363) + (-1 * x484 * (*_x0).Pose.Rot[0]) + (-1 * x480 * (*_x0).Pose.Rot[2]) + x30 + (-1 * x478 * x369) + (x40 * x482); const FLT x490 = (x483 * sensor_pt[1]) + (-1 * x489 * sensor_pt[0]) + (x487 * sensor_pt[2]); const FLT x491 = (-1 * x486 * x225) + (-1 * x488 * x241) + (x490 * x224) + (x489 * x243); const FLT x492 = (-1 * x486 * sensor_pt[1]) + (x487 * sensor_pt[0]) + (x489 * sensor_pt[2]); const FLT x493 = (x483 * x225) + (-1 * x489 * x228) + (-1 * x492 * x224) + (x488 * x234); const FLT x494 = (x492 * x241) + (-1 * x483 * x243) + (-1 * x490 * x234) + (x486 * x228); const FLT x495 = (x494 * (*lh_p).Rot[1]) + (-1 * x491 * (*lh_p).Rot[2]) + (x493 * (*lh_p).Rot[0]); const FLT x496 = (-1 * x494 * (*lh_p).Rot[3]) + (x491 * (*lh_p).Rot[0]) + (x493 * (*lh_p).Rot[2]); const FLT x497 = x494 + (-1 * x495 * x166) + (x496 * x168); const FLT x498 = (-1 * x493 * (*lh_p).Rot[1]) + (x491 * (*lh_p).Rot[3]) + (x494 * (*lh_p).Rot[0]); const FLT x499 = x491 + (-1 * x498 * x168) + (x495 * x176); const FLT x500 = (x498 * x166) + x493 + (-1 * x496 * x176); const FLT x501 = (x72 * x500) + (x67 * x499); const FLT x502 = (-1 * x75 * (x501 + (x61 * x497))) + (x60 * x497); const FLT x503 = x502 * x103; const FLT x504 = (x84 * ((-1 * x502 * x105) + x503)) + (x502 * x102); const FLT x505 = (x502 * x107) + (x84 * x504); const FLT x506 = (x80 * x497) + (-1 * x501 * x114); const FLT x507 = ((-1 * x500 * x117) + (x499 * x116)) * x118; const FLT x508 = x507 + (-1 * x506 * x113); const FLT x509 = x507 + (-1 * x135 * (x506 + (-1 * x131 * ((-1 * x97 * ((x502 * x130) + (x84 * x505) + (x502 * x124) + (x84 * (x505 + (x84 * ((x84 * ((x502 * x127) + x503 + (-1 * x502 * x126))) + x504 + (x502 * x128))) + (x502 * x129))))) + (-1 * x508 * x123))) + (x508 * x133) + (x502 * x101) + (x505 * x111))); const FLT x510 = dt * x167; const FLT x511 = dt * x169; const FLT x512 = x511 + x510; const FLT x513 = -1 * dt * x171; const FLT x514 = -1 * dt * x173; const FLT x515 = x514 + dt + x513; const FLT x516 = dt * x177; const FLT x517 = dt * x178; const FLT x518 = x517 + (-1 * x516); const FLT x519 = (x72 * x518) + (x67 * x515); const FLT x520 = x77 * ((-1 * x75 * (x519 + (x61 * x512))) + (x60 * x512)); const FLT x521 = x86 * x520; const FLT x522 = (x84 * ((-1 * x85 * x520) + x521)) + (x87 * x520); const FLT x523 = (x88 * x520) + (x84 * x522); const FLT x524 = (x80 * x512) + (-1 * x519 * x114); const FLT x525 = ((-1 * x518 * x117) + (x515 * x116)) * x118; const FLT x526 = x525 + (-1 * x524 * x113); const FLT x527 = x525 + (-1 * x135 * (x524 + (-1 * x131 * ((-1 * x97 * ((x84 * (x523 + (x84 * (x522 + (x84 * ((x91 * x520) + x521 + (-1 * x520 * x125))) + (x92 * x520))) + (x93 * x520))) + (x89 * x520) + (x94 * x520) + (x84 * x523))) + (-1 * x526 * x123))) + (x526 * x133) + (x520 * x100) + (x523 * x111))); const FLT x528 = (-1 * dt * x189) + dt; const FLT x529 = x528 + x514; const FLT x530 = x510 + (-1 * x511); const FLT x531 = dt * x193; const FLT x532 = dt * x194; const FLT x533 = x532 + x531; const FLT x534 = (x72 * x533) + (x67 * x530); const FLT x535 = (-1 * x75 * (x534 + (x61 * x529))) + (x60 * x529); const FLT x536 = x77 * x535; const FLT x537 = x86 * x536; const FLT x538 = (x84 * ((-1 * x85 * x536) + x537)) + (x87 * x536); const FLT x539 = (x88 * x536) + (x84 * x538); const FLT x540 = (x80 * x529) + (-1 * x534 * x114); const FLT x541 = ((-1 * x533 * x117) + (x530 * x116)) * x118; const FLT x542 = x541 + (-1 * x540 * x113); const FLT x543 = x541 + (-1 * x135 * ((x542 * x133) + (x539 * x111) + x540 + (x536 * x100) + (-1 * x131 * ((-1 * x97 * ((x94 * x536) + (x535 * x130) + (x84 * x539) + (x84 * (x539 + (x84 * (x538 + (x84 * ((x535 * x127) + x537 + (-1 * x536 * x125))) + (x92 * x536))) + (x535 * x129))))) + (-1 * x542 * x123))))); const FLT x544 = x531 + (-1 * x532); const FLT x545 = x516 + x517; const FLT x546 = x528 + x513; const FLT x547 = (x72 * x546) + (x67 * x545); const FLT x548 = (-1 * x75 * (x547 + (x61 * x544))) + (x60 * x544); const FLT x549 = x548 * x103; const FLT x550 = (x84 * ((-1 * x548 * x105) + x549)) + (x548 * x102); const FLT x551 = (x548 * x107) + (x84 * x550); const FLT x552 = (x80 * x544) + (-1 * x547 * x114); const FLT x553 = ((-1 * x546 * x117) + (x545 * x116)) * x118; const FLT x554 = x553 + (-1 * x552 * x113); const FLT x555 = x553 + (-1 * x135 * ((x554 * x133) + (-1 * x131 * ((-1 * x97 * ((x84 * x551) + (x84 * (x551 + (x84 * ((x84 * ((x548 * x127) + x549 + (-1 * x548 * x126))) + x550 + (x548 * x128))) + (x548 * x129))) + (x548 * x130) + (x548 * x124))) + (-1 * x554 * x123))) + (x551 * x111) + x552 + (x548 * x101))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x136 + (x137 * x136)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x153 + (x137 * x153)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x165 + (x165 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x188 + (x188 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x204 + (x204 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x216 + (x216 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x263 + (x263 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x294 + (x294 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x318 + (x318 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x339 + (x339 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x424 + (x424 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x471 + (x471 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x509 + (x509 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x527 + (x527 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x543 + (x543 * x137)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x555 + (x555 * x137)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen2_jac_x0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x13 * (*_x0).Pose.Rot[0]; const FLT x15 = x11 * x10; const FLT x16 = x15 * (*_x0).Pose.Rot[2]; const FLT x17 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x17 * (*_x0).Pose.Rot[1]; const FLT x19 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x20 = x19 * (*_x0).Pose.Rot[3]; const FLT x21 = x18 + x14 + (-1 * x20) + x16; const FLT x22 = (-1 * x19 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x23 = x17 * (*_x0).Pose.Rot[0]; const FLT x24 = x13 * (*_x0).Pose.Rot[1]; const FLT x25 = x15 * (*_x0).Pose.Rot[3]; const FLT x26 = x19 * (*_x0).Pose.Rot[2]; const FLT x27 = x26 + x25 + x23 + (-1 * x24); const FLT x28 = (x21 * sensor_pt[2]) + (-1 * x27 * sensor_pt[1]) + (x22 * sensor_pt[0]); const FLT x29 = x17 * (*_x0).Pose.Rot[2]; const FLT x30 = x13 * (*_x0).Pose.Rot[3]; const FLT x31 = x15 * (*_x0).Pose.Rot[1]; const FLT x32 = x19 * (*_x0).Pose.Rot[0]; const FLT x33 = x32 + x31 + (-1 * x29) + x30; const FLT x34 = (-1 * x33 * sensor_pt[2]) + (x27 * sensor_pt[0]) + (x22 * sensor_pt[1]); const FLT x35 = sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (2 * ((x34 * x33) + (-1 * x21 * x28))) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x36 = (-1 * x21 * sensor_pt[0]) + (x22 * sensor_pt[2]) + (x33 * sensor_pt[1]); const FLT x37 = (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (2 * ((x28 * x27) + (-1 * x33 * x36))) + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x38 = (2 * ((x36 * x21) + (-1 * x34 * x27))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x39 = (x35 * (*lh_p).Rot[0]) + (-1 * x38 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[1]); const FLT x40 = (-1 * x37 * (*lh_p).Rot[3]) + (x35 * (*lh_p).Rot[2]) + (x38 * (*lh_p).Rot[0]); const FLT x41 = x37 + (*lh_p).Pos[1] + (2 * ((x40 * (*lh_p).Rot[3]) + (-1 * x39 * (*lh_p).Rot[1]))); const FLT x42 = x41 * x41; const FLT x43 = (-1 * x35 * (*lh_p).Rot[1]) + (x37 * (*lh_p).Rot[0]) + (x38 * (*lh_p).Rot[3]); const FLT x44 = x35 + (2 * ((x43 * (*lh_p).Rot[1]) + (-1 * x40 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x45 = x38 + (2 * ((x39 * (*lh_p).Rot[2]) + (-1 * x43 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x46 = x45 * x45; const FLT x47 = x46 + (x44 * x44); const FLT x48 = x47 + x42; const FLT x49 = 0.523598775598299 + (*bsc0).tilt; const FLT x50 = cos(x49); const FLT x51 = 1. / x50; const FLT x52 = x51 * (1. / sqrt(x48)); const FLT x53 = asin(x52 * x41); const FLT x54 = 8.0108022e-06 * x53; const FLT x55 = -8.0108022e-06 + (-1 * x54); const FLT x56 = 0.0028679863 + (x53 * x55); const FLT x57 = 1. / sqrt(1 + (-1 * (1. / (x50 * x50)) * x42 * (1. / x48))); const FLT x58 = (-1 * x18) + x20 + (-1 * x16) + (-1 * x14); const FLT x59 = 2 * x33; const FLT x60 = 2 * x22; const FLT x61 = x60 * x27; const FLT x62 = x61 + (-1 * x58 * x59); const FLT x63 = x60 * x21; const FLT x64 = (x59 * x27) + (-1 * x63); const FLT x65 = 2 * x21; const FLT x66 = 1 + (x65 * x58) + (-2 * (x27 * x27)); const FLT x67 = (x66 * (*lh_p).Rot[0]) + (-1 * x62 * (*lh_p).Rot[3]) + (x64 * (*lh_p).Rot[2]); const FLT x68 = 2 * (*lh_p).Rot[3]; const FLT x69 = (-1 * x66 * (*lh_p).Rot[2]) + (x62 * (*lh_p).Rot[1]) + (x64 * (*lh_p).Rot[0]); const FLT x70 = 2 * (*lh_p).Rot[1]; const FLT x71 = x62 + (x67 * x68) + (-1 * x70 * x69); const FLT x72 = 2 * x41; const FLT x73 = 2 * (*lh_p).Rot[2]; const FLT x74 = (x66 * (*lh_p).Rot[3]) + (-1 * x64 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]); const FLT x75 = x66 + (x73 * x69) + (-1 * x74 * x68); const FLT x76 = 2 * x45; const FLT x77 = x64 + (x70 * x74) + (-1 * x73 * x67); const FLT x78 = 2 * x44; const FLT x79 = (x78 * x77) + (x75 * x76); const FLT x80 = 1.0/2.0 * x41; const FLT x81 = x80 * x51 * (1. / (x48 * sqrt(x48))); const FLT x82 = x57 * ((-1 * x81 * (x79 + (x71 * x72))) + (x71 * x52)); const FLT x83 = x82 * x55; const FLT x84 = (x53 * ((-1 * x82 * x54) + x83)) + (x82 * x56); const FLT x85 = 5.3685255e-06 + (x53 * x56); const FLT x86 = (x82 * x85) + (x84 * x53); const FLT x87 = 0.0076069798 + (x85 * x53); const FLT x88 = x87 * x53; const FLT x89 = -8.0108022e-06 + (-1.60216044e-05 * x53); const FLT x90 = x56 + (x89 * x53); const FLT x91 = x85 + (x53 * x90); const FLT x92 = x87 + (x53 * x91); const FLT x93 = (x53 * x92) + x88; const FLT x94 = atan2(-1 * x44, x45); const FLT x95 = tan(x49); const FLT x96 = x95 * (1. / sqrt(x47)); const FLT x97 = x96 * x41; const FLT x98 = (-1 * asin(x97)) + x94 + (*bsc0).ogeephase; const FLT x99 = (sin(x98) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x100 = sin(x49); const FLT x101 = x99 * x100; const FLT x102 = x50 + (-1 * x93 * x101); const FLT x103 = 1. / x102; const FLT x104 = x53 * x53; const FLT x105 = x99 * x104; const FLT x106 = x103 * x105; const FLT x107 = x80 * x95 * (1. / (x47 * sqrt(x47))); const FLT x108 = (x71 * x96) + (-1 * x79 * x107); const FLT x109 = 1. / x47; const FLT x110 = 1. / sqrt(1 + (-1 * (x95 * x95) * x42 * x109)); const FLT x111 = x44 * (1. / x46); const FLT x112 = 1. / x45; const FLT x113 = x46 * x109; const FLT x114 = ((-1 * x77 * x112) + (x75 * x111)) * x113; const FLT x115 = cos(x98) * (*bsc0).ogeemag; const FLT x116 = x115 * (x114 + (-1 * x108 * x110)); const FLT x117 = x93 * x100; const FLT x118 = 2.40324066e-05 * x53; const FLT x119 = x87 * (1. / (x102 * x102)) * x105; const FLT x120 = 2 * x88 * x99 * x103; const FLT x121 = x87 * x103 * x104; const FLT x122 = x97 + (x87 * x106); const FLT x123 = 1. / sqrt(1 + (-1 * (x122 * x122))); const FLT x124 = x114 + (-1 * x123 * (x108 + (x82 * x120) + (x86 * x106) + (x116 * x121) + (-1 * x119 * ((-1 * x101 * ((x82 * x87) + (x82 * x92) + (x86 * x53) + (x53 * (x86 + (x53 * (x84 + (x53 * ((x82 * x89) + x83 + (-1 * x82 * x118))) + (x82 * x90))) + (x82 * x91))))) + (-1 * x116 * x117))))); const FLT x125 = cos((*bsc0).gibpha + (-1 * asin(x122)) + x94) * (*bsc0).gibmag; const FLT x126 = 2 * ((-1 * x25) + (-1 * x26) + x24 + (-1 * x23)); const FLT x127 = x60 * x33; const FLT x128 = x127 + (-1 * x21 * x126); const FLT x129 = 1 + (x27 * x126) + (-2 * (x33 * x33)); const FLT x130 = (x65 * x33) + (-1 * x61); const FLT x131 = (x130 * (*lh_p).Rot[0]) + (x128 * (*lh_p).Rot[2]) + (-1 * x129 * (*lh_p).Rot[3]); const FLT x132 = (x129 * (*lh_p).Rot[1]) + (x128 * (*lh_p).Rot[0]) + (-1 * x130 * (*lh_p).Rot[2]); const FLT x133 = x129 + (x68 * x131) + (-1 * x70 * x132); const FLT x134 = 2 * ((x130 * (*lh_p).Rot[3]) + (-1 * x128 * (*lh_p).Rot[1]) + (x129 * (*lh_p).Rot[0])); const FLT x135 = x130 + (x73 * x132) + (-1 * x134 * (*lh_p).Rot[3]); const FLT x136 = x128 + (x134 * (*lh_p).Rot[1]) + (-1 * x73 * x131); const FLT x137 = (x78 * x136) + (x76 * x135); const FLT x138 = x57 * ((-1 * x81 * (x137 + (x72 * x133))) + (x52 * x133)); const FLT x139 = x55 * x138; const FLT x140 = (x53 * ((-1 * x54 * x138) + x139)) + (x56 * x138); const FLT x141 = (x85 * x138) + (x53 * x140); const FLT x142 = (x96 * x133) + (-1 * x107 * x137); const FLT x143 = ((-1 * x112 * x136) + (x111 * x135)) * x113; const FLT x144 = x143 + (-1 * x110 * x142); const FLT x145 = x115 * x117; const FLT x146 = x115 * x121; const FLT x147 = x143 + (-1 * x123 * ((x120 * x138) + (x106 * x141) + x142 + (x144 * x146) + (-1 * x119 * ((-1 * x101 * ((x87 * x138) + (x53 * x141) + (x92 * x138) + (x53 * ((x53 * (x140 + (x53 * (x139 + (x89 * x138) + (-1 * x118 * x138))) + (x90 * x138))) + x141 + (x91 * x138))))) + (-1 * x144 * x145))))); const FLT x148 = (x65 * x27) + (-1 * x127); const FLT x149 = 2 * ((-1 * x32) + (-1 * x30) + x29 + (-1 * x31)); const FLT x150 = 1 + (x33 * x149) + (-2 * (x21 * x21)); const FLT x151 = x63 + (-1 * x27 * x149); const FLT x152 = (x151 * (*lh_p).Rot[0]) + (-1 * x148 * (*lh_p).Rot[3]) + (x150 * (*lh_p).Rot[2]); const FLT x153 = (x148 * (*lh_p).Rot[1]) + (-1 * x151 * (*lh_p).Rot[2]) + (x150 * (*lh_p).Rot[0]); const FLT x154 = x148 + (x68 * x152) + (-1 * x70 * x153); const FLT x155 = (x151 * (*lh_p).Rot[3]) + (-1 * x150 * (*lh_p).Rot[1]) + (x148 * (*lh_p).Rot[0]); const FLT x156 = x151 + (x73 * x153) + (-1 * x68 * x155); const FLT x157 = x150 + (x70 * x155) + (-1 * x73 * x152); const FLT x158 = (x78 * x157) + (x76 * x156); const FLT x159 = x57 * ((-1 * x81 * (x158 + (x72 * x154))) + (x52 * x154)); const FLT x160 = x55 * x159; const FLT x161 = (x53 * ((-1 * x54 * x159) + x160)) + (x56 * x159); const FLT x162 = (x85 * x159) + (x53 * x161); const FLT x163 = (x96 * x154) + (-1 * x107 * x158); const FLT x164 = ((-1 * x112 * x157) + (x111 * x156)) * x113; const FLT x165 = x164 + (-1 * x110 * x163); const FLT x166 = x164 + (-1 * x123 * (x163 + (x165 * x146) + (-1 * x119 * ((-1 * x101 * ((x87 * x159) + (x53 * ((x53 * (x161 + (x53 * ((x89 * x159) + x160 + (-1 * x118 * x159))) + (x90 * x159))) + x162 + (x91 * x159))) + (x53 * x162) + (x92 * x159))) + (-1 * x165 * x145))) + (x106 * x162) + (x120 * x159))); cnMatrixOptionalSet(Hx, 0, 0, x124 + (x124 * x125)); cnMatrixOptionalSet(Hx, 0, 1, x147 + (x125 * x147)); cnMatrixOptionalSet(Hx, 0, 2, x166 + (x125 * x166)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen2_jac_sensor_pt(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 0.523598775598299 + (*bsc0).tilt; const FLT x1 = tan(x0); const FLT x2 = dt * (*_x0).Velocity.Pos[2]; const FLT x3 = 1.0/2.0 * dt * fabs(dt); const FLT x4 = x3 * (*_x0).Acc[2]; const FLT x5 = dt * dt; const FLT x6 = x5 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x7 = x5 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x8 = x5 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x9 = 1e-10 + x6 + x7 + x8; const FLT x10 = sqrt(x9); const FLT x11 = 0.5 * x10; const FLT x12 = sin(x11); const FLT x13 = (1. / x9) * (x12 * x12); const FLT x14 = cos(x11); const FLT x15 = 1. / sqrt((x7 * x13) + (x6 * x13) + (x8 * x13) + (x14 * x14)); const FLT x16 = dt * x15 * x12 * (1. / x10); const FLT x17 = x16 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x18 = x15 * x14; const FLT x19 = x16 * (*_x0).Pose.Rot[1]; const FLT x20 = x16 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x21 = (x19 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x20 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[0]) + (x18 * (*_x0).Pose.Rot[2]); const FLT x22 = x16 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x23 = (-1 * x20 * (*_x0).Pose.Rot[1]) + (x18 * (*_x0).Pose.Rot[0]) + (-1 * x22 * (*_x0).Pose.Rot[3]) + (-1 * x17 * (*_x0).Pose.Rot[2]); const FLT x24 = (x18 * (*_x0).Pose.Rot[3]) + (x20 * (*_x0).Pose.Rot[2]) + (x22 * (*_x0).Pose.Rot[0]) + (-1 * x19 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x25 = (-1 * x24 * sensor_pt[1]) + (x21 * sensor_pt[2]) + (x23 * sensor_pt[0]); const FLT x26 = (x20 * (*_x0).Pose.Rot[0]) + (-1 * x22 * (*_x0).Pose.Rot[2]) + (x18 * (*_x0).Pose.Rot[1]) + (x17 * (*_x0).Pose.Rot[3]); const FLT x27 = (-1 * x26 * sensor_pt[2]) + (x24 * sensor_pt[0]) + (x23 * sensor_pt[1]); const FLT x28 = 2 * ((x26 * x27) + (-1 * x25 * x21)); const FLT x29 = x28 + x4 + sensor_pt[2] + x2 + (*_x0).Pose.Pos[2]; const FLT x30 = x29 * (*lh_p).Rot[0]; const FLT x31 = x3 * (*_x0).Acc[1]; const FLT x32 = dt * (*_x0).Velocity.Pos[1]; const FLT x33 = (-1 * x21 * sensor_pt[0]) + (x23 * sensor_pt[2]) + (x26 * sensor_pt[1]); const FLT x34 = 2 * ((x24 * x25) + (-1 * x33 * x26)); const FLT x35 = x34 + x32 + (*_x0).Pose.Pos[1] + sensor_pt[1] + x31; const FLT x36 = x35 * (*lh_p).Rot[1]; const FLT x37 = dt * (*_x0).Velocity.Pos[0]; const FLT x38 = x3 * (*_x0).Acc[0]; const FLT x39 = 2 * ((x33 * x21) + (-1 * x24 * x27)); const FLT x40 = x39 + sensor_pt[0] + x37 + (*_x0).Pose.Pos[0] + x38; const FLT x41 = x40 * (*lh_p).Rot[2]; const FLT x42 = (-1 * x41) + x30 + x36; const FLT x43 = x29 * (*lh_p).Rot[2]; const FLT x44 = x40 * (*lh_p).Rot[0]; const FLT x45 = x35 * (*lh_p).Rot[3]; const FLT x46 = (-1 * x45) + x43 + x44; const FLT x47 = x35 + (*lh_p).Pos[1] + (2 * ((x46 * (*lh_p).Rot[3]) + (-1 * x42 * (*lh_p).Rot[1]))); const FLT x48 = x47 * x47; const FLT x49 = x35 * (*lh_p).Rot[0]; const FLT x50 = x40 * (*lh_p).Rot[3]; const FLT x51 = x29 * (*lh_p).Rot[1]; const FLT x52 = (-1 * x51) + x49 + x50; const FLT x53 = x29 + (2 * ((x52 * (*lh_p).Rot[1]) + (-1 * x46 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x54 = x40 + (2 * ((x42 * (*lh_p).Rot[2]) + (-1 * x52 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x55 = x54 * x54; const FLT x56 = x55 + (x53 * x53); const FLT x57 = 1. / x56; const FLT x58 = 1. / sqrt(1 + (-1 * (x1 * x1) * x57 * x48)); const FLT x59 = x54 * x47; const FLT x60 = x1 * (1. / (x56 * sqrt(x56))); const FLT x61 = x60 * x59; const FLT x62 = x53 * x57; const FLT x63 = x62 + (x61 * x58); const FLT x64 = atan2(-1 * x53, x54); const FLT x65 = x1 * (1. / sqrt(x56)); const FLT x66 = x65 * x47; const FLT x67 = (-1 * asin(x66)) + x64 + (*bsc0).ogeephase; const FLT x68 = cos(x67) * (*bsc0).ogeemag; const FLT x69 = x56 + x48; const FLT x70 = cos(x0); const FLT x71 = 1. / x70; const FLT x72 = x71 * (1. / sqrt(x69)); const FLT x73 = asin(x72 * x47); const FLT x74 = 8.0108022e-06 * x73; const FLT x75 = -8.0108022e-06 + (-1 * x74); const FLT x76 = 0.0028679863 + (x73 * x75); const FLT x77 = 5.3685255e-06 + (x73 * x76); const FLT x78 = 0.0076069798 + (x73 * x77); const FLT x79 = x73 * x78; const FLT x80 = -8.0108022e-06 + (-1.60216044e-05 * x73); const FLT x81 = x76 + (x80 * x73); const FLT x82 = x77 + (x81 * x73); const FLT x83 = x78 + (x82 * x73); const FLT x84 = (x83 * x73) + x79; const FLT x85 = (sin(x67) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x86 = sin(x0); const FLT x87 = x85 * x86; const FLT x88 = x70 + (-1 * x84 * x87); const FLT x89 = 1. / x88; const FLT x90 = x73 * x73; const FLT x91 = x78 * x90; const FLT x92 = x89 * x91; const FLT x93 = x68 * x92; const FLT x94 = 2 * x54; const FLT x95 = 1. / sqrt(1 + (-1 * (1. / (x70 * x70)) * (1. / x69) * x48)); const FLT x96 = x71 * (1. / (x69 * sqrt(x69))); const FLT x97 = x96 * x47; const FLT x98 = x89 * x85; const FLT x99 = x79 * x98; const FLT x100 = x99 * x97 * x95; const FLT x101 = x84 * x86; const FLT x102 = x68 * x101; const FLT x103 = x83 * x95; const FLT x104 = x59 * x96; const FLT x105 = x75 * x95; const FLT x106 = -1 * x105 * x104; const FLT x107 = 2.40324066e-05 * x73; const FLT x108 = x95 * x107; const FLT x109 = x80 * x95; const FLT x110 = x81 * x95; const FLT x111 = x76 * x95; const FLT x112 = x74 * x95; const FLT x113 = (x73 * ((x104 * x112) + x106)) + (-1 * x104 * x111); const FLT x114 = x82 * x95; const FLT x115 = x77 * x95; const FLT x116 = (-1 * x104 * x115) + (x73 * x113); const FLT x117 = x78 * x95; const FLT x118 = (1. / (x88 * x88)) * x85 * x91; const FLT x119 = x90 * x98; const FLT x120 = x66 + (x78 * x119); const FLT x121 = 1. / sqrt(1 + (-1 * (x120 * x120))); const FLT x122 = x62 + (-1 * x121 * ((-1 * x61) + (-1 * x118 * ((-1 * x87 * ((x73 * x116) + (-1 * x103 * x104) + (-1 * x104 * x117) + (x73 * (x116 + (x73 * (x113 + (x73 * (x106 + (-1 * x109 * x104) + (x108 * x104))) + (-1 * x104 * x110))) + (-1 * x104 * x114))))) + (-1 * x63 * x102))) + (x63 * x93) + (x119 * x116) + (-1 * x94 * x100))); const FLT x123 = cos((-1 * asin(x120)) + (*bsc0).gibpha + x64) * (*bsc0).gibmag; const FLT x124 = x65 * x58; const FLT x125 = (-1 * x96 * x48) + x72; const FLT x126 = x105 * x125; const FLT x127 = x95 * x125; const FLT x128 = (x73 * ((-1 * x74 * x127) + x126)) + (x111 * x125); const FLT x129 = (x115 * x125) + (x73 * x128); const FLT x130 = 2 * x99; const FLT x131 = x121 * ((x119 * x129) + (x127 * x130) + (-1 * x118 * ((-1 * x87 * ((x117 * x125) + (x73 * x129) + (x103 * x125) + (x73 * ((x73 * (x128 + (x73 * (x126 + (x109 * x125) + (-1 * x107 * x127))) + (x110 * x125))) + x129 + (x114 * x125))))) + (x102 * x124))) + (-1 * x93 * x124) + x65); const FLT x132 = x60 * x47; const FLT x133 = x53 * x132; const FLT x134 = -1 * x54 * x57; const FLT x135 = x68 * (x134 + (x58 * x133)); const FLT x136 = 2 * x53; const FLT x137 = x53 * x97; const FLT x138 = -1 * x105 * x137; const FLT x139 = (x73 * ((x112 * x137) + x138)) + (-1 * x111 * x137); const FLT x140 = (-1 * x115 * x137) + (x73 * x139); const FLT x141 = x134 + (-1 * x121 * ((x119 * x140) + (-1 * x118 * ((-1 * x87 * ((-1 * x103 * x137) + (x73 * (x140 + (x73 * (x139 + (x73 * ((-1 * x109 * x137) + (x108 * x137) + x138)) + (-1 * x110 * x137))) + (-1 * x114 * x137))) + (-1 * x117 * x137) + (x73 * x140))) + (-1 * x101 * x135))) + (-1 * x133) + (x92 * x135) + (-1 * x100 * x136))); const FLT x142 = 2 * x45; const FLT x143 = (2 * x43) + (-1 * x142); const FLT x144 = 2 * x41; const FLT x145 = (2 * x36) + (-1 * x144); const FLT x146 = (x136 * x145) + (x94 * x143); const FLT x147 = 1.0/2.0 * x132; const FLT x148 = 2 * x51; const FLT x149 = (2 * x50) + (-1 * x148); const FLT x150 = (x65 * x149) + (-1 * x146 * x147); const FLT x151 = x53 * (1. / x55); const FLT x152 = 1. / x54; const FLT x153 = x57 * x55; const FLT x154 = ((-1 * x145 * x152) + (x143 * x151)) * x153; const FLT x155 = x154 + (-1 * x58 * x150); const FLT x156 = 2 * x47; const FLT x157 = 1.0/2.0 * x97; const FLT x158 = (-1 * x157 * (x146 + (x149 * x156))) + (x72 * x149); const FLT x159 = x95 * x158; const FLT x160 = x75 * x159; const FLT x161 = (x73 * ((-1 * x74 * x159) + x160)) + (x76 * x159); const FLT x162 = (x77 * x159) + (x73 * x161); const FLT x163 = x154 + (-1 * x121 * (x150 + (x93 * x155) + (x119 * x162) + (-1 * x118 * ((-1 * x87 * ((x78 * x159) + (x73 * x162) + (x73 * (x162 + (x73 * ((x73 * ((x109 * x158) + x160 + (-1 * x107 * x159))) + x161 + (x81 * x159))) + (x82 * x159))) + (x103 * x158))) + (-1 * x102 * x155))) + (x130 * x159))); const FLT x164 = (-1 * x4) + (-1 * x28) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * sensor_pt[2]) + (-1 * x2); const FLT x165 = 2 * (*lh_p).Rot[3]; const FLT x166 = 2 * (*lh_p).Rot[2]; const FLT x167 = (x35 * x166) + (-1 * x165 * x164); const FLT x168 = 2 * (*lh_p).Rot[1]; const FLT x169 = 2 * x49; const FLT x170 = x149 + (x168 * x164) + x169; const FLT x171 = (x170 * x136) + (x94 * x167); const FLT x172 = 2 * x30; const FLT x173 = (-4 * x36) + (-1 * x172) + x144; const FLT x174 = (x65 * x173) + (-1 * x171 * x147); const FLT x175 = ((-1 * x170 * x152) + (x167 * x151)) * x153; const FLT x176 = x175 + (-1 * x58 * x174); const FLT x177 = (-1 * x157 * (x171 + (x173 * x156))) + (x72 * x173); const FLT x178 = x105 * x177; const FLT x179 = x95 * x177; const FLT x180 = (x73 * ((-1 * x74 * x179) + x178)) + (x111 * x177); const FLT x181 = (x115 * x177) + (x73 * x180); const FLT x182 = x175 + (-1 * x121 * (x174 + (x93 * x176) + (x119 * x181) + (-1 * x118 * ((-1 * x87 * ((x73 * x181) + (x103 * x177) + (x117 * x177) + (x73 * (x181 + (x73 * (x180 + (x73 * (x178 + (x109 * x177) + (-1 * x107 * x179))) + (x110 * x177))) + (x114 * x177))))) + (-1 * x102 * x176))) + (x179 * x130))); const FLT x183 = (-1 * x39) + (-1 * (*_x0).Pose.Pos[0]) + (-1 * x38) + (-1 * x37) + (-1 * sensor_pt[0]); const FLT x184 = x145 + x172 + (x166 * x183); const FLT x185 = 2 * x44; const FLT x186 = (-1 * x185) + (-4 * x43) + x142; const FLT x187 = (x186 * x136) + (x94 * x184); const FLT x188 = (x29 * x165) + (-1 * x168 * x183); const FLT x189 = (x65 * x188) + (-1 * x187 * x147); const FLT x190 = ((-1 * x186 * x152) + (x184 * x151)) * x153; const FLT x191 = x190 + (-1 * x58 * x189); const FLT x192 = (-1 * x157 * (x187 + (x188 * x156))) + (x72 * x188); const FLT x193 = x105 * x192; const FLT x194 = (x73 * ((-1 * x112 * x192) + x193)) + (x111 * x192); const FLT x195 = (x115 * x192) + (x73 * x194); const FLT x196 = x95 * x130; const FLT x197 = x190 + (-1 * x121 * ((x196 * x192) + (x93 * x191) + (-1 * x118 * ((-1 * x87 * ((x117 * x192) + (x73 * x195) + (x103 * x192) + (x73 * (x195 + (x73 * (x194 + (x73 * ((x109 * x192) + x193 + (-1 * x108 * x192))) + (x110 * x192))) + (x114 * x192))))) + (-1 * x102 * x191))) + x189 + (x119 * x195))); const FLT x198 = x148 + (-1 * x169) + (-4 * x50); const FLT x199 = (-1 * x34) + (-1 * x32) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * x31) + (-1 * sensor_pt[1]); const FLT x200 = (x40 * x168) + (-1 * x166 * x199); const FLT x201 = (x200 * x136) + (x94 * x198); const FLT x202 = x143 + (x165 * x199) + x185; const FLT x203 = (x65 * x202) + (-1 * x201 * x147); const FLT x204 = ((-1 * x200 * x152) + (x198 * x151)) * x153; const FLT x205 = x204 + (-1 * x58 * x203); const FLT x206 = (-1 * x157 * (x201 + (x202 * x156))) + (x72 * x202); const FLT x207 = x206 * x105; const FLT x208 = (x73 * ((-1 * x206 * x112) + x207)) + (x206 * x111); const FLT x209 = (x206 * x115) + (x73 * x208); const FLT x210 = x204 + (-1 * x121 * (x203 + (x93 * x205) + (x209 * x119) + (-1 * x118 * ((-1 * x87 * ((x206 * x117) + (x73 * (x209 + (x73 * (x208 + (x73 * ((x206 * x109) + x207 + (-1 * x206 * x108))) + (x206 * x110))) + (x206 * x114))) + (x206 * x103) + (x73 * x209))) + (-1 * x205 * x102))) + (x206 * x196))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), x122 + (x123 * x122)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x123 * x131) + (-1 * x131)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x141 + (x123 * x141)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x163 + (x123 * x163)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x182 + (x123 * x182)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x197 + (x123 * x197)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), x210 + (x210 * x123)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen2_jac_lh_p(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 0.523598775598299 + (*bsc0).tilt; const FLT x1 = cos(x0); const FLT x2 = 1.0/2.0 * dt * fabs(dt); const FLT x3 = dt * dt; const FLT x4 = x3 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x5 = x3 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x6 = x3 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x7 = 1e-10 + x4 + x5 + x6; const FLT x8 = sqrt(x7); const FLT x9 = 0.5 * x8; const FLT x10 = sin(x9); const FLT x11 = (1. / x7) * (x10 * x10); const FLT x12 = cos(x9); const FLT x13 = 1. / sqrt((x6 * x11) + (x5 * x11) + (x4 * x11) + (x12 * x12)); const FLT x14 = (1. / x8) * dt * x13 * x10; const FLT x15 = x14 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x16 = x13 * x12; const FLT x17 = x14 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x14 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = (-1 * x18 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]); const FLT x20 = (x16 * (*_x0).Pose.Rot[0]) + (-1 * x18 * (*_x0).Pose.Rot[1]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x15 * (*_x0).Pose.Rot[2]); const FLT x21 = (x18 * (*_x0).Pose.Rot[2]) + (x16 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[1]); const FLT x22 = (-1 * x21 * sensor_pt[1]) + (x19 * sensor_pt[2]) + (x20 * sensor_pt[0]); const FLT x23 = (-1 * x17 * (*_x0).Pose.Rot[2]) + (x18 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[3]); const FLT x24 = (x21 * sensor_pt[0]) + (-1 * x23 * sensor_pt[2]) + (x20 * sensor_pt[1]); const FLT x25 = sensor_pt[2] + (x2 * (*_x0).Acc[2]) + (2 * ((x24 * x23) + (-1 * x22 * x19))) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x26 = (x20 * sensor_pt[2]) + (-1 * x19 * sensor_pt[0]) + (x23 * sensor_pt[1]); const FLT x27 = (2 * ((x22 * x21) + (-1 * x23 * x26))) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (dt * (*_x0).Velocity.Pos[1]) + (x2 * (*_x0).Acc[1]); const FLT x28 = (*_x0).Pose.Pos[0] + sensor_pt[0] + (2 * ((x26 * x19) + (-1 * x24 * x21))) + (dt * (*_x0).Velocity.Pos[0]) + (x2 * (*_x0).Acc[0]); const FLT x29 = (-1 * x28 * (*lh_p).Rot[2]) + (x25 * (*lh_p).Rot[0]) + (x27 * (*lh_p).Rot[1]); const FLT x30 = (-1 * x27 * (*lh_p).Rot[3]) + (x25 * (*lh_p).Rot[2]) + (x28 * (*lh_p).Rot[0]); const FLT x31 = x27 + (*lh_p).Pos[1] + (2 * ((x30 * (*lh_p).Rot[3]) + (-1 * x29 * (*lh_p).Rot[1]))); const FLT x32 = x31 * x31; const FLT x33 = (-1 * x25 * (*lh_p).Rot[1]) + (x27 * (*lh_p).Rot[0]) + (x28 * (*lh_p).Rot[3]); const FLT x34 = x25 + (2 * ((x33 * (*lh_p).Rot[1]) + (-1 * x30 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x35 = x28 + (2 * ((x29 * (*lh_p).Rot[2]) + (-1 * x33 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x36 = (x35 * x35) + (x34 * x34); const FLT x37 = x36 + x32; const FLT x38 = x31 * (1. / sqrt(x37)); const FLT x39 = asin((1. / x1) * x38); const FLT x40 = 8.0108022e-06 * x39; const FLT x41 = -8.0108022e-06 + (-1 * x40); const FLT x42 = 0.0028679863 + (x41 * x39); const FLT x43 = 5.3685255e-06 + (x42 * x39); const FLT x44 = 0.0076069798 + (x43 * x39); const FLT x45 = x39 * x39; const FLT x46 = x44 * x39; const FLT x47 = -8.0108022e-06 + (-1.60216044e-05 * x39); const FLT x48 = x42 + (x47 * x39); const FLT x49 = x43 + (x48 * x39); const FLT x50 = x44 + (x49 * x39); const FLT x51 = (x50 * x39) + x46; const FLT x52 = atan2(-1 * x34, x35); const FLT x53 = tan(x0); const FLT x54 = x31 * (1. / sqrt(x36)); const FLT x55 = x54 * x53; const FLT x56 = (-1 * asin(x55)) + x52 + (*bsc0).ogeephase; const FLT x57 = sin(x56); const FLT x58 = (x57 * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x59 = sin(x0); const FLT x60 = x58 * x59; const FLT x61 = x60 * x51; const FLT x62 = x1 + (-1 * x61); const FLT x63 = 1. / x62; const FLT x64 = x63 * x45; const FLT x65 = x64 * x44; const FLT x66 = (1. / (x62 * x62)) * x44 * x45; const FLT x67 = x61 * x66; const FLT x68 = x55 + (x65 * x58); const FLT x69 = 1. / sqrt(1 + (-1 * (x68 * x68))); const FLT x70 = (x67 + x65) * x69; const FLT x71 = (-1 * asin(x68)) + (*bsc0).gibpha + x52; const FLT x72 = cos(x71) * (*bsc0).gibmag; const FLT x73 = ((x67 * x57) + (x65 * x57)) * x69; const FLT x74 = cos(x56) * (*bsc0).ogeemag; const FLT x75 = x74 * x65; const FLT x76 = x69 * ((x74 * x67) + x75); const FLT x77 = x53 * x53; const FLT x78 = x54 * (1 + x77); const FLT x79 = x78 * (1. / sqrt(1 + (-1 * x77 * x32 * (1. / x36)))); const FLT x80 = 1. / (x1 * x1); const FLT x81 = x80 * x38 * (1. / sqrt(1 + (-1 * x80 * x32 * (1. / x37)))); const FLT x82 = x81 * x59; const FLT x83 = x82 * x41; const FLT x84 = (x39 * ((-1 * x82 * x40) + x83)) + (x82 * x42); const FLT x85 = (x82 * x43) + (x84 * x39); const FLT x86 = x69 * ((-1 * x66 * x58 * ((x79 * x74 * x51 * x59) + (-1 * x1 * x51 * x58) + (-1 * x60 * ((x82 * x44) + (x85 * x39) + (x39 * (x85 + (x39 * (x84 + (x39 * (x83 + (x82 * x47) + (-2.40324066e-05 * x82 * x39))) + (x82 * x48))) + (x82 * x49))) + (x82 * x50))) + (-1 * x59))) + (x85 * x64 * x58) + x78 + (2 * x81 * x60 * x63 * x46) + (-1 * x79 * x75)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), (-1 * x70 * x72) + (-1 * x70)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), sin(x71)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x72); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeemag)/sizeof(FLT), (-1 * x73 * x72) + (-1 * x73)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeephase)/sizeof(FLT), (-1 * x72 * x76) + (-1 * x76)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x86 * x72) + (-1 * x86)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_x_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_x_gen2_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_x_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_x_gen2_jac_bsc0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanModel_LightMeas_y_gen2(const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x11 * x10; const FLT x15 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x16 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x17 = (-1 * x16 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[1]) + (x13 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[2]); const FLT x18 = (-1 * x16 * (*_x0).Pose.Rot[1]) + (x14 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]) + (-1 * x13 * (*_x0).Pose.Rot[1]); const FLT x20 = (-1 * x19 * sensor_pt[1]) + (x17 * sensor_pt[2]) + (x18 * sensor_pt[0]); const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (x14 * (*_x0).Pose.Rot[1]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (x13 * (*_x0).Pose.Rot[3]); const FLT x22 = (x19 * sensor_pt[0]) + (-1 * x21 * sensor_pt[2]) + (x18 * sensor_pt[1]); const FLT x23 = (2 * ((x22 * x21) + (-1 * x20 * x17))) + sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x24 = (-1 * x17 * sensor_pt[0]) + (x18 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x25 = (*_x0).Pose.Pos[0] + (2 * ((x24 * x17) + (-1 * x22 * x19))) + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x26 = (2 * ((x20 * x19) + (-1 * x24 * x21))) + (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x27 = (-1 * x26 * (*lh_p).Rot[3]) + (x23 * (*lh_p).Rot[2]) + (x25 * (*lh_p).Rot[0]); const FLT x28 = (-1 * x23 * (*lh_p).Rot[1]) + (x26 * (*lh_p).Rot[0]) + (x25 * (*lh_p).Rot[3]); const FLT x29 = x23 + (2 * ((x28 * (*lh_p).Rot[1]) + (-1 * x27 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x30 = (-1 * x25 * (*lh_p).Rot[2]) + (x23 * (*lh_p).Rot[0]) + (x26 * (*lh_p).Rot[1]); const FLT x31 = (2 * ((x30 * (*lh_p).Rot[2]) + (-1 * x28 * (*lh_p).Rot[3]))) + x25 + (*lh_p).Pos[0]; const FLT x32 = atan2(-1 * x29, x31); const FLT x33 = (*lh_p).Pos[1] + x26 + (2 * ((x27 * (*lh_p).Rot[3]) + (-1 * x30 * (*lh_p).Rot[1]))); const FLT x34 = (x31 * x31) + (x29 * x29); const FLT x35 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x36 = cos(x35); const FLT x37 = asin(x33 * (1. / x36) * (1. / sqrt(x34 + (x33 * x33)))); const FLT x38 = -1 * (1. / sqrt(x34)) * x33 * tan(x35); const FLT x39 = ((*bsc0).ogeemag * sin((-1 * asin(x38)) + (*bsc0).ogeephase + x32)) + (*bsc0).curve; const FLT x40 = 0.0028679863 + (x37 * (-8.0108022e-06 + (-8.0108022e-06 * x37))); const FLT x41 = 5.3685255e-06 + (x40 * x37); const FLT x42 = 0.0076069798 + (x41 * x37); const FLT x43 = asin(x38 + (x42 * (x37 * x37) * x39 * (1. / (x36 + (x39 * sin(x35) * ((x37 * (x42 + (x37 * (x41 + (x37 * (x40 + (x37 * (-8.0108022e-06 + (-1.60216044e-05 * x37))))))))) + (x42 * x37))))))); return -1.5707963267949 + x32 + (-1 * (*bsc0).phase) + (-1 * x43) + (-1 * sin(x43 + (-1 * (*bsc0).gibpha) + (-1 * x32)) * (*bsc0).gibmag); } // Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = 1.0/2.0 * x0; const FLT x2 = dt * dt; const FLT x3 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x4 = x2 * x3; const FLT x5 = (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x6 = x2 * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x8 = x2 * x7; const FLT x9 = 1e-10 + x4 + x6 + x8; const FLT x10 = sqrt(x9); const FLT x11 = 0.5 * x10; const FLT x12 = sin(x11); const FLT x13 = x12 * x12; const FLT x14 = 1. / x9; const FLT x15 = x14 * x13; const FLT x16 = cos(x11); const FLT x17 = (x8 * x15) + (x6 * x15) + (x4 * x15) + (x16 * x16); const FLT x18 = 1. / sqrt(x17); const FLT x19 = x12 * x18; const FLT x20 = 1. / x10; const FLT x21 = dt * x20; const FLT x22 = x21 * x19; const FLT x23 = x22 * (*_x0).Pose.Rot[0]; const FLT x24 = x18 * x16; const FLT x25 = x24 * (*_x0).Pose.Rot[2]; const FLT x26 = x19 * (*_x0).Pose.Rot[1]; const FLT x27 = x21 * x26; const FLT x28 = x22 * (*_x0).Pose.Rot[3]; const FLT x29 = (-1 * x28 * (*_x0).Velocity.AxisAngleRot[0]) + (x27 * (*_x0).Velocity.AxisAngleRot[2]) + (x23 * (*_x0).Velocity.AxisAngleRot[1]) + x25; const FLT x30 = x22 * (*_x0).Pose.Rot[2]; const FLT x31 = x24 * (*_x0).Pose.Rot[0]; const FLT x32 = (-1 * x27 * (*_x0).Velocity.AxisAngleRot[0]) + x31 + (-1 * x28 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x30 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x33 = x24 * (*_x0).Pose.Rot[3]; const FLT x34 = x33 + (x23 * (*_x0).Velocity.AxisAngleRot[2]) + (x30 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x27 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x35 = (-1 * x34 * sensor_pt[1]) + (x29 * sensor_pt[2]) + (x32 * sensor_pt[0]); const FLT x36 = x24 * (*_x0).Pose.Rot[1]; const FLT x37 = (x23 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x30 * (*_x0).Velocity.AxisAngleRot[2]) + x36 + (x28 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x38 = (x34 * sensor_pt[0]) + (-1 * x37 * sensor_pt[2]) + (x32 * sensor_pt[1]); const FLT x39 = sensor_pt[2] + (x1 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (2 * ((x38 * x37) + (-1 * x35 * x29))) + (*_x0).Pose.Pos[2]; const FLT x40 = (-1 * x29 * sensor_pt[0]) + (x32 * sensor_pt[2]) + (x37 * sensor_pt[1]); const FLT x41 = (2 * ((x34 * x35) + (-1 * x40 * x37))) + (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (x1 * (*_x0).Acc[1]); const FLT x42 = (2 * ((x40 * x29) + (-1 * x34 * x38))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x1 * (*_x0).Acc[0]); const FLT x43 = (-1 * x42 * (*lh_p).Rot[2]) + (x39 * (*lh_p).Rot[0]) + (x41 * (*lh_p).Rot[1]); const FLT x44 = (x39 * (*lh_p).Rot[2]) + (-1 * x41 * (*lh_p).Rot[3]) + (x42 * (*lh_p).Rot[0]); const FLT x45 = x41 + (*lh_p).Pos[1] + (2 * ((x44 * (*lh_p).Rot[3]) + (-1 * x43 * (*lh_p).Rot[1]))); const FLT x46 = x45 * x45; const FLT x47 = (-1 * x39 * (*lh_p).Rot[1]) + (x41 * (*lh_p).Rot[0]) + (x42 * (*lh_p).Rot[3]); const FLT x48 = x39 + (2 * ((x47 * (*lh_p).Rot[1]) + (-1 * x44 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x49 = x42 + (2 * ((x43 * (*lh_p).Rot[2]) + (-1 * x47 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x50 = x49 * x49; const FLT x51 = x50 + (x48 * x48); const FLT x52 = x51 + x46; const FLT x53 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x54 = cos(x53); const FLT x55 = 1. / x54; const FLT x56 = (1. / sqrt(x52)) * x55; const FLT x57 = asin(x56 * x45); const FLT x58 = 8.0108022e-06 * x57; const FLT x59 = -8.0108022e-06 + (-1 * x58); const FLT x60 = 0.0028679863 + (x57 * x59); const FLT x61 = 5.3685255e-06 + (x60 * x57); const FLT x62 = 0.0076069798 + (x61 * x57); const FLT x63 = x62 * x57; const FLT x64 = -8.0108022e-06 + (-1.60216044e-05 * x57); const FLT x65 = x60 + (x64 * x57); const FLT x66 = x61 + (x65 * x57); const FLT x67 = x62 + (x66 * x57); const FLT x68 = (x67 * x57) + x63; const FLT x69 = sin(x53); const FLT x70 = atan2(-1 * x48, x49); const FLT x71 = tan(x53); const FLT x72 = x71 * (1. / sqrt(x51)); const FLT x73 = -1 * x72 * x45; const FLT x74 = (-1 * asin(x73)) + (*bsc0).ogeephase + x70; const FLT x75 = (sin(x74) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x76 = x75 * x69; const FLT x77 = x54 + (x76 * x68); const FLT x78 = 1. / x77; const FLT x79 = x57 * x57; const FLT x80 = x79 * x75; const FLT x81 = x80 * x78; const FLT x82 = x73 + (x81 * x62); const FLT x83 = 1. / sqrt(1 + (-1 * (x82 * x82))); const FLT x84 = 1. / x51; const FLT x85 = 1. / sqrt(1 + (-1 * x84 * (x71 * x71) * x46)); const FLT x86 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x87 = -1 * x0 * x86; const FLT x88 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x89 = x1 + (-1 * x0 * x88); const FLT x90 = x89 + x87; const FLT x91 = 2 * x49; const FLT x92 = x0 * (*lh_p).Rot[2] * (*lh_p).Rot[0]; const FLT x93 = x0 * (*lh_p).Rot[3]; const FLT x94 = x93 * (*lh_p).Rot[1]; const FLT x95 = x94 + (-1 * x92); const FLT x96 = 2 * x48; const FLT x97 = (x96 * x95) + (x91 * x90); const FLT x98 = 1.0/2.0 * x45; const FLT x99 = x71 * (1. / (x51 * sqrt(x51))) * x98; const FLT x100 = x0 * (*lh_p).Rot[1]; const FLT x101 = x100 * (*lh_p).Rot[2]; const FLT x102 = x93 * (*lh_p).Rot[0]; const FLT x103 = x102 + x101; const FLT x104 = (-1 * x72 * x103) + (x99 * x97); const FLT x105 = (1. / x50) * x48; const FLT x106 = 1. / x49; const FLT x107 = x84 * x50; const FLT x108 = ((-1 * x95 * x106) + (x90 * x105)) * x107; const FLT x109 = x108 + (-1 * x85 * x104); const FLT x110 = cos(x74) * (*bsc0).ogeemag; const FLT x111 = x68 * x69; const FLT x112 = x110 * x111; const FLT x113 = 1. / sqrt(1 + (-1 * (1. / (x54 * x54)) * (1. / x52) * x46)); const FLT x114 = 2 * x45; const FLT x115 = (1. / (x52 * sqrt(x52))) * x55 * x98; const FLT x116 = (-1 * x115 * (x97 + (x103 * x114))) + (x56 * x103); const FLT x117 = x113 * x116; const FLT x118 = x59 * x117; const FLT x119 = 2.40324066e-05 * x57; const FLT x120 = x64 * x113; const FLT x121 = x60 * x113; const FLT x122 = (x57 * ((-1 * x58 * x117) + x118)) + (x116 * x121); const FLT x123 = x66 * x113; const FLT x124 = (x57 * x122) + (x61 * x117); const FLT x125 = x67 * x113; const FLT x126 = x62 * x113; const FLT x127 = x80 * (1. / (x77 * x77)) * x62; const FLT x128 = 2 * x78 * x75 * x63; const FLT x129 = x79 * x78 * x62; const FLT x130 = x110 * x129; const FLT x131 = x83 * ((x109 * x130) + (-1 * x127 * ((x76 * ((x116 * x126) + (x57 * (x124 + (x57 * (x122 + (x57 * ((x116 * x120) + x118 + (-1 * x119 * x117))) + (x65 * x117))) + (x116 * x123))) + (x57 * x124) + (x116 * x125))) + (x109 * x112))) + x104 + (x117 * x128) + (x81 * x124)); const FLT x132 = cos((-1 * asin(x82)) + (*bsc0).gibpha + x70) * (*bsc0).gibmag; const FLT x133 = x101 + (-1 * x102); const FLT x134 = x93 * (*lh_p).Rot[2]; const FLT x135 = x100 * (*lh_p).Rot[0]; const FLT x136 = x135 + x134; const FLT x137 = (x96 * x136) + (x91 * x133); const FLT x138 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x139 = -1 * x0 * x138; const FLT x140 = x89 + x139; const FLT x141 = (-1 * x72 * x140) + (x99 * x137); const FLT x142 = ((-1 * x106 * x136) + (x105 * x133)) * x107; const FLT x143 = x142 + (-1 * x85 * x141); const FLT x144 = x113 * ((-1 * x115 * (x137 + (x114 * x140))) + (x56 * x140)); const FLT x145 = x59 * x144; const FLT x146 = (x57 * ((-1 * x58 * x144) + x145)) + (x60 * x144); const FLT x147 = (x57 * x146) + (x61 * x144); const FLT x148 = x83 * ((-1 * x127 * ((x76 * ((x62 * x144) + (x57 * (x147 + (x57 * (x146 + (x57 * ((x64 * x144) + x145 + (-1 * x119 * x144))) + (x65 * x144))) + (x66 * x144))) + (x57 * x147) + (x67 * x144))) + (x112 * x143))) + x141 + (x130 * x143) + (x81 * x147) + (x128 * x144)); const FLT x149 = x134 + (-1 * x135); const FLT x150 = x92 + x94; const FLT x151 = x87 + x1 + x139; const FLT x152 = (x96 * x151) + (x91 * x150); const FLT x153 = (-1 * x115 * (x152 + (x114 * x149))) + (x56 * x149); const FLT x154 = x61 * x113; const FLT x155 = x59 * x113; const FLT x156 = x153 * x155; const FLT x157 = x58 * x113; const FLT x158 = (x57 * ((-1 * x153 * x157) + x156)) + (x121 * x153); const FLT x159 = (x57 * x158) + (x153 * x154); const FLT x160 = x113 * x128; const FLT x161 = (-1 * x72 * x149) + (x99 * x152); const FLT x162 = ((-1 * x106 * x151) + (x105 * x150)) * x107; const FLT x163 = x110 * (x162 + (-1 * x85 * x161)); const FLT x164 = x113 * x119; const FLT x165 = x65 * x113; const FLT x166 = x83 * ((x129 * x163) + (-1 * x127 * ((x76 * ((x126 * x153) + (x57 * x159) + (x57 * (x159 + (x57 * (x158 + (x57 * (x156 + (x120 * x153) + (-1 * x164 * x153))) + (x165 * x153))) + (x123 * x153))) + (x125 * x153))) + (x111 * x163))) + x161 + (x81 * x159) + (x160 * x153)); const FLT x167 = 2 * x86; const FLT x168 = -1 * x167; const FLT x169 = 2 * x88; const FLT x170 = 1 + (-1 * x169); const FLT x171 = x170 + x168; const FLT x172 = 2 * (*lh_p).Rot[2]; const FLT x173 = x172 * (*lh_p).Rot[0]; const FLT x174 = 2 * (*lh_p).Rot[1]; const FLT x175 = x174 * (*lh_p).Rot[3]; const FLT x176 = x175 + (-1 * x173); const FLT x177 = (x96 * x176) + (x91 * x171); const FLT x178 = x172 * (*lh_p).Rot[1]; const FLT x179 = 2 * (*lh_p).Rot[3]; const FLT x180 = x179 * (*lh_p).Rot[0]; const FLT x181 = x180 + x178; const FLT x182 = (-1 * x72 * x181) + (x99 * x177); const FLT x183 = ((-1 * x106 * x176) + (x105 * x171)) * x107; const FLT x184 = x183 + (-1 * x85 * x182); const FLT x185 = (-1 * x115 * (x177 + (x114 * x181))) + (x56 * x181); const FLT x186 = x113 * x185; const FLT x187 = x59 * x186; const FLT x188 = (x57 * ((-1 * x58 * x186) + x187)) + (x121 * x185); const FLT x189 = (x57 * x188) + (x61 * x186); const FLT x190 = x83 * (x182 + (x184 * x130) + (x81 * x189) + (-1 * x127 * ((x76 * ((x126 * x185) + (x57 * x189) + (x57 * (x189 + (x57 * (x188 + (x57 * (x187 + (x120 * x185) + (-1 * x119 * x186))) + (x65 * x186))) + (x123 * x185))) + (x67 * x186))) + (x112 * x184))) + (x128 * x186)); const FLT x191 = x178 + (-1 * x180); const FLT x192 = x172 * (*lh_p).Rot[3]; const FLT x193 = x174 * (*lh_p).Rot[0]; const FLT x194 = x193 + x192; const FLT x195 = (x96 * x194) + (x91 * x191); const FLT x196 = 2 * x138; const FLT x197 = -1 * x196; const FLT x198 = x170 + x197; const FLT x199 = (-1 * x72 * x198) + (x99 * x195); const FLT x200 = ((-1 * x106 * x194) + (x105 * x191)) * x107; const FLT x201 = x200 + (-1 * x85 * x199); const FLT x202 = (-1 * x115 * (x195 + (x114 * x198))) + (x56 * x198); const FLT x203 = x202 * x155; const FLT x204 = (x57 * ((-1 * x202 * x157) + x203)) + (x202 * x121); const FLT x205 = (x57 * x204) + (x202 * x154); const FLT x206 = x83 * (x199 + (x201 * x130) + (x81 * x205) + (-1 * x127 * ((x76 * ((x57 * x205) + (x57 * (x205 + (x57 * (x204 + (x57 * ((x202 * x120) + x203 + (-1 * x202 * x164))) + (x202 * x165))) + (x202 * x123))) + (x202 * x126) + (x202 * x125))) + (x201 * x112))) + (x202 * x160)); const FLT x207 = x192 + (-1 * x193); const FLT x208 = x173 + x175; const FLT x209 = 1 + x197 + x168; const FLT x210 = (x96 * x209) + (x91 * x208); const FLT x211 = (-1 * x115 * (x210 + (x207 * x114))) + (x56 * x207); const FLT x212 = x211 * x155; const FLT x213 = (x57 * ((-1 * x211 * x157) + x212)) + (x211 * x121); const FLT x214 = (x57 * x213) + (x211 * x154); const FLT x215 = (-1 * x72 * x207) + (x99 * x210); const FLT x216 = ((-1 * x209 * x106) + (x208 * x105)) * x107; const FLT x217 = x216 + (-1 * x85 * x215); const FLT x218 = x83 * ((x211 * x160) + x215 + (x81 * x214) + (x217 * x130) + (-1 * x127 * ((x76 * ((x211 * x126) + (x57 * x214) + (x57 * (x214 + (x57 * ((x57 * ((x211 * x120) + x212 + (-1 * x211 * x164))) + x213 + (x211 * x165))) + (x211 * x123))) + (x211 * x125))) + (x217 * x112)))); const FLT x219 = x22 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x220 = -1 * x219 * sensor_pt[2]; const FLT x221 = x22 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x222 = x221 * sensor_pt[0]; const FLT x223 = x24 * sensor_pt[1]; const FLT x224 = x223 + x220 + x222; const FLT x225 = 2 * x34; const FLT x226 = 2 * x38; const FLT x227 = -1 * x221 * x226; const FLT x228 = x22 * sensor_pt[1]; const FLT x229 = x228 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x230 = x22 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x231 = -1 * x230 * sensor_pt[0]; const FLT x232 = x24 * sensor_pt[2]; const FLT x233 = x232 + x231; const FLT x234 = x233 + x229; const FLT x235 = 2 * x29; const FLT x236 = 2 * x40; const FLT x237 = x230 * x236; const FLT x238 = (x234 * x235) + x237 + (-1 * x224 * x225) + x227; const FLT x239 = x230 * sensor_pt[2]; const FLT x240 = -1 * x228 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x241 = x24 * sensor_pt[0]; const FLT x242 = x241 + x240; const FLT x243 = x242 + x239; const FLT x244 = x219 * x226; const FLT x245 = 2 * x35; const FLT x246 = -1 * x230 * x245; const FLT x247 = 2 * x37; const FLT x248 = (x224 * x247) + x246 + (-1 * x235 * x243) + x244; const FLT x249 = x221 * x245; const FLT x250 = -1 * x219 * x236; const FLT x251 = x250 + x249 + (-1 * x234 * x247) + (x225 * x243); const FLT x252 = (x251 * (*lh_p).Rot[1]) + (-1 * x238 * (*lh_p).Rot[2]) + (x248 * (*lh_p).Rot[0]); const FLT x253 = (x238 * (*lh_p).Rot[0]) + (-1 * x251 * (*lh_p).Rot[3]) + (x248 * (*lh_p).Rot[2]); const FLT x254 = x251 + (-1 * x252 * x174) + (x253 * x179); const FLT x255 = (x238 * (*lh_p).Rot[3]) + (-1 * x248 * (*lh_p).Rot[1]) + (x251 * (*lh_p).Rot[0]); const FLT x256 = x238 + (-1 * x255 * x179) + (x252 * x172); const FLT x257 = x248 + (-1 * x253 * x172) + (x255 * x174); const FLT x258 = (x96 * x257) + (x91 * x256); const FLT x259 = (-1 * x115 * (x258 + (x254 * x114))) + (x56 * x254); const FLT x260 = x259 * x113; const FLT x261 = x59 * x260; const FLT x262 = (x57 * ((-1 * x58 * x260) + x261)) + (x259 * x121); const FLT x263 = (x57 * x262) + (x61 * x260); const FLT x264 = (-1 * x72 * x254) + (x99 * x258); const FLT x265 = ((-1 * x257 * x106) + (x256 * x105)) * x107; const FLT x266 = x265 + (-1 * x85 * x264); const FLT x267 = x83 * (x264 + (x266 * x130) + (-1 * x127 * ((x76 * ((x259 * x126) + (x57 * x263) + (x57 * (x263 + (x57 * (x262 + (x57 * ((x259 * x120) + x261 + (-1 * x260 * x119))) + (x259 * x165))) + (x259 * x123))) + (x259 * x125))) + (x266 * x112))) + (x260 * x128) + (x81 * x263)); const FLT x268 = x230 * sensor_pt[1]; const FLT x269 = x221 * sensor_pt[2]; const FLT x270 = x219 * sensor_pt[0]; const FLT x271 = (-1 * x270) + x268 + x269; const FLT x272 = -1 * x229; const FLT x273 = x272 + (-1 * x232) + x231; const FLT x274 = x24 * x226; const FLT x275 = (-1 * x249) + (-1 * x235 * x271) + x274 + (x273 * x247); const FLT x276 = (-1 * x222) + x220; const FLT x277 = x276 + x223; const FLT x278 = x24 * x236; const FLT x279 = x246 + (-1 * x277 * x247) + (-1 * x278) + (x271 * x225); const FLT x280 = x236 * x221; const FLT x281 = x230 * x226; const FLT x282 = (x235 * x277) + x280 + x281 + (-1 * x273 * x225); const FLT x283 = (x282 * (*lh_p).Rot[3]) + (-1 * x275 * (*lh_p).Rot[1]) + (x279 * (*lh_p).Rot[0]); const FLT x284 = (x279 * (*lh_p).Rot[1]) + (-1 * x282 * (*lh_p).Rot[2]) + (x275 * (*lh_p).Rot[0]); const FLT x285 = x282 + (-1 * x283 * x179) + (x284 * x172); const FLT x286 = (x282 * (*lh_p).Rot[0]) + (-1 * x279 * (*lh_p).Rot[3]) + (x275 * (*lh_p).Rot[2]); const FLT x287 = x275 + (-1 * x286 * x172) + (x283 * x174); const FLT x288 = (x96 * x287) + (x91 * x285); const FLT x289 = x279 + (-1 * x284 * x174) + (x286 * x179); const FLT x290 = (-1 * x72 * x289) + (x99 * x288); const FLT x291 = ((-1 * x287 * x106) + (x285 * x105)) * x107; const FLT x292 = x291 + (-1 * x85 * x290); const FLT x293 = (-1 * x115 * (x288 + (x289 * x114))) + (x56 * x289); const FLT x294 = x293 * x155; const FLT x295 = (x57 * ((-1 * x293 * x157) + x294)) + (x293 * x121); const FLT x296 = (x57 * x295) + (x293 * x154); const FLT x297 = x83 * ((x292 * x130) + (x81 * x296) + x290 + (-1 * x127 * ((x76 * ((x293 * x126) + (x57 * x296) + (x57 * (x296 + (x57 * ((x57 * ((x293 * x120) + x294 + (-1 * x293 * x164))) + x295 + (x293 * x165))) + (x293 * x123))) + (x293 * x125))) + (x292 * x112))) + (x293 * x160)); const FLT x298 = -1 * x239; const FLT x299 = (-1 * x241) + x240 + x298; const FLT x300 = x233 + x272; const FLT x301 = x219 * x245; const FLT x302 = x301 + (x225 * x300) + (-1 * x299 * x247) + x280; const FLT x303 = x24 * x245; const FLT x304 = (-1 * x268) + x269 + x270; const FLT x305 = (x247 * x304) + (-1 * x235 * x300) + x227 + (-1 * x303); const FLT x306 = (x235 * x299) + x278 + (-1 * x244) + (-1 * x225 * x304); const FLT x307 = (x306 * (*lh_p).Rot[3]) + (x302 * (*lh_p).Rot[0]) + (-1 * x305 * (*lh_p).Rot[1]); const FLT x308 = (x302 * (*lh_p).Rot[1]) + (x305 * (*lh_p).Rot[0]) + (-1 * x306 * (*lh_p).Rot[2]); const FLT x309 = x306 + (-1 * x307 * x179) + (x308 * x172); const FLT x310 = (x306 * (*lh_p).Rot[0]) + (-1 * x302 * (*lh_p).Rot[3]) + (x305 * (*lh_p).Rot[2]); const FLT x311 = x305 + (-1 * x310 * x172) + (x307 * x174); const FLT x312 = (x96 * x311) + (x91 * x309); const FLT x313 = x302 + (-1 * x308 * x174) + (x310 * x179); const FLT x314 = (-1 * x72 * x313) + (x99 * x312); const FLT x315 = ((-1 * x311 * x106) + (x309 * x105)) * x107; const FLT x316 = x315 + (-1 * x85 * x314); const FLT x317 = (-1 * x115 * (x312 + (x313 * x114))) + (x56 * x313); const FLT x318 = x317 * x155; const FLT x319 = (x57 * ((-1 * x317 * x157) + x318)) + (x317 * x121); const FLT x320 = (x57 * x319) + (x317 * x154); const FLT x321 = x83 * ((x316 * x130) + (x81 * x320) + x314 + (-1 * x127 * ((x76 * ((x57 * x320) + (x57 * (x320 + (x57 * (x319 + (x57 * ((x317 * x120) + x318 + (-1 * x317 * x164))) + (x317 * x165))) + (x317 * x123))) + (x317 * x126) + (x317 * x125))) + (x316 * x112))) + (x317 * x160)); const FLT x322 = 2 * (x268 + x270 + (-1 * x269)); const FLT x323 = x276 + (-1 * x223); const FLT x324 = (-1 * x237) + x303 + (-1 * x37 * x322) + (x225 * x323); const FLT x325 = x242 + x298; const FLT x326 = (x247 * x325) + x281 + (-1 * x235 * x323) + x301; const FLT x327 = x250 + (-1 * x274) + (x29 * x322) + (-1 * x225 * x325); const FLT x328 = (x327 * (*lh_p).Rot[0]) + (-1 * x324 * (*lh_p).Rot[3]) + (x326 * (*lh_p).Rot[2]); const FLT x329 = (x324 * (*lh_p).Rot[1]) + (-1 * x327 * (*lh_p).Rot[2]) + (x326 * (*lh_p).Rot[0]); const FLT x330 = x324 + (x328 * x179) + (-1 * x329 * x174); const FLT x331 = (x327 * (*lh_p).Rot[3]) + (-1 * x326 * (*lh_p).Rot[1]) + (x324 * (*lh_p).Rot[0]); const FLT x332 = x327 + (-1 * x331 * x179) + (x329 * x172); const FLT x333 = x326 + (-1 * x328 * x172) + (x331 * x174); const FLT x334 = (x96 * x333) + (x91 * x332); const FLT x335 = (-1 * x115 * (x334 + (x330 * x114))) + (x56 * x330); const FLT x336 = x335 * x155; const FLT x337 = (x57 * ((-1 * x335 * x157) + x336)) + (x335 * x121); const FLT x338 = (x57 * x337) + (x335 * x154); const FLT x339 = (-1 * x72 * x330) + (x99 * x334); const FLT x340 = ((-1 * x333 * x106) + (x332 * x105)) * x107; const FLT x341 = x340 + (-1 * x85 * x339); const FLT x342 = x83 * (x339 + (-1 * x127 * ((x76 * ((x57 * x338) + (x335 * x126) + (x57 * (x338 + (x57 * (x337 + (x57 * (x336 + (x335 * x120) + (-1 * x335 * x164))) + (x335 * x165))) + (x335 * x123))) + (x335 * x125))) + (x341 * x112))) + (x341 * x130) + (x335 * x160) + (x81 * x338)); const FLT x343 = x19 * (*_x0).Pose.Rot[2]; const FLT x344 = 0.5 * x20; const FLT x345 = x2 * x344; const FLT x346 = x345 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x347 = -1 * x28; const FLT x348 = dt * dt * dt; const FLT x349 = 0.5 * x14 * x348; const FLT x350 = x33 * x349; const FLT x351 = 1.0/2.0 * (1. / (x17 * sqrt(x17))); const FLT x352 = x21 * x12 * x351; const FLT x353 = x352 * (*_x0).Pose.Rot[3]; const FLT x354 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x355 = dt * dt * dt * dt; const FLT x356 = 2 * (1. / (x9 * x9)) * x13; const FLT x357 = x356 * x355; const FLT x358 = 1. / (x9 * sqrt(x9)); const FLT x359 = 1.0 * x12 * x16; const FLT x360 = x359 * x358; const FLT x361 = x360 * x355; const FLT x362 = x3 * x361; const FLT x363 = x3 * x357; const FLT x364 = x20 * x359; const FLT x365 = x2 * x364; const FLT x366 = 2 * x15; const FLT x367 = x2 * x366; const FLT x368 = x5 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x369 = (-1 * x368 * x357) + (x361 * x368) + (x367 * (*_x0).Velocity.AxisAngleRot[0]) + (x362 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x354 * x357) + (-1 * x363 * (*_x0).Velocity.AxisAngleRot[0]) + (x361 * x354) + (-1 * x365 * (*_x0).Velocity.AxisAngleRot[0]); const FLT x370 = x369 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x371 = x16 * x351; const FLT x372 = x371 * x369; const FLT x373 = x352 * (*_x0).Pose.Rot[0]; const FLT x374 = x369 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x375 = x358 * x348; const FLT x376 = x375 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x377 = x376 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x378 = x26 * x377; const FLT x379 = (*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x380 = x379 * x349; const FLT x381 = x36 * x380; const FLT x382 = x352 * (*_x0).Pose.Rot[1]; const FLT x383 = x369 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x384 = x19 * (*_x0).Pose.Rot[3]; const FLT x385 = x7 * x375; const FLT x386 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[0]; const FLT x387 = x31 * x349; const FLT x388 = x19 * (*_x0).Pose.Rot[0]; const FLT x389 = x388 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x390 = x375 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x391 = (-1 * x390 * x389) + (x387 * x386); const FLT x392 = x381 + (-1 * x378) + (-1 * x374 * x373) + x391 + (x384 * x385) + x347 + (-1 * x383 * x382) + (-1 * x346 * x343) + (-1 * x7 * x350) + (x370 * x353) + (-1 * x372 * (*_x0).Pose.Rot[2]); const FLT x393 = x350 * x386; const FLT x394 = x390 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x395 = x394 * x384; const FLT x396 = x25 * x380; const FLT x397 = x377 * x343; const FLT x398 = x352 * (*_x0).Pose.Rot[2]; const FLT x399 = x7 * x349; const FLT x400 = x19 * x385; const FLT x401 = (-1 * x373 * x370) + x23 + (-1 * x26 * x346) + x393 + (x31 * x399) + (-1 * x372 * (*_x0).Pose.Rot[1]) + (-1 * x395) + x397 + (x398 * x383) + (-1 * x396) + (-1 * x400 * (*_x0).Pose.Rot[0]) + (-1 * x374 * x353); const FLT x402 = (x31 * x380) + (-1 * x376 * x389); const FLT x403 = x36 * x349; const FLT x404 = (-1 * x403 * x386) + (x26 * x394); const FLT x405 = (-1 * x398 * x370) + (-1 * x384 * x346) + x404 + x30 + (-1 * x373 * x383) + (-1 * x372 * (*_x0).Pose.Rot[3]) + (x25 * x399) + (-1 * x400 * (*_x0).Pose.Rot[2]) + x402 + (x374 * x382); const FLT x406 = x25 * x349; const FLT x407 = x406 * x386; const FLT x408 = x343 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x409 = x408 * x375 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x410 = -1 * x27; const FLT x411 = (-1 * x379 * x350) + (x377 * x384); const FLT x412 = (x26 * x385) + x410 + (-1 * x389 * x345) + (x382 * x370) + x411 + (-1 * x36 * x399) + (-1 * x372 * (*_x0).Pose.Rot[0]) + (-1 * x407) + x409 + (x374 * x398) + (x383 * x353); const FLT x413 = (x412 * sensor_pt[1]) + (-1 * x401 * sensor_pt[2]) + (x405 * sensor_pt[0]); const FLT x414 = (x401 * sensor_pt[1]) + (-1 * x392 * sensor_pt[0]) + (x412 * sensor_pt[2]); const FLT x415 = (x414 * x235) + (-1 * x405 * x226) + (x236 * x392) + (-1 * x413 * x225); const FLT x416 = (x412 * sensor_pt[0]) + (-1 * x405 * sensor_pt[1]) + (x392 * sensor_pt[2]); const FLT x417 = (-1 * x416 * x235) + (x401 * x226) + (x413 * x247) + (-1 * x245 * x392); const FLT x418 = (-1 * x401 * x236) + (x416 * x225) + (-1 * x414 * x247) + (x405 * x245); const FLT x419 = (-1 * x415 * (*lh_p).Rot[2]) + (x418 * (*lh_p).Rot[1]) + (x417 * (*lh_p).Rot[0]); const FLT x420 = (x415 * (*lh_p).Rot[0]) + (x417 * (*lh_p).Rot[2]) + (-1 * x418 * (*lh_p).Rot[3]); const FLT x421 = x418 + (-1 * x419 * x174) + (x420 * x179); const FLT x422 = (-1 * x417 * (*lh_p).Rot[1]) + (x415 * (*lh_p).Rot[3]) + (x418 * (*lh_p).Rot[0]); const FLT x423 = x415 + (-1 * x422 * x179) + (x419 * x172); const FLT x424 = x417 + (-1 * x420 * x172) + (x422 * x174); const FLT x425 = (x96 * x424) + (x91 * x423); const FLT x426 = x113 * ((-1 * x115 * (x425 + (x421 * x114))) + (x56 * x421)); const FLT x427 = x59 * x426; const FLT x428 = (x57 * ((-1 * x58 * x426) + x427)) + (x60 * x426); const FLT x429 = (x57 * x428) + (x61 * x426); const FLT x430 = (-1 * x72 * x421) + (x99 * x425); const FLT x431 = ((-1 * x424 * x106) + (x423 * x105)) * x107; const FLT x432 = x431 + (-1 * x85 * x430); const FLT x433 = x83 * (x430 + (x432 * x130) + (x426 * x128) + (x81 * x429) + (-1 * x127 * ((x76 * ((x57 * x429) + (x62 * x426) + (x57 * (x429 + (x57 * (x428 + (x57 * ((x64 * x426) + (-1 * x426 * x119) + x427)) + (x65 * x426))) + (x66 * x426))) + (x67 * x426))) + (x432 * x112)))); const FLT x434 = x357 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x435 = x361 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x436 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]; const FLT x437 = (x436 * x361) + (-1 * x5 * x434) + (-1 * x365 * (*_x0).Velocity.AxisAngleRot[1]) + (x5 * x435) + (-1 * x7 * x434) + (-1 * x436 * x357) + (x7 * x435) + (x367 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x438 = x437 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x439 = x3 * x375; const FLT x440 = x437 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x441 = x437 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x442 = x3 * x349; const FLT x443 = x437 * x371; const FLT x444 = x345 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x445 = x376 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x446 = (*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[2]; const FLT x447 = (-1 * x406 * x446) + (x445 * x343); const FLT x448 = x447 + x391 + (-1 * x443 * (*_x0).Pose.Rot[1]) + (-1 * x26 * x444) + (x33 * x442) + x28 + (-1 * x438 * x373) + (-1 * x439 * x384) + (x440 * x398) + (-1 * x441 * x353); const FLT x449 = (x446 * x387) + (-1 * x445 * x388); const FLT x450 = (-1 * x444 * x384) + (-1 * x409) + (-1 * x36 * x442) + (-1 * x438 * x398) + x407 + (x26 * x439) + (-1 * x443 * (*_x0).Pose.Rot[3]) + x449 + x410 + (-1 * x440 * x373) + (x441 * x382); const FLT x451 = -1 * x30; const FLT x452 = x446 * x350; const FLT x453 = x445 * x384; const FLT x454 = x404 + (-1 * x444 * x388) + x453 + (-1 * x443 * (*_x0).Pose.Rot[0]) + (x440 * x353) + (-1 * x452) + (x439 * x343) + (x441 * x398) + x451 + (x438 * x382) + (-1 * x25 * x442); const FLT x455 = (x454 * sensor_pt[1]) + (-1 * x448 * sensor_pt[2]) + (x450 * sensor_pt[0]); const FLT x456 = x26 * x445; const FLT x457 = x403 * x446; const FLT x458 = (-1 * x393) + (-1 * x441 * x373) + (-1 * x439 * x388) + x457 + (x438 * x353) + x395 + (-1 * x456) + (-1 * x443 * (*_x0).Pose.Rot[2]) + (-1 * x440 * x382) + x23 + (-1 * x408 * x345) + (x31 * x442); const FLT x459 = (x448 * sensor_pt[1]) + (-1 * x458 * sensor_pt[0]) + (x454 * sensor_pt[2]); const FLT x460 = (-1 * x450 * x226) + (x458 * x236) + (-1 * x455 * x225) + (x459 * x235); const FLT x461 = 2 * ((-1 * x450 * sensor_pt[1]) + (x454 * sensor_pt[0]) + (x458 * sensor_pt[2])); const FLT x462 = (x455 * x247) + (-1 * x29 * x461) + (-1 * x458 * x245) + (x448 * x226); const FLT x463 = (x450 * x245) + (x34 * x461) + (-1 * x459 * x247) + (-1 * x448 * x236); const FLT x464 = (-1 * x460 * (*lh_p).Rot[2]) + (x463 * (*lh_p).Rot[1]) + (x462 * (*lh_p).Rot[0]); const FLT x465 = (x460 * (*lh_p).Rot[0]) + (-1 * x463 * (*lh_p).Rot[3]) + (x462 * (*lh_p).Rot[2]); const FLT x466 = x463 + (-1 * x464 * x174) + (x465 * x179); const FLT x467 = (x460 * (*lh_p).Rot[3]) + (-1 * x462 * (*lh_p).Rot[1]) + (x463 * (*lh_p).Rot[0]); const FLT x468 = x460 + (-1 * x467 * x179) + (x464 * x172); const FLT x469 = x462 + (-1 * x465 * x172) + (x467 * x174); const FLT x470 = (x96 * x469) + (x91 * x468); const FLT x471 = x113 * ((-1 * x115 * (x470 + (x466 * x114))) + (x56 * x466)); const FLT x472 = x59 * x471; const FLT x473 = (x57 * ((-1 * x58 * x471) + x472)) + (x60 * x471); const FLT x474 = (x57 * x473) + (x61 * x471); const FLT x475 = (-1 * x72 * x466) + (x99 * x470); const FLT x476 = ((-1 * x469 * x106) + (x468 * x105)) * x107; const FLT x477 = x476 + (-1 * x85 * x475); const FLT x478 = x83 * (x475 + (x471 * x128) + (x477 * x130) + (x81 * x474) + (-1 * x127 * ((x76 * ((x62 * x471) + (x57 * x474) + (x57 * (x474 + (x57 * ((x57 * ((x64 * x471) + x472 + (-1 * x471 * x119))) + x473 + (x65 * x471))) + (x66 * x471))) + (x67 * x471))) + (x477 * x112)))); const FLT x479 = x7 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x480 = x355 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x481 = x2 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x482 = (-1 * x480 * x356) + (-1 * x363 * (*_x0).Velocity.AxisAngleRot[2]) + (x481 * x366) + (-1 * x479 * x357) + (-1 * x481 * x364) + (x479 * x361) + (x480 * x360) + (x362 * (*_x0).Velocity.AxisAngleRot[2]); const FLT x483 = x482 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x484 = x5 * x375; const FLT x485 = x482 * x353; const FLT x486 = x482 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x487 = x482 * x371; const FLT x488 = x481 * x344; const FLT x489 = x5 * x349; const FLT x490 = (-1 * x25 * x489) + (-1 * x26 * x488) + (-1 * x487 * (*_x0).Pose.Rot[1]) + (-1 * x485 * (*_x0).Velocity.AxisAngleRot[1]) + (x484 * x343) + (x483 * x398) + (-1 * x453) + x452 + x402 + (-1 * x486 * x373) + x451; const FLT x491 = x482 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x492 = x491 * x352; const FLT x493 = (-1 * x397) + x23 + (-1 * x486 * x398) + x396 + (x492 * (*_x0).Pose.Rot[1]) + (-1 * x484 * x388) + x456 + (-1 * x483 * x373) + (x31 * x489) + (-1 * x488 * x384) + (-1 * x487 * (*_x0).Pose.Rot[3]) + (-1 * x457); const FLT x494 = (-1 * x488 * x388) + (x486 * x382) + (-1 * x487 * (*_x0).Pose.Rot[0]) + x347 + x447 + (-1 * x5 * x350) + (-1 * x381) + (x485 * (*_x0).Velocity.AxisAngleRot[2]) + (x484 * x384) + x378 + (x491 * x398); const FLT x495 = (x494 * sensor_pt[1]) + (-1 * x490 * sensor_pt[2]) + (x493 * sensor_pt[0]); const FLT x496 = x411 + (-1 * x483 * x382) + (-1 * x26 * x484) + x27 + (-1 * x488 * x343) + x449 + (x36 * x489) + (x485 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x492 * (*_x0).Pose.Rot[0]) + (-1 * x487 * (*_x0).Pose.Rot[2]); const FLT x497 = 2 * x496; const FLT x498 = 2 * ((x490 * sensor_pt[1]) + (-1 * x496 * sensor_pt[0]) + (x494 * sensor_pt[2])); const FLT x499 = (x29 * x498) + (-1 * x493 * x226) + (-1 * x495 * x225) + (x40 * x497); const FLT x500 = (x494 * sensor_pt[0]) + (-1 * x493 * sensor_pt[1]) + (x496 * sensor_pt[2]); const FLT x501 = (x490 * x226) + (-1 * x35 * x497) + (-1 * x500 * x235) + (x495 * x247); const FLT x502 = (-1 * x490 * x236) + (x500 * x225) + (-1 * x37 * x498) + (x493 * x245); const FLT x503 = (-1 * x499 * (*lh_p).Rot[2]) + (x502 * (*lh_p).Rot[1]) + (x501 * (*lh_p).Rot[0]); const FLT x504 = 2 * ((x499 * (*lh_p).Rot[0]) + (-1 * x502 * (*lh_p).Rot[3]) + (x501 * (*lh_p).Rot[2])); const FLT x505 = (-1 * x503 * x174) + x502 + (x504 * (*lh_p).Rot[3]); const FLT x506 = 2 * ((x499 * (*lh_p).Rot[3]) + (-1 * x501 * (*lh_p).Rot[1]) + (x502 * (*lh_p).Rot[0])); const FLT x507 = x499 + (-1 * x506 * (*lh_p).Rot[3]) + (x503 * x172); const FLT x508 = x501 + (x506 * (*lh_p).Rot[1]) + (-1 * x504 * (*lh_p).Rot[2]); const FLT x509 = (x96 * x508) + (x91 * x507); const FLT x510 = (-1 * x115 * (x509 + (x505 * x114))) + (x56 * x505); const FLT x511 = x510 * x155; const FLT x512 = (x57 * ((-1 * x510 * x157) + x511)) + (x510 * x121); const FLT x513 = (x57 * x512) + (x510 * x154); const FLT x514 = (-1 * x72 * x505) + (x99 * x509); const FLT x515 = ((-1 * x508 * x106) + (x507 * x105)) * x107; const FLT x516 = x515 + (-1 * x85 * x514); const FLT x517 = x83 * ((x516 * x130) + (-1 * x127 * ((x76 * ((x510 * x126) + (x57 * (x513 + (x57 * (x512 + (x57 * ((x510 * x120) + x511 + (-1 * x510 * x164))) + (x510 * x165))) + (x510 * x123))) + (x57 * x513) + (x510 * x125))) + (x516 * x112))) + x514 + (x81 * x513) + (x510 * x160)); const FLT x518 = dt * x178; const FLT x519 = dt * x180; const FLT x520 = x519 + x518; const FLT x521 = -1 * dt * x169; const FLT x522 = (-1 * dt * x167) + dt; const FLT x523 = x522 + x521; const FLT x524 = dt * x173; const FLT x525 = dt * x175; const FLT x526 = x525 + (-1 * x524); const FLT x527 = (x96 * x526) + (x91 * x523); const FLT x528 = (-1 * x115 * (x527 + (x520 * x114))) + (x56 * x520); const FLT x529 = x528 * x155; const FLT x530 = (x57 * ((-1 * x528 * x157) + x529)) + (x528 * x121); const FLT x531 = (x57 * x530) + (x528 * x154); const FLT x532 = (-1 * x72 * x520) + (x99 * x527); const FLT x533 = ((-1 * x526 * x106) + (x523 * x105)) * x107; const FLT x534 = x533 + (-1 * x85 * x532); const FLT x535 = x83 * (x532 + (x534 * x130) + (-1 * x127 * ((x76 * ((x57 * (x531 + (x57 * (x530 + (x57 * ((x528 * x120) + x529 + (-1 * x528 * x164))) + (x528 * x165))) + (x528 * x123))) + (x528 * x126) + (x57 * x531) + (x528 * x125))) + (x534 * x112))) + (x528 * x160) + (x81 * x531)); const FLT x536 = -1 * dt * x196; const FLT x537 = x536 + x521 + dt; const FLT x538 = x518 + (-1 * x519); const FLT x539 = dt * x192; const FLT x540 = dt * x193; const FLT x541 = x540 + x539; const FLT x542 = (x96 * x541) + (x91 * x538); const FLT x543 = x113 * ((-1 * x115 * (x542 + (x537 * x114))) + (x56 * x537)); const FLT x544 = x59 * x543; const FLT x545 = (x57 * ((-1 * x58 * x543) + x544)) + (x60 * x543); const FLT x546 = (x57 * x545) + (x61 * x543); const FLT x547 = (-1 * x72 * x537) + (x99 * x542); const FLT x548 = ((-1 * x541 * x106) + (x538 * x105)) * x107; const FLT x549 = x548 + (-1 * x85 * x547); const FLT x550 = x83 * (x547 + (x543 * x128) + (x549 * x130) + (-1 * x127 * ((x76 * ((x62 * x543) + (x57 * x546) + (x57 * ((x57 * (x545 + (x57 * ((x64 * x543) + x544 + (-1 * x543 * x119))) + (x65 * x543))) + x546 + (x66 * x543))) + (x67 * x543))) + (x549 * x112))) + (x81 * x546)); const FLT x551 = x524 + x525; const FLT x552 = x522 + x536; const FLT x553 = (x96 * x552) + (x91 * x551); const FLT x554 = x539 + (-1 * x540); const FLT x555 = (-1 * x72 * x554) + (x99 * x553); const FLT x556 = ((-1 * x552 * x106) + (x551 * x105)) * x107; const FLT x557 = x556 + (-1 * x85 * x555); const FLT x558 = (-1 * x115 * (x553 + (x554 * x114))) + (x56 * x554); const FLT x559 = x558 * x155; const FLT x560 = x558 * x113; const FLT x561 = (x57 * ((-1 * x58 * x560) + x559)) + (x558 * x121); const FLT x562 = (x57 * x561) + (x558 * x154); const FLT x563 = x83 * ((x557 * x130) + (x560 * x128) + x555 + (-1 * x127 * ((x76 * ((x558 * x125) + (x57 * (x562 + (x57 * (x561 + (x57 * (x559 + (x558 * x120) + (-1 * x560 * x119))) + (x558 * x165))) + (x558 * x123))) + (x558 * x126) + (x57 * x562))) + (x557 * x112))) + (x81 * x562)); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x108 + (-1 * x131) + (-1 * ((-1 * x108) + x131) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x142 + (-1 * x148) + (-1 * ((-1 * x142) + x148) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x162 + (-1 * x166) + (-1 * ((-1 * x162) + x166) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x183 + (-1 * x190) + (-1 * ((-1 * x183) + x190) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x200 + (-1 * x206) + (-1 * ((-1 * x200) + x206) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x216 + (-1 * ((-1 * x216) + x218) * x132) + (-1 * x218)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x265 + (-1 * x267) + (-1 * ((-1 * x265) + x267) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x297) + x291 + (-1 * ((-1 * x291) + x297) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x315 + (-1 * x321) + (-1 * ((-1 * x315) + x321) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x342) + x340 + (-1 * ((-1 * x340) + x342) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (-1 * x433) + x431 + (-1 * ((-1 * x431) + x433) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x476 + (-1 * x478) + (-1 * ((-1 * x476) + x478) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x515 + (-1 * ((-1 * x515) + x517) * x132) + (-1 * x517)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x533 + (-1 * x535) + (-1 * ((-1 * x533) + x535) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x548 + (-1 * x550) + (-1 * ((-1 * x548) + x550) * x132)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x556 + (-1 * x563) + (-1 * ((-1 * x556) + x563) * x132)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen2_jac_x0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = dt * dt; const FLT x2 = x1 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x3 = x1 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x4 = x1 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x5 = 1e-10 + x2 + x3 + x4; const FLT x6 = sqrt(x5); const FLT x7 = 0.5 * x6; const FLT x8 = sin(x7); const FLT x9 = (1. / x5) * (x8 * x8); const FLT x10 = cos(x7); const FLT x11 = 1. / sqrt((x4 * x9) + (x3 * x9) + (x2 * x9) + (x10 * x10)); const FLT x12 = (1. / x6) * x8 * dt * x11; const FLT x13 = x12 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x14 = x13 * (*_x0).Pose.Rot[0]; const FLT x15 = x11 * x10; const FLT x16 = x15 * (*_x0).Pose.Rot[2]; const FLT x17 = x12 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x17 * (*_x0).Pose.Rot[1]; const FLT x19 = x12 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x20 = x19 * (*_x0).Pose.Rot[3]; const FLT x21 = x18 + x14 + (-1 * x20) + x16; const FLT x22 = (-1 * x19 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x13 * (*_x0).Pose.Rot[2]); const FLT x23 = x17 * (*_x0).Pose.Rot[0]; const FLT x24 = x13 * (*_x0).Pose.Rot[1]; const FLT x25 = x15 * (*_x0).Pose.Rot[3]; const FLT x26 = x19 * (*_x0).Pose.Rot[2]; const FLT x27 = x26 + x25 + x23 + (-1 * x24); const FLT x28 = (x21 * sensor_pt[2]) + (-1 * x27 * sensor_pt[1]) + (x22 * sensor_pt[0]); const FLT x29 = x17 * (*_x0).Pose.Rot[2]; const FLT x30 = x13 * (*_x0).Pose.Rot[3]; const FLT x31 = x15 * (*_x0).Pose.Rot[1]; const FLT x32 = x19 * (*_x0).Pose.Rot[0]; const FLT x33 = x32 + x31 + (-1 * x29) + x30; const FLT x34 = (-1 * x33 * sensor_pt[2]) + (x27 * sensor_pt[0]) + (x22 * sensor_pt[1]); const FLT x35 = sensor_pt[2] + (x0 * (*_x0).Acc[2]) + (2 * ((x34 * x33) + (-1 * x21 * x28))) + (dt * (*_x0).Velocity.Pos[2]) + (*_x0).Pose.Pos[2]; const FLT x36 = (-1 * x21 * sensor_pt[0]) + (x22 * sensor_pt[2]) + (x33 * sensor_pt[1]); const FLT x37 = (dt * (*_x0).Velocity.Pos[1]) + sensor_pt[1] + (2 * ((x28 * x27) + (-1 * x33 * x36))) + (*_x0).Pose.Pos[1] + (x0 * (*_x0).Acc[1]); const FLT x38 = (2 * ((x36 * x21) + (-1 * x34 * x27))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x0 * (*_x0).Acc[0]); const FLT x39 = (x35 * (*lh_p).Rot[0]) + (-1 * x38 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[1]); const FLT x40 = (-1 * x37 * (*lh_p).Rot[3]) + (x35 * (*lh_p).Rot[2]) + (x38 * (*lh_p).Rot[0]); const FLT x41 = x37 + (*lh_p).Pos[1] + (2 * ((x40 * (*lh_p).Rot[3]) + (-1 * x39 * (*lh_p).Rot[1]))); const FLT x42 = x41 * x41; const FLT x43 = (-1 * x35 * (*lh_p).Rot[1]) + (x37 * (*lh_p).Rot[0]) + (x38 * (*lh_p).Rot[3]); const FLT x44 = x35 + (2 * ((x43 * (*lh_p).Rot[1]) + (-1 * x40 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x45 = x38 + (2 * ((x39 * (*lh_p).Rot[2]) + (-1 * x43 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x46 = x45 * x45; const FLT x47 = x46 + (x44 * x44); const FLT x48 = x47 + x42; const FLT x49 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x50 = cos(x49); const FLT x51 = 1. / x50; const FLT x52 = x51 * (1. / sqrt(x48)); const FLT x53 = asin(x52 * x41); const FLT x54 = 8.0108022e-06 * x53; const FLT x55 = -8.0108022e-06 + (-1 * x54); const FLT x56 = 0.0028679863 + (x53 * x55); const FLT x57 = 5.3685255e-06 + (x53 * x56); const FLT x58 = 0.0076069798 + (x53 * x57); const FLT x59 = x53 * x58; const FLT x60 = -8.0108022e-06 + (-1.60216044e-05 * x53); const FLT x61 = x56 + (x60 * x53); const FLT x62 = x57 + (x61 * x53); const FLT x63 = x58 + (x62 * x53); const FLT x64 = (x63 * x53) + x59; const FLT x65 = sin(x49); const FLT x66 = atan2(-1 * x44, x45); const FLT x67 = tan(x49); const FLT x68 = x67 * (1. / sqrt(x47)); const FLT x69 = -1 * x68 * x41; const FLT x70 = (-1 * asin(x69)) + (*bsc0).ogeephase + x66; const FLT x71 = (sin(x70) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x72 = x71 * x65; const FLT x73 = x50 + (x72 * x64); const FLT x74 = 1. / x73; const FLT x75 = x53 * x53; const FLT x76 = x71 * x75; const FLT x77 = x74 * x76; const FLT x78 = x69 + (x77 * x58); const FLT x79 = 1. / sqrt(1 + (-1 * (x78 * x78))); const FLT x80 = (-1 * x18) + x20 + (-1 * x16) + (-1 * x14); const FLT x81 = 2 * x21; const FLT x82 = 1 + (x80 * x81) + (-2 * (x27 * x27)); const FLT x83 = 2 * x22; const FLT x84 = x83 * x21; const FLT x85 = 2 * x33; const FLT x86 = (x85 * x27) + (-1 * x84); const FLT x87 = x83 * x27; const FLT x88 = x87 + (-1 * x80 * x85); const FLT x89 = (x88 * (*lh_p).Rot[1]) + (-1 * x82 * (*lh_p).Rot[2]) + (x86 * (*lh_p).Rot[0]); const FLT x90 = 2 * (*lh_p).Rot[2]; const FLT x91 = (x82 * (*lh_p).Rot[3]) + (-1 * x86 * (*lh_p).Rot[1]) + (x88 * (*lh_p).Rot[0]); const FLT x92 = 2 * (*lh_p).Rot[3]; const FLT x93 = x82 + (x89 * x90) + (-1 * x92 * x91); const FLT x94 = 2 * x45; const FLT x95 = 2 * (*lh_p).Rot[1]; const FLT x96 = (x82 * (*lh_p).Rot[0]) + (-1 * x88 * (*lh_p).Rot[3]) + (x86 * (*lh_p).Rot[2]); const FLT x97 = x86 + (x91 * x95) + (-1 * x90 * x96); const FLT x98 = 2 * x44; const FLT x99 = (x98 * x97) + (x93 * x94); const FLT x100 = 1.0/2.0 * x41; const FLT x101 = x67 * (1. / (x47 * sqrt(x47))) * x100; const FLT x102 = x88 + (x92 * x96) + (-1 * x89 * x95); const FLT x103 = (-1 * x68 * x102) + (x99 * x101); const FLT x104 = 1. / x47; const FLT x105 = 1. / sqrt(1 + (-1 * (x67 * x67) * x42 * x104)); const FLT x106 = x44 * (1. / x46); const FLT x107 = 1. / x45; const FLT x108 = x46 * x104; const FLT x109 = ((-1 * x97 * x107) + (x93 * x106)) * x108; const FLT x110 = cos(x70) * (*bsc0).ogeemag; const FLT x111 = x110 * (x109 + (-1 * x103 * x105)); const FLT x112 = x64 * x65; const FLT x113 = 1. / sqrt(1 + (-1 * (1. / (x50 * x50)) * x42 * (1. / x48))); const FLT x114 = 2 * x41; const FLT x115 = x51 * (1. / (x48 * sqrt(x48))) * x100; const FLT x116 = x113 * ((-1 * x115 * (x99 + (x102 * x114))) + (x52 * x102)); const FLT x117 = x55 * x116; const FLT x118 = 2.40324066e-05 * x53; const FLT x119 = (x53 * ((-1 * x54 * x116) + x117)) + (x56 * x116); const FLT x120 = (x53 * x119) + (x57 * x116); const FLT x121 = (1. / (x73 * x73)) * x76 * x58; const FLT x122 = x75 * x74 * x58; const FLT x123 = 2 * x71 * x74 * x59; const FLT x124 = x79 * (x103 + (x111 * x122) + (x116 * x123) + (-1 * x121 * ((x72 * ((x53 * x120) + (x53 * (x120 + (x53 * (x119 + (x53 * ((x60 * x116) + x117 + (-1 * x118 * x116))) + (x61 * x116))) + (x62 * x116))) + (x58 * x116) + (x63 * x116))) + (x111 * x112))) + (x77 * x120)); const FLT x125 = cos((-1 * asin(x78)) + (*bsc0).gibpha + x66) * (*bsc0).gibmag; const FLT x126 = 2 * ((-1 * x25) + (-1 * x26) + x24 + (-1 * x23)); const FLT x127 = x83 * x33; const FLT x128 = x127 + (-1 * x21 * x126); const FLT x129 = 1 + (x27 * x126) + (-2 * (x33 * x33)); const FLT x130 = (x85 * x21) + (-1 * x87); const FLT x131 = (x130 * (*lh_p).Rot[0]) + (x128 * (*lh_p).Rot[2]) + (-1 * x129 * (*lh_p).Rot[3]); const FLT x132 = (x129 * (*lh_p).Rot[1]) + (x128 * (*lh_p).Rot[0]) + (-1 * x130 * (*lh_p).Rot[2]); const FLT x133 = x129 + (x92 * x131) + (-1 * x95 * x132); const FLT x134 = 2 * ((x130 * (*lh_p).Rot[3]) + (-1 * x128 * (*lh_p).Rot[1]) + (x129 * (*lh_p).Rot[0])); const FLT x135 = x130 + (x90 * x132) + (-1 * x134 * (*lh_p).Rot[3]); const FLT x136 = (x134 * (*lh_p).Rot[1]) + x128 + (-1 * x90 * x131); const FLT x137 = (x98 * x136) + (x94 * x135); const FLT x138 = (-1 * x115 * (x137 + (x114 * x133))) + (x52 * x133); const FLT x139 = x113 * x138; const FLT x140 = x57 * x113; const FLT x141 = x55 * x139; const FLT x142 = (x53 * ((-1 * x54 * x139) + x141)) + (x56 * x139); const FLT x143 = (x53 * x142) + (x138 * x140); const FLT x144 = (-1 * x68 * x133) + (x101 * x137); const FLT x145 = ((-1 * x107 * x136) + (x106 * x135)) * x108; const FLT x146 = x145 + (-1 * x105 * x144); const FLT x147 = x110 * x122; const FLT x148 = x110 * x112; const FLT x149 = x79 * (x144 + (-1 * x121 * ((x72 * ((x58 * x139) + (x63 * x139) + (x53 * (x143 + (x53 * ((x53 * (x141 + (x60 * x139) + (-1 * x118 * x139))) + x142 + (x61 * x139))) + (x62 * x139))) + (x53 * x143))) + (x146 * x148))) + (x146 * x147) + (x123 * x139) + (x77 * x143)); const FLT x150 = (x81 * x27) + (-1 * x127); const FLT x151 = (-1 * x32) + (-1 * x30) + x29 + (-1 * x31); const FLT x152 = 1 + (x85 * x151) + (-2 * (x21 * x21)); const FLT x153 = x84 + (-2 * x27 * x151); const FLT x154 = (x153 * (*lh_p).Rot[0]) + (-1 * x150 * (*lh_p).Rot[3]) + (x152 * (*lh_p).Rot[2]); const FLT x155 = (x150 * (*lh_p).Rot[1]) + (-1 * x153 * (*lh_p).Rot[2]) + (x152 * (*lh_p).Rot[0]); const FLT x156 = x150 + (x92 * x154) + (-1 * x95 * x155); const FLT x157 = (x153 * (*lh_p).Rot[3]) + (-1 * x152 * (*lh_p).Rot[1]) + (x150 * (*lh_p).Rot[0]); const FLT x158 = x153 + (x90 * x155) + (-1 * x92 * x157); const FLT x159 = x152 + (x95 * x157) + (-1 * x90 * x154); const FLT x160 = (x98 * x159) + (x94 * x158); const FLT x161 = (-1 * x115 * (x160 + (x114 * x156))) + (x52 * x156); const FLT x162 = x113 * x161; const FLT x163 = x55 * x162; const FLT x164 = (x53 * ((-1 * x54 * x162) + x163)) + (x56 * x162); const FLT x165 = (x53 * x164) + (x161 * x140); const FLT x166 = (-1 * x68 * x156) + (x101 * x160); const FLT x167 = ((-1 * x107 * x159) + (x106 * x158)) * x108; const FLT x168 = x167 + (-1 * x105 * x166); const FLT x169 = x79 * ((-1 * x121 * ((x72 * ((x58 * x162) + (x53 * x165) + (x53 * (x165 + (x53 * (x164 + (x53 * ((x60 * x162) + x163 + (-1 * x118 * x162))) + (x61 * x162))) + (x62 * x162))) + (x63 * x162))) + (x168 * x148))) + (x168 * x147) + (x77 * x165) + x166 + (x123 * x162)); cnMatrixOptionalSet(Hx, 0, 0, x109 + (-1 * x124) + (-1 * ((-1 * x109) + x124) * x125)); cnMatrixOptionalSet(Hx, 0, 1, x145 + (-1 * x149) + (-1 * ((-1 * x145) + x149) * x125)); cnMatrixOptionalSet(Hx, 0, 2, x167 + (-1 * x169) + (-1 * ((-1 * x167) + x169) * x125)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen2_jac_sensor_pt(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = x0 * (*_x0).Acc[1]; const FLT x2 = dt * (*_x0).Velocity.Pos[1]; const FLT x3 = dt * dt; const FLT x4 = x3 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x5 = x3 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x6 = x3 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x7 = 1e-10 + x4 + x5 + x6; const FLT x8 = sqrt(x7); const FLT x9 = 0.5 * x8; const FLT x10 = sin(x9); const FLT x11 = (1. / x7) * (x10 * x10); const FLT x12 = cos(x9); const FLT x13 = 1. / sqrt((x6 * x11) + (x5 * x11) + (x4 * x11) + (x12 * x12)); const FLT x14 = (1. / x8) * dt * x13 * x10; const FLT x15 = x14 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x16 = x14 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x17 = x13 * x12; const FLT x18 = x14 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = (-1 * x18 * (*_x0).Pose.Rot[1]) + (x17 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (-1 * x16 * (*_x0).Pose.Rot[2]); const FLT x20 = x14 * (*_x0).Pose.Rot[0]; const FLT x21 = (x20 * (*_x0).Velocity.AxisAngleRot[0]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (x17 * (*_x0).Pose.Rot[1]) + (x16 * (*_x0).Pose.Rot[3]); const FLT x22 = (x20 * (*_x0).Velocity.AxisAngleRot[1]) + (-1 * x18 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[1]) + (x17 * (*_x0).Pose.Rot[2]); const FLT x23 = (-1 * x22 * sensor_pt[0]) + (x19 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x24 = (x18 * (*_x0).Pose.Rot[2]) + (x17 * (*_x0).Pose.Rot[3]) + (x20 * (*_x0).Velocity.AxisAngleRot[2]) + (-1 * x16 * (*_x0).Pose.Rot[1]); const FLT x25 = (-1 * x24 * sensor_pt[1]) + (x22 * sensor_pt[2]) + (x19 * sensor_pt[0]); const FLT x26 = 2 * ((x24 * x25) + (-1 * x23 * x21)); const FLT x27 = x26 + x2 + sensor_pt[1] + (*_x0).Pose.Pos[1] + x1; const FLT x28 = x27 * (*lh_p).Rot[0]; const FLT x29 = dt * (*_x0).Velocity.Pos[0]; const FLT x30 = x0 * (*_x0).Acc[0]; const FLT x31 = (-1 * x21 * sensor_pt[2]) + (x24 * sensor_pt[0]) + (x19 * sensor_pt[1]); const FLT x32 = 2 * ((x22 * x23) + (-1 * x31 * x24)); const FLT x33 = x32 + (*_x0).Pose.Pos[0] + x29 + sensor_pt[0] + x30; const FLT x34 = x33 * (*lh_p).Rot[3]; const FLT x35 = dt * (*_x0).Velocity.Pos[2]; const FLT x36 = x0 * (*_x0).Acc[2]; const FLT x37 = 2 * ((x31 * x21) + (-1 * x25 * x22)); const FLT x38 = x37 + sensor_pt[2] + x36 + x35 + (*_x0).Pose.Pos[2]; const FLT x39 = x38 * (*lh_p).Rot[1]; const FLT x40 = (-1 * x39) + x28 + x34; const FLT x41 = x38 * (*lh_p).Rot[0]; const FLT x42 = x27 * (*lh_p).Rot[1]; const FLT x43 = x33 * (*lh_p).Rot[2]; const FLT x44 = (-1 * x43) + x41 + x42; const FLT x45 = x33 + (2 * ((x44 * (*lh_p).Rot[2]) + (-1 * x40 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x46 = x38 * (*lh_p).Rot[2]; const FLT x47 = x33 * (*lh_p).Rot[0]; const FLT x48 = x27 * (*lh_p).Rot[3]; const FLT x49 = (-1 * x48) + x46 + x47; const FLT x50 = x27 + (*lh_p).Pos[1] + (2 * ((x49 * (*lh_p).Rot[3]) + (-1 * x44 * (*lh_p).Rot[1]))); const FLT x51 = x50 * x45; const FLT x52 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x53 = tan(x52); const FLT x54 = x38 + (2 * ((x40 * (*lh_p).Rot[1]) + (-1 * x49 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x55 = x45 * x45; const FLT x56 = x55 + (x54 * x54); const FLT x57 = x53 * (1. / (x56 * sqrt(x56))); const FLT x58 = x51 * x57; const FLT x59 = x50 * x50; const FLT x60 = 1. / x56; const FLT x61 = 1. / sqrt(1 + (-1 * x60 * (x53 * x53) * x59)); const FLT x62 = x60 * x54; const FLT x63 = atan2(-1 * x54, x45); const FLT x64 = x53 * (1. / sqrt(x56)); const FLT x65 = -1 * x64 * x50; const FLT x66 = (-1 * asin(x65)) + (*bsc0).ogeephase + x63; const FLT x67 = cos(x66) * (*bsc0).ogeemag; const FLT x68 = x67 * (x62 + (-1 * x61 * x58)); const FLT x69 = x56 + x59; const FLT x70 = cos(x52); const FLT x71 = 1. / x70; const FLT x72 = x71 * (1. / sqrt(x69)); const FLT x73 = asin(x72 * x50); const FLT x74 = 8.0108022e-06 * x73; const FLT x75 = -8.0108022e-06 + (-1 * x74); const FLT x76 = 0.0028679863 + (x73 * x75); const FLT x77 = 5.3685255e-06 + (x73 * x76); const FLT x78 = 0.0076069798 + (x73 * x77); const FLT x79 = x73 * x73; const FLT x80 = x73 * x78; const FLT x81 = -8.0108022e-06 + (-1.60216044e-05 * x73); const FLT x82 = x76 + (x81 * x73); const FLT x83 = x77 + (x82 * x73); const FLT x84 = x78 + (x83 * x73); const FLT x85 = (x84 * x73) + x80; const FLT x86 = sin(x52); const FLT x87 = (sin(x66) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x88 = x86 * x87; const FLT x89 = x70 + (x88 * x85); const FLT x90 = 1. / x89; const FLT x91 = x79 * x78 * x90; const FLT x92 = 2 * x45; const FLT x93 = 1. / sqrt(1 + (-1 * (1. / (x70 * x70)) * (1. / x69) * x59)); const FLT x94 = x71 * (1. / (x69 * sqrt(x69))); const FLT x95 = x50 * x94; const FLT x96 = x80 * x87 * x90; const FLT x97 = x93 * x96 * x95; const FLT x98 = x77 * x93; const FLT x99 = x51 * x94; const FLT x100 = x93 * x99; const FLT x101 = -1 * x75 * x100; const FLT x102 = (x73 * ((x74 * x100) + x101)) + (-1 * x76 * x100); const FLT x103 = (x73 * x102) + (-1 * x99 * x98); const FLT x104 = x87 * x79; const FLT x105 = x90 * x104; const FLT x106 = x85 * x86; const FLT x107 = 2.40324066e-05 * x73; const FLT x108 = x81 * x93; const FLT x109 = x84 * x93; const FLT x110 = (1. / (x89 * x89)) * x78 * x104; const FLT x111 = x65 + (x78 * x105); const FLT x112 = 1. / sqrt(1 + (-1 * (x111 * x111))); const FLT x113 = x112 * ((-1 * x110 * ((x88 * ((-1 * x78 * x100) + (x73 * x103) + (x73 * ((x73 * (x102 + (x73 * ((-1 * x99 * x108) + x101 + (x100 * x107))) + (-1 * x82 * x100))) + x103 + (-1 * x83 * x100))) + (-1 * x99 * x109))) + (x68 * x106))) + (x103 * x105) + (-1 * x92 * x97) + x58 + (x68 * x91)); const FLT x114 = cos((-1 * asin(x111)) + (*bsc0).gibpha + x63) * (*bsc0).gibmag; const FLT x115 = x67 * x91; const FLT x116 = x64 * x61; const FLT x117 = (-1 * x59 * x94) + x72; const FLT x118 = x93 * x117; const FLT x119 = x75 * x118; const FLT x120 = (x73 * ((-1 * x74 * x118) + x119)) + (x76 * x118); const FLT x121 = (x73 * x120) + (x77 * x118); const FLT x122 = x67 * x106; const FLT x123 = 2 * x96; const FLT x124 = x112 * ((x118 * x123) + (-1 * x110 * ((x88 * ((x78 * x118) + (x73 * x121) + (x73 * (x121 + (x73 * (x120 + (x73 * ((x108 * x117) + x119 + (-1 * x107 * x118))) + (x82 * x118))) + (x83 * x118))) + (x84 * x118))) + (x116 * x122))) + (x105 * x121) + (-1 * x64) + (x115 * x116)); const FLT x125 = x50 * x57; const FLT x126 = x54 * x125; const FLT x127 = x60 * x45; const FLT x128 = -1 * x127; const FLT x129 = x128 + (-1 * x61 * x126); const FLT x130 = x54 * x95; const FLT x131 = x93 * x130; const FLT x132 = -1 * x75 * x131; const FLT x133 = (x73 * ((x74 * x131) + x132)) + (-1 * x76 * x131); const FLT x134 = (x73 * x133) + (-1 * x98 * x130); const FLT x135 = 2 * x54; const FLT x136 = x112 * ((-1 * x97 * x135) + (x105 * x134) + (-1 * x110 * ((x88 * ((-1 * x109 * x130) + (x73 * (x134 + (x73 * (x133 + (x73 * ((-1 * x108 * x130) + x132 + (x107 * x131))) + (-1 * x82 * x131))) + (-1 * x83 * x131))) + (-1 * x78 * x131) + (x73 * x134))) + (x122 * x129))) + x126 + (x115 * x129)); const FLT x137 = 2 * x48; const FLT x138 = (2 * x46) + (-1 * x137); const FLT x139 = 2 * x43; const FLT x140 = (2 * x42) + (-1 * x139); const FLT x141 = (x135 * x140) + (x92 * x138); const FLT x142 = 1.0/2.0 * x125; const FLT x143 = 2 * x39; const FLT x144 = (2 * x34) + (-1 * x143); const FLT x145 = (-1 * x64 * x144) + (x142 * x141); const FLT x146 = x54 * (1. / x55); const FLT x147 = 1. / x45; const FLT x148 = x60 * x55; const FLT x149 = ((-1 * x140 * x147) + (x138 * x146)) * x148; const FLT x150 = x149 + (-1 * x61 * x145); const FLT x151 = 2 * x50; const FLT x152 = 1.0/2.0 * x95; const FLT x153 = (-1 * x152 * (x141 + (x144 * x151))) + (x72 * x144); const FLT x154 = x93 * x153; const FLT x155 = x75 * x154; const FLT x156 = (x73 * ((-1 * x74 * x154) + x155)) + (x76 * x154); const FLT x157 = (x73 * x156) + (x77 * x154); const FLT x158 = x112 * (x145 + (x115 * x150) + (x105 * x157) + (-1 * x110 * ((x88 * ((x78 * x154) + (x73 * (x157 + (x73 * (x156 + (x73 * ((x108 * x153) + x155 + (-1 * x107 * x154))) + (x82 * x154))) + (x83 * x154))) + (x84 * x154) + (x73 * x157))) + (x122 * x150))) + (x123 * x154)); const FLT x159 = 2 * x41; const FLT x160 = (-4 * x42) + (-1 * x159) + x139; const FLT x161 = (-1 * x36) + (-1 * sensor_pt[2]) + (-1 * x37) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x35); const FLT x162 = 2 * (*lh_p).Rot[3]; const FLT x163 = 2 * (*lh_p).Rot[2]; const FLT x164 = (x27 * x163) + (-1 * x161 * x162); const FLT x165 = 2 * (*lh_p).Rot[1]; const FLT x166 = 2 * x28; const FLT x167 = x144 + (x161 * x165) + x166; const FLT x168 = (x167 * x135) + (x92 * x164); const FLT x169 = (-1 * x152 * (x168 + (x160 * x151))) + (x72 * x160); const FLT x170 = x93 * x169; const FLT x171 = (-1 * x64 * x160) + (x168 * x142); const FLT x172 = ((-1 * x167 * x147) + (x164 * x146)) * x148; const FLT x173 = x172 + (-1 * x61 * x171); const FLT x174 = x75 * x170; const FLT x175 = (x73 * ((-1 * x74 * x170) + x174)) + (x76 * x170); const FLT x176 = (x73 * x175) + (x77 * x170); const FLT x177 = x112 * ((x115 * x173) + x171 + (x105 * x176) + (x123 * x170) + (-1 * x110 * ((x88 * ((x78 * x170) + (x73 * (x176 + (x73 * (x175 + (x73 * ((x108 * x169) + x174 + (-1 * x107 * x170))) + (x82 * x170))) + (x83 * x170))) + (x73 * x176) + (x84 * x170))) + (x122 * x173)))); const FLT x178 = (-1 * (*_x0).Pose.Pos[0]) + (-1 * x32) + (-1 * x29) + (-1 * x30) + (-1 * sensor_pt[0]); const FLT x179 = x140 + x159 + (x163 * x178); const FLT x180 = 2 * x47; const FLT x181 = (-1 * x180) + (-4 * x46) + x137; const FLT x182 = (x181 * x135) + (x92 * x179); const FLT x183 = (x38 * x162) + (-1 * x165 * x178); const FLT x184 = (-1 * x64 * x183) + (x182 * x142); const FLT x185 = ((-1 * x181 * x147) + (x179 * x146)) * x148; const FLT x186 = x185 + (-1 * x61 * x184); const FLT x187 = (-1 * x152 * (x182 + (x183 * x151))) + (x72 * x183); const FLT x188 = x93 * x187; const FLT x189 = x75 * x188; const FLT x190 = (x73 * ((-1 * x74 * x188) + x189)) + (x76 * x188); const FLT x191 = (x73 * x190) + (x77 * x188); const FLT x192 = x112 * (x184 + (x115 * x186) + (x123 * x188) + (-1 * x110 * ((x88 * ((x73 * x191) + (x78 * x188) + (x73 * (x191 + (x73 * (x190 + (x73 * ((x108 * x187) + x189 + (-1 * x107 * x188))) + (x82 * x188))) + (x83 * x188))) + (x84 * x188))) + (x122 * x186))) + (x105 * x191)); const FLT x193 = (-1 * x2) + (-1 * x26) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * x1) + (-1 * sensor_pt[1]); const FLT x194 = x138 + (x162 * x193) + x180; const FLT x195 = x143 + (-1 * x166) + (-4 * x34); const FLT x196 = (x33 * x165) + (-1 * x163 * x193); const FLT x197 = (x196 * x135) + (x92 * x195); const FLT x198 = (-1 * x152 * (x197 + (x194 * x151))) + (x72 * x194); const FLT x199 = x93 * x198; const FLT x200 = x75 * x199; const FLT x201 = (x73 * ((-1 * x74 * x199) + x200)) + (x76 * x199); const FLT x202 = (x73 * x201) + (x77 * x199); const FLT x203 = (-1 * x64 * x194) + (x197 * x142); const FLT x204 = ((-1 * x196 * x147) + (x195 * x146)) * x148; const FLT x205 = x204 + (-1 * x61 * x203); const FLT x206 = x112 * ((x205 * x115) + (-1 * x110 * ((x88 * ((x73 * (x202 + (x73 * (x201 + (x73 * ((x108 * x198) + (-1 * x107 * x199) + x200)) + (x82 * x199))) + (x83 * x199))) + (x84 * x199) + (x78 * x199) + (x73 * x202))) + (x205 * x122))) + (x202 * x105) + x203 + (x123 * x199)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), x62 + (-1 * x113) + (-1 * x114 * ((-1 * x62) + x113))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x114 * x124) + (-1 * x124)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x128 + (-1 * x136) + (-1 * (x127 + x136) * x114)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x149 + (-1 * x158) + (-1 * ((-1 * x149) + x158) * x114)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x172 + (-1 * x177) + (-1 * ((-1 * x172) + x177) * x114)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x185 + (-1 * x192) + (-1 * ((-1 * x185) + x192) * x114)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), x204 + (-1 * x206) + (-1 * ((-1 * x204) + x206) * x114)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen2_jac_lh_p(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x1 = cos(x0); const FLT x2 = 1.0/2.0 * dt * fabs(dt); const FLT x3 = dt * dt; const FLT x4 = x3 * ((*_x0).Velocity.AxisAngleRot[1] * (*_x0).Velocity.AxisAngleRot[1]); const FLT x5 = x3 * ((*_x0).Velocity.AxisAngleRot[2] * (*_x0).Velocity.AxisAngleRot[2]); const FLT x6 = x3 * ((*_x0).Velocity.AxisAngleRot[0] * (*_x0).Velocity.AxisAngleRot[0]); const FLT x7 = 1e-10 + x4 + x5 + x6; const FLT x8 = sqrt(x7); const FLT x9 = 0.5 * x8; const FLT x10 = sin(x9); const FLT x11 = (1. / x7) * (x10 * x10); const FLT x12 = cos(x9); const FLT x13 = 1. / sqrt((x6 * x11) + (x5 * x11) + (x4 * x11) + (x12 * x12)); const FLT x14 = (1. / x8) * dt * x13 * x10; const FLT x15 = x14 * (*_x0).Velocity.AxisAngleRot[1]; const FLT x16 = x13 * x12; const FLT x17 = x14 * (*_x0).Velocity.AxisAngleRot[2]; const FLT x18 = x14 * (*_x0).Velocity.AxisAngleRot[0]; const FLT x19 = (-1 * x18 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[2]); const FLT x20 = x14 * (*_x0).Pose.Rot[2]; const FLT x21 = (x16 * (*_x0).Pose.Rot[0]) + (-1 * x18 * (*_x0).Pose.Rot[1]) + (-1 * x17 * (*_x0).Pose.Rot[3]) + (-1 * x20 * (*_x0).Velocity.AxisAngleRot[1]); const FLT x22 = (x20 * (*_x0).Velocity.AxisAngleRot[0]) + (x16 * (*_x0).Pose.Rot[3]) + (x17 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[1]); const FLT x23 = (x19 * sensor_pt[2]) + (-1 * x22 * sensor_pt[1]) + (x21 * sensor_pt[0]); const FLT x24 = (-1 * x20 * (*_x0).Velocity.AxisAngleRot[2]) + (x18 * (*_x0).Pose.Rot[0]) + (x16 * (*_x0).Pose.Rot[1]) + (x15 * (*_x0).Pose.Rot[3]); const FLT x25 = (x22 * sensor_pt[0]) + (-1 * x24 * sensor_pt[2]) + (x21 * sensor_pt[1]); const FLT x26 = sensor_pt[2] + (x2 * (*_x0).Acc[2]) + (dt * (*_x0).Velocity.Pos[2]) + (2 * ((x24 * x25) + (-1 * x23 * x19))) + (*_x0).Pose.Pos[2]; const FLT x27 = (-1 * x19 * sensor_pt[0]) + (x21 * sensor_pt[2]) + (x24 * sensor_pt[1]); const FLT x28 = (2 * ((x22 * x23) + (-1 * x24 * x27))) + sensor_pt[1] + (*_x0).Pose.Pos[1] + (dt * (*_x0).Velocity.Pos[1]) + (x2 * (*_x0).Acc[1]); const FLT x29 = (2 * ((x27 * x19) + (-1 * x25 * x22))) + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * (*_x0).Velocity.Pos[0]) + (x2 * (*_x0).Acc[0]); const FLT x30 = (-1 * x29 * (*lh_p).Rot[2]) + (x26 * (*lh_p).Rot[0]) + (x28 * (*lh_p).Rot[1]); const FLT x31 = (-1 * x28 * (*lh_p).Rot[3]) + (x26 * (*lh_p).Rot[2]) + (x29 * (*lh_p).Rot[0]); const FLT x32 = x28 + (*lh_p).Pos[1] + (2 * ((x31 * (*lh_p).Rot[3]) + (-1 * x30 * (*lh_p).Rot[1]))); const FLT x33 = x32 * x32; const FLT x34 = (-1 * x26 * (*lh_p).Rot[1]) + (x28 * (*lh_p).Rot[0]) + (x29 * (*lh_p).Rot[3]); const FLT x35 = x26 + (2 * ((x34 * (*lh_p).Rot[1]) + (-1 * x31 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x36 = x29 + (2 * ((x30 * (*lh_p).Rot[2]) + (-1 * x34 * (*lh_p).Rot[3]))) + (*lh_p).Pos[0]; const FLT x37 = (x36 * x36) + (x35 * x35); const FLT x38 = x37 + x33; const FLT x39 = x32 * (1. / sqrt(x38)); const FLT x40 = asin((1. / x1) * x39); const FLT x41 = 8.0108022e-06 * x40; const FLT x42 = -8.0108022e-06 + (-1 * x41); const FLT x43 = 0.0028679863 + (x40 * x42); const FLT x44 = 5.3685255e-06 + (x40 * x43); const FLT x45 = 0.0076069798 + (x40 * x44); const FLT x46 = x40 * x40; const FLT x47 = sin(x0); const FLT x48 = atan2(-1 * x35, x36); const FLT x49 = tan(x0); const FLT x50 = x32 * (1. / sqrt(x37)); const FLT x51 = -1 * x50 * x49; const FLT x52 = (-1 * asin(x51)) + (*bsc0).ogeephase + x48; const FLT x53 = sin(x52); const FLT x54 = (x53 * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x55 = x40 * x45; const FLT x56 = -8.0108022e-06 + (-1.60216044e-05 * x40); const FLT x57 = x43 + (x56 * x40); const FLT x58 = x44 + (x57 * x40); const FLT x59 = x45 + (x58 * x40); const FLT x60 = (x59 * x40) + x55; const FLT x61 = x60 * x54; const FLT x62 = x61 * x47; const FLT x63 = x1 + x62; const FLT x64 = 1. / x63; const FLT x65 = x64 * x46; const FLT x66 = x65 * x45; const FLT x67 = (1. / (x63 * x63)) * x45 * x46; const FLT x68 = x62 * x67; const FLT x69 = x51 + (x66 * x54); const FLT x70 = 1. / sqrt(1 + (-1 * (x69 * x69))); const FLT x71 = ((-1 * x68) + x66) * x70; const FLT x72 = asin(x69) + (-1 * (*bsc0).gibpha) + (-1 * x48); const FLT x73 = cos(x72) * (*bsc0).gibmag; const FLT x74 = ((-1 * x68 * x53) + (x66 * x53)) * x70; const FLT x75 = cos(x52) * (*bsc0).ogeemag; const FLT x76 = x75 * x66; const FLT x77 = x70 * ((-1 * x75 * x68) + x76); const FLT x78 = x49 * x49; const FLT x79 = x50 * (1 + x78); const FLT x80 = x79 * (1. / sqrt(1 + (-1 * x78 * x33 * (1. / x37)))); const FLT x81 = x54 * x47; const FLT x82 = 1. / (x1 * x1); const FLT x83 = x82 * x39 * (1. / sqrt(1 + (-1 * x82 * x33 * (1. / x38)))); const FLT x84 = x83 * x47; const FLT x85 = -1 * x84 * x42; const FLT x86 = (x40 * ((x84 * x41) + x85)) + (-1 * x84 * x43); const FLT x87 = (x86 * x40) + (-1 * x84 * x44); const FLT x88 = x70 * ((-1 * x67 * x54 * ((x81 * ((-1 * x84 * x45) + (-1 * x84 * x59) + (x40 * ((x40 * (x86 + (x40 * ((-1 * x84 * x56) + x85 + (2.40324066e-05 * x84 * x40))) + (-1 * x84 * x57))) + x87 + (-1 * x84 * x58))) + (x87 * x40))) + (-1 * x80 * x75 * x60 * x47) + x47 + (-1 * x1 * x61))) + (x87 * x65 * x54) + x79 + (-1 * x80 * x76) + (-2 * x81 * x83 * x64 * x55)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), (-1 * x71 * x73) + (-1 * x71)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * sin(x72)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x73); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeemag)/sizeof(FLT), (-1 * x73 * x74) + (-1 * x74)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeephase)/sizeof(FLT), (-1 * x73 * x77) + (-1 * x77)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x88 * x73) + (-1 * x88)); } // Full version Jacobian of SurviveKalmanModel_LightMeas_y_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanModel_LightMeas_y_gen2_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanModel_LightMeas_y_gen2(dt, _x0, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanModel_LightMeas_y_gen2_jac_bsc0(Hx, dt, _x0, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanErrorModel_LightMeas_x_gen1(const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x5 = (*_x0).Pose.Rot[3] + (x4 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (x3 * (*_x0).Pose.Rot[0]); const FLT x6 = dt * dt; const FLT x7 = (x1 * x1) * x6; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x6 * (x8 * x8); const FLT x10 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x11 = x6 * (x10 * x10); const FLT x12 = 1e-10 + x11 + x7 + x9; const FLT x13 = sqrt(x12); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = (-1 * x3 * (*_x0).Pose.Rot[1]) + (x4 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (-1 * x4 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x3 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x4 * (*_x0).Pose.Rot[3]) + (x3 * (*_x0).Pose.Rot[2]); const FLT x19 = (x15 * x15) * (1. / x12); const FLT x20 = cos(x14); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x5 * x5) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x11 * x19))); const FLT x22 = dt * x21 * x15 * (1. / x13); const FLT x23 = x5 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x8 * x26) + (x25 * x17) + (-1 * x1 * x23) + (-1 * x24 * x10); const FLT x28 = x22 * x17; const FLT x29 = (-1 * x8 * x23) + (x1 * x26) + (x28 * x10) + (x25 * x16); const FLT x30 = (x5 * x25) + (x8 * x24) + (x1 * x28) + (-1 * x26 * x10); const FLT x31 = (-1 * x30 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x29 * sensor_pt[2]); const FLT x32 = (x8 * x28) + (-1 * x1 * x24) + (x23 * x10) + (x25 * x18); const FLT x33 = (-1 * x32 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x30 * sensor_pt[0]); const FLT x34 = (*_x0).Pose.Pos[2] + (x0 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + (2 * ((x32 * x33) + (-1 * x31 * x29))) + sensor_pt[2]; const FLT x35 = (-1 * x29 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x32 * sensor_pt[1]); const FLT x36 = (*_x0).Pose.Pos[1] + (2 * ((x30 * x31) + (-1 * x32 * x35))) + (x0 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + (*error_model).Pose.Pos[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + sensor_pt[1]; const FLT x37 = (x0 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (2 * ((x35 * x29) + (-1 * x30 * x33))) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x38 = (-1 * x37 * (*lh_p).Rot[2]) + (x34 * (*lh_p).Rot[0]) + (x36 * (*lh_p).Rot[1]); const FLT x39 = (-1 * x36 * (*lh_p).Rot[3]) + (x37 * (*lh_p).Rot[0]) + (x34 * (*lh_p).Rot[2]); const FLT x40 = x36 + (*lh_p).Pos[1] + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x38 * (*lh_p).Rot[1]))); const FLT x41 = (-1 * x34 * (*lh_p).Rot[1]) + (x36 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[3]); const FLT x42 = x34 + (2 * ((x41 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x43 = -1 * x42; const FLT x44 = x37 + (*lh_p).Pos[0] + (2 * ((x38 * (*lh_p).Rot[2]) + (-1 * x41 * (*lh_p).Rot[3]))); const FLT x45 = (-1 * (*bsc0).phase) + (-1 * atan2(x44, x43)) + (-1 * asin((1. / sqrt((x44 * x44) + (x42 * x42))) * x40 * (*bsc0).tilt)); return x45 + ((atan2(x40, x43) * atan2(x40, x43)) * (*bsc0).curve) + (-1 * cos(1.5707963267949 + x45 + (*bsc0).gibpha) * (*bsc0).gibmag); } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = x0 * (*lh_p).Rot[2]; const FLT x2 = x1 * (*lh_p).Rot[0]; const FLT x3 = x0 * (*lh_p).Rot[1]; const FLT x4 = x3 * (*lh_p).Rot[3]; const FLT x5 = x4 + (-1 * x2); const FLT x6 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x8 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x9 = (*_x0).Pose.Rot[3] + (-1 * x6 * (*_x0).Pose.Rot[2]) + (x8 * (*_x0).Pose.Rot[1]) + (x7 * (*_x0).Pose.Rot[0]); const FLT x10 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x11 = 0.5 * (*_x0).Pose.Rot[3]; const FLT x12 = (x8 * (*_x0).Pose.Rot[0]) + (x11 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x7 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[2]; const FLT x13 = (-1 * x8 * (*_x0).Pose.Rot[2]) + (-1 * x6 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x11 * (*error_model).Pose.AxisAngleRot[2]); const FLT x14 = (x6 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x11 * (*error_model).Pose.AxisAngleRot[1]) + (x7 * (*_x0).Pose.Rot[2]); const FLT x15 = (x14 * x14) + (x9 * x9) + (x13 * x13) + (x12 * x12); const FLT x16 = 1. / sqrt(x15); const FLT x17 = dt * dt; const FLT x18 = x10 * x10; const FLT x19 = x18 * x17; const FLT x20 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x21 = x20 * x20; const FLT x22 = x21 * x17; const FLT x23 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x24 = x23 * x23; const FLT x25 = x24 * x17; const FLT x26 = 1e-10 + x25 + x19 + x22; const FLT x27 = sqrt(x26); const FLT x28 = 0.5 * x27; const FLT x29 = sin(x28); const FLT x30 = x29 * x29; const FLT x31 = 1. / x26; const FLT x32 = x30 * x31; const FLT x33 = cos(x28); const FLT x34 = (x32 * x19) + (x32 * x22) + (x33 * x33) + (x32 * x25); const FLT x35 = 1. / sqrt(x34); const FLT x36 = x35 * x16; const FLT x37 = x29 * (1. / x27); const FLT x38 = dt * x37; const FLT x39 = x36 * x38; const FLT x40 = x39 * x10; const FLT x41 = x36 * x12; const FLT x42 = x41 * x38; const FLT x43 = x33 * x36; const FLT x44 = x43 * x13; const FLT x45 = x39 * x14; const FLT x46 = x44 + (-1 * x45 * x20) + (-1 * x9 * x40) + (-1 * x42 * x23); const FLT x47 = x39 * x23; const FLT x48 = x9 * x43; const FLT x49 = (x42 * x20) + x48 + (x40 * x13) + (-1 * x47 * x14); const FLT x50 = x43 * x14; const FLT x51 = x39 * x13; const FLT x52 = (x51 * x20) + (-1 * x42 * x10) + (x9 * x47) + x50; const FLT x53 = (-1 * x52 * sensor_pt[2]) + (x46 * sensor_pt[1]) + (x49 * sensor_pt[0]); const FLT x54 = x43 * x12; const FLT x55 = x9 * x39; const FLT x56 = (x40 * x14) + (x47 * x13) + (-1 * x55 * x20) + x54; const FLT x57 = (-1 * x56 * sensor_pt[0]) + (x46 * sensor_pt[2]) + (x52 * sensor_pt[1]); const FLT x58 = 1.0/2.0 * x0; const FLT x59 = (x58 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])) + (2 * ((x57 * x56) + (-1 * x53 * x49))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0]; const FLT x60 = (-1 * x49 * sensor_pt[1]) + (x46 * sensor_pt[0]) + (x56 * sensor_pt[2]); const FLT x61 = (*_x0).Pose.Pos[2] + (x58 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x53 * x52) + (-1 * x60 * x56))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x62 = (*_x0).Pose.Pos[1] + (2 * ((x60 * x49) + (-1 * x52 * x57))) + (*error_model).Pose.Pos[1] + (x58 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])); const FLT x63 = (-1 * x62 * (*lh_p).Rot[3]) + (x59 * (*lh_p).Rot[0]) + (x61 * (*lh_p).Rot[2]); const FLT x64 = (-1 * x61 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]) + (x59 * (*lh_p).Rot[3]); const FLT x65 = x61 + (2 * ((x64 * (*lh_p).Rot[1]) + (-1 * x63 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x66 = x65 * x65; const FLT x67 = 1. / x66; const FLT x68 = (x61 * (*lh_p).Rot[0]) + (-1 * x59 * (*lh_p).Rot[2]) + (x62 * (*lh_p).Rot[1]); const FLT x69 = x62 + (*lh_p).Pos[1] + (2 * ((x63 * (*lh_p).Rot[3]) + (-1 * x68 * (*lh_p).Rot[1]))); const FLT x70 = x67 * x69; const FLT x71 = x3 * (*lh_p).Rot[2]; const FLT x72 = x0 * (*lh_p).Rot[3] * (*lh_p).Rot[0]; const FLT x73 = x72 + x71; const FLT x74 = 1. / x65; const FLT x75 = x69 * x69; const FLT x76 = -1 * x65; const FLT x77 = 2 * (1. / (x66 + x75)) * x66 * atan2(x69, x76) * (*bsc0).curve; const FLT x78 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x79 = -1 * x0 * x78; const FLT x80 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x81 = (-1 * x0 * x80) + x58; const FLT x82 = x81 + x79; const FLT x83 = x59 + (*lh_p).Pos[0] + (2 * ((x68 * (*lh_p).Rot[2]) + (-1 * x64 * (*lh_p).Rot[3]))); const FLT x84 = 2 * x83; const FLT x85 = 2 * x65; const FLT x86 = (x83 * x83) + x66; const FLT x87 = 1.0/2.0 * (1. / (x86 * sqrt(x86))) * x69 * (*bsc0).tilt; const FLT x88 = (1. / sqrt(x86)) * (*bsc0).tilt; const FLT x89 = 1. / x86; const FLT x90 = 1. / sqrt(1 + (-1 * x89 * x75 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x91 = x83 * x67; const FLT x92 = x89 * x66; const FLT x93 = (-1 * x92 * ((x5 * x91) + (-1 * x82 * x74))) + (-1 * x90 * ((x88 * x73) + (-1 * x87 * ((x5 * x85) + (x82 * x84))))); const FLT x94 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * asin(x88 * x69)) + (-1 * atan2(x83, x76)) + (-1 * (*bsc0).phase) + (*bsc0).gibpha); const FLT x95 = x1 * (*lh_p).Rot[3]; const FLT x96 = x3 * (*lh_p).Rot[0]; const FLT x97 = x96 + x95; const FLT x98 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x99 = -1 * x0 * x98; const FLT x100 = x99 + x79 + x58; const FLT x101 = x71 + (-1 * x72); const FLT x102 = (-1 * x92 * ((x91 * x97) + (-1 * x74 * x101))) + (-1 * x90 * ((x88 * x100) + (-1 * x87 * ((x85 * x97) + (x84 * x101))))); const FLT x103 = x81 + x99; const FLT x104 = x95 + (-1 * x96); const FLT x105 = x2 + x4; const FLT x106 = (-1 * ((x91 * x103) + (-1 * x74 * x105)) * x92) + (-1 * x90 * ((x88 * x104) + (-1 * ((x85 * x103) + (x84 * x105)) * x87))); const FLT x107 = 2 * (*lh_p).Rot[2]; const FLT x108 = x107 * (*lh_p).Rot[0]; const FLT x109 = 2 * (*lh_p).Rot[1]; const FLT x110 = x109 * (*lh_p).Rot[3]; const FLT x111 = x110 + (-1 * x108); const FLT x112 = x107 * (*lh_p).Rot[1]; const FLT x113 = 2 * (*lh_p).Rot[3]; const FLT x114 = x113 * (*lh_p).Rot[0]; const FLT x115 = x114 + x112; const FLT x116 = 2 * x78; const FLT x117 = -1 * x116; const FLT x118 = 2 * x80; const FLT x119 = -1 * x118; const FLT x120 = 1 + x119 + x117; const FLT x121 = (-1 * ((x91 * x111) + (-1 * x74 * x120)) * x92) + (-1 * x90 * ((x88 * x115) + (-1 * ((x85 * x111) + (x84 * x120)) * x87))); const FLT x122 = x107 * (*lh_p).Rot[3]; const FLT x123 = x109 * (*lh_p).Rot[0]; const FLT x124 = x123 + x122; const FLT x125 = 2 * x98; const FLT x126 = 1 + (-1 * x125); const FLT x127 = x126 + x117; const FLT x128 = x112 + (-1 * x114); const FLT x129 = (-1 * ((x91 * x124) + (-1 * x74 * x128)) * x92) + (-1 * x90 * ((x88 * x127) + (-1 * ((x85 * x124) + (x84 * x128)) * x87))); const FLT x130 = x126 + x119; const FLT x131 = x122 + (-1 * x123); const FLT x132 = x108 + x110; const FLT x133 = (-1 * ((x91 * x130) + (-1 * x74 * x132)) * x92) + (-1 * x90 * ((x88 * x131) + (-1 * ((x85 * x130) + (x84 * x132)) * x87))); const FLT x134 = x7 * x47; const FLT x135 = x39 * x20; const FLT x136 = 1.0 * x9; const FLT x137 = 1.0 * x14; const FLT x138 = 1.0 * x12; const FLT x139 = x35 * (1. / (x15 * sqrt(x15))); const FLT x140 = x139 * ((x138 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x13) + (x137 * (*error_model).Pose.AxisAngleRot[0]) + (x136 * (*error_model).Pose.AxisAngleRot[2])); const FLT x141 = 1.0/2.0 * x38; const FLT x142 = x10 * x141; const FLT x143 = x12 * x142; const FLT x144 = x23 * x141; const FLT x145 = x9 * x144; const FLT x146 = x20 * x141; const FLT x147 = x13 * x146; const FLT x148 = 1.0/2.0 * x33; const FLT x149 = x14 * x148; const FLT x150 = -1 * x8 * x40; const FLT x151 = x6 * x43; const FLT x152 = x151 + x150; const FLT x153 = x152 + (-1 * x140 * x149) + x135 + (x140 * x143) + x134 + (-1 * x140 * x145) + (-1 * x140 * x147); const FLT x154 = 2 * x57; const FLT x155 = x9 * x146; const FLT x156 = x14 * x10; const FLT x157 = x141 * x156; const FLT x158 = x8 * x43; const FLT x159 = x6 * x40; const FLT x160 = x13 * x144; const FLT x161 = x12 * x148; const FLT x162 = -1 * x7 * x135; const FLT x163 = x47 + x162; const FLT x164 = (-1 * x140 * x157) + (-1 * x160 * x140) + x163 + (x140 * x155) + (-1 * x161 * x140) + x158 + x159; const FLT x165 = x14 * x146; const FLT x166 = x13 * x148; const FLT x167 = x9 * x142; const FLT x168 = x8 * x47; const FLT x169 = -1 * x168; const FLT x170 = x12 * x144; const FLT x171 = x7 * x40; const FLT x172 = -1 * x171; const FLT x173 = x6 * x135; const FLT x174 = x43 + (-1 * x173); const FLT x175 = x174 + x172 + (-1 * x166 * x140) + (x170 * x140) + (x165 * x140) + x169 + (x167 * x140); const FLT x176 = (x153 * sensor_pt[1]) + (-1 * x164 * sensor_pt[0]) + (x175 * sensor_pt[2]); const FLT x177 = 2 * x52; const FLT x178 = x9 * x148; const FLT x179 = x13 * x142; const FLT x180 = x12 * x146; const FLT x181 = x7 * x43; const FLT x182 = x14 * x144; const FLT x183 = x8 * x135; const FLT x184 = -1 * x6 * x47; const FLT x185 = x184 + x183; const FLT x186 = (x182 * x140) + (-1 * x178 * x140) + x40 + (-1 * x180 * x140) + x185 + (-1 * x179 * x140) + x181; const FLT x187 = 2 * x60; const FLT x188 = (x164 * sensor_pt[2]) + (x175 * sensor_pt[0]) + (-1 * x186 * sensor_pt[1]); const FLT x189 = 2 * x49; const FLT x190 = (x189 * x188) + (x187 * x186) + (-1 * x153 * x154) + (-1 * x176 * x177); const FLT x191 = (x186 * sensor_pt[0]) + (-1 * x153 * sensor_pt[2]) + (x175 * sensor_pt[1]); const FLT x192 = 2 * x53; const FLT x193 = 2 * x56; const FLT x194 = (x164 * x154) + (x176 * x193) + (-1 * x189 * x191) + (-1 * x186 * x192); const FLT x195 = (x192 * x153) + (x177 * x191) + (-1 * x188 * x193) + (-1 * x164 * x187); const FLT x196 = (x195 * (*lh_p).Rot[2]) + (-1 * x190 * (*lh_p).Rot[3]) + (x194 * (*lh_p).Rot[0]); const FLT x197 = (x190 * (*lh_p).Rot[0]) + (x194 * (*lh_p).Rot[3]) + (-1 * x195 * (*lh_p).Rot[1]); const FLT x198 = x195 + (-1 * x107 * x196) + (x109 * x197); const FLT x199 = (x190 * (*lh_p).Rot[1]) + (-1 * x194 * (*lh_p).Rot[2]) + (x195 * (*lh_p).Rot[0]); const FLT x200 = x190 + (-1 * x109 * x199) + (x113 * x196); const FLT x201 = (-1 * x113 * x197) + x194 + (x107 * x199); const FLT x202 = (-1 * ((x91 * x198) + (-1 * x74 * x201)) * x92) + (-1 * x90 * ((x88 * x200) + (-1 * ((x85 * x198) + (x84 * x201)) * x87))); const FLT x203 = 1.0 * x13; const FLT x204 = x139 * ((-1 * x138 * (*error_model).Pose.AxisAngleRot[2]) + (x136 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x14) + (-1 * x203 * (*error_model).Pose.AxisAngleRot[0])); const FLT x205 = x12 * x204; const FLT x206 = (-1 * x204 * x149) + x174 + (-1 * x204 * x147) + (-1 * x204 * x145) + (x205 * x142) + x168 + x171; const FLT x207 = -1 * x159; const FLT x208 = (-1 * x47) + x162; const FLT x209 = (x204 * x182) + x207 + (-1 * x205 * x146) + x208 + (-1 * x204 * x179) + x158 + (-1 * x204 * x178); const FLT x210 = -1 * x135; const FLT x211 = (-1 * x151) + x150; const FLT x212 = x211 + (x205 * x144) + (x204 * x165) + x210 + (x204 * x167) + x134 + (-1 * x204 * x166); const FLT x213 = -1 * x181; const FLT x214 = (-1 * x183) + x184; const FLT x215 = (-1 * x204 * x161) + x40 + (-1 * x204 * x160) + x213 + (-1 * x204 * x157) + x214 + (x204 * x155); const FLT x216 = (-1 * x209 * sensor_pt[1]) + (x215 * sensor_pt[2]) + (x212 * sensor_pt[0]); const FLT x217 = (x206 * sensor_pt[1]) + (x212 * sensor_pt[2]) + (-1 * x215 * sensor_pt[0]); const FLT x218 = (x209 * x187) + (-1 * x217 * x177) + (-1 * x206 * x154) + (x216 * x189); const FLT x219 = (-1 * x206 * sensor_pt[2]) + (x209 * sensor_pt[0]) + (x212 * sensor_pt[1]); const FLT x220 = (-1 * x215 * x187) + (-1 * x216 * x193) + (x219 * x177) + (x206 * x192); const FLT x221 = (x215 * x154) + (x217 * x193) + (-1 * x219 * x189) + (-1 * x209 * x192); const FLT x222 = (x218 * (*lh_p).Rot[0]) + (x221 * (*lh_p).Rot[3]) + (-1 * x220 * (*lh_p).Rot[1]); const FLT x223 = (x221 * (*lh_p).Rot[0]) + (x220 * (*lh_p).Rot[2]) + (-1 * x218 * (*lh_p).Rot[3]); const FLT x224 = x220 + (x222 * x109) + (-1 * x223 * x107); const FLT x225 = (-1 * x221 * (*lh_p).Rot[2]) + (x218 * (*lh_p).Rot[1]) + (x220 * (*lh_p).Rot[0]); const FLT x226 = x218 + (x223 * x113) + (-1 * x225 * x109); const FLT x227 = x221 + (-1 * x222 * x113) + (x225 * x107); const FLT x228 = (-1 * ((x91 * x224) + (-1 * x74 * x227)) * x92) + (-1 * x90 * ((x88 * x226) + (-1 * ((x85 * x224) + (x84 * x227)) * x87))); const FLT x229 = -1 * x40; const FLT x230 = 1.0 * (*error_model).Pose.AxisAngleRot[2]; const FLT x231 = x139 * ((2 * x12) + (-1 * x136 * (*error_model).Pose.AxisAngleRot[0]) + (x14 * x230) + (-1 * x203 * (*error_model).Pose.AxisAngleRot[1])); const FLT x232 = x13 * x231; const FLT x233 = x214 + (-1 * x231 * x149) + x181 + x229 + (-1 * x231 * x145) + (-1 * x232 * x146) + (x231 * x143); const FLT x234 = -1 * x158; const FLT x235 = x12 * x231; const FLT x236 = x208 + (x235 * x144) + x234 + (-1 * x231 * x166) + (x231 * x165) + x159 + (x231 * x167); const FLT x237 = x173 + x43; const FLT x238 = (x231 * x155) + x171 + x169 + (-1 * x231 * x157) + x237 + (-1 * x232 * x144) + (-1 * x231 * x161); const FLT x239 = (x236 * sensor_pt[2]) + (x233 * sensor_pt[1]) + (-1 * x238 * sensor_pt[0]); const FLT x240 = -1 * x134; const FLT x241 = x211 + x240 + x135 + (-1 * x232 * x142) + (-1 * x235 * x146) + (x231 * x182) + (-1 * x231 * x178); const FLT x242 = (x238 * sensor_pt[2]) + (-1 * x241 * sensor_pt[1]) + (x236 * sensor_pt[0]); const FLT x243 = (x242 * x189) + (x241 * x187) + (-1 * x233 * x154) + (-1 * x239 * x177); const FLT x244 = (x241 * sensor_pt[0]) + (-1 * x233 * sensor_pt[2]) + (x236 * sensor_pt[1]); const FLT x245 = (x244 * x177) + (x233 * x192) + (-1 * x238 * x187) + (-1 * x242 * x193); const FLT x246 = (x238 * x154) + (x239 * x193) + (-1 * x244 * x189) + (-1 * x241 * x192); const FLT x247 = 2 * ((x243 * (*lh_p).Rot[0]) + (x246 * (*lh_p).Rot[3]) + (-1 * x245 * (*lh_p).Rot[1])); const FLT x248 = 2 * ((x245 * (*lh_p).Rot[2]) + (-1 * x243 * (*lh_p).Rot[3]) + (x246 * (*lh_p).Rot[0])); const FLT x249 = x245 + (x247 * (*lh_p).Rot[1]) + (-1 * x248 * (*lh_p).Rot[2]); const FLT x250 = 2 * ((x243 * (*lh_p).Rot[1]) + (-1 * x246 * (*lh_p).Rot[2]) + (x245 * (*lh_p).Rot[0])); const FLT x251 = (-1 * x250 * (*lh_p).Rot[1]) + x243 + (x248 * (*lh_p).Rot[3]); const FLT x252 = x246 + (-1 * x247 * (*lh_p).Rot[3]) + (x250 * (*lh_p).Rot[2]); const FLT x253 = (-1 * ((x91 * x249) + (-1 * x74 * x252)) * x92) + (-1 * x90 * ((x88 * x251) + (-1 * ((x85 * x249) + (x84 * x252)) * x87))); const FLT x254 = x139 * ((x138 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x137 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x9) + (-1 * x13 * x230)); const FLT x255 = x254 * x144; const FLT x256 = x152 + (-1 * x254 * x161) + (x254 * x155) + x240 + x210 + (-1 * x254 * x157) + (-1 * x13 * x255); const FLT x257 = x185 + (x254 * x167) + (x254 * x165) + (x12 * x255) + x213 + (-1 * x254 * x166) + x229; const FLT x258 = x13 * x254; const FLT x259 = x207 + (x254 * x143) + (-1 * x254 * x149) + x163 + (-1 * x258 * x146) + (-1 * x9 * x255) + x234; const FLT x260 = (x259 * sensor_pt[1]) + (-1 * x256 * sensor_pt[0]) + (x257 * sensor_pt[2]); const FLT x261 = x237 + x168 + (-1 * x254 * x178) + (-1 * x254 * x180) + (x14 * x255) + x172 + (-1 * x258 * x142); const FLT x262 = (x256 * sensor_pt[2]) + (-1 * x261 * sensor_pt[1]) + (x257 * sensor_pt[0]); const FLT x263 = (x262 * x189) + (-1 * x260 * x177) + (x261 * x187) + (-1 * x259 * x154); const FLT x264 = (x261 * sensor_pt[0]) + (x257 * sensor_pt[1]) + (-1 * x259 * sensor_pt[2]); const FLT x265 = (x256 * x154) + (x260 * x193) + (-1 * x264 * x189) + (-1 * x261 * x192); const FLT x266 = (x259 * x192) + (x264 * x177) + (-1 * x262 * x193) + (-1 * x256 * x187); const FLT x267 = (x266 * (*lh_p).Rot[2]) + (-1 * x263 * (*lh_p).Rot[3]) + (x265 * (*lh_p).Rot[0]); const FLT x268 = (-1 * x266 * (*lh_p).Rot[1]) + (x265 * (*lh_p).Rot[3]) + (x263 * (*lh_p).Rot[0]); const FLT x269 = x266 + (-1 * x267 * x107) + (x268 * x109); const FLT x270 = (x263 * (*lh_p).Rot[1]) + (-1 * x265 * (*lh_p).Rot[2]) + (x266 * (*lh_p).Rot[0]); const FLT x271 = x263 + (x267 * x113) + (-1 * x270 * x109); const FLT x272 = (-1 * x268 * x113) + x265 + (x270 * x107); const FLT x273 = (-1 * ((x91 * x269) + (-1 * x74 * x272)) * x92) + (-1 * x90 * ((x88 * x271) + (-1 * ((x85 * x269) + (x84 * x272)) * x87))); const FLT x274 = dt * dt * dt * dt; const FLT x275 = (x20 * x20 * x20) * x274; const FLT x276 = 1.0 * x33; const FLT x277 = x29 * (1. / (x26 * sqrt(x26))); const FLT x278 = x277 * x276; const FLT x279 = x20 * x17; const FLT x280 = 2 * x32; const FLT x281 = 2 * x30 * (1. / (x26 * x26)); const FLT x282 = x274 * x281; const FLT x283 = x18 * x282; const FLT x284 = x278 * x274; const FLT x285 = x20 * x284; const FLT x286 = x37 * x276; const FLT x287 = x24 * x282; const FLT x288 = (1. / (x34 * sqrt(x34))) * x16; const FLT x289 = ((-1 * x20 * x287) + (x24 * x285) + (x279 * x280) + (-1 * x279 * x286) + (x278 * x275) + (-1 * x20 * x283) + (-1 * x275 * x281) + (x18 * x285)) * x288; const FLT x290 = 0.5 * x37; const FLT x291 = x279 * x290; const FLT x292 = x36 * x291; const FLT x293 = x23 * x20; const FLT x294 = dt * dt * dt; const FLT x295 = 0.5 * x31 * x294; const FLT x296 = x48 * x295; const FLT x297 = x296 * x293; const FLT x298 = x13 * x289; const FLT x299 = x277 * x294; const FLT x300 = x41 * x299; const FLT x301 = x10 * x300; const FLT x302 = x20 * x301; const FLT x303 = x44 * x295; const FLT x304 = x36 * x299; const FLT x305 = x9 * x304; const FLT x306 = x293 * x305; const FLT x307 = x13 * x304; const FLT x308 = x54 * x295; const FLT x309 = x10 * x308; const FLT x310 = x20 * x309; const FLT x311 = (-1 * x310) + x297 + (x289 * x143) + (-1 * x298 * x146) + (-1 * x289 * x145) + (-1 * x289 * x149) + (-1 * x14 * x292) + (x21 * x303) + (-1 * x21 * x307) + x51 + x302 + (-1 * x306); const FLT x312 = -1 * x55; const FLT x313 = x50 * x295; const FLT x314 = x20 * x10; const FLT x315 = x313 * x314; const FLT x316 = x14 * x304; const FLT x317 = x10 * x316; const FLT x318 = x20 * x317; const FLT x319 = (-1 * x293 * x307) + (x293 * x303); const FLT x320 = x319 + (x21 * x305) + (-1 * x289 * x157) + (-1 * x298 * x144) + (-1 * x289 * x161) + (-1 * x41 * x291) + x315 + (-1 * x318) + (x289 * x155) + x312 + (-1 * x21 * x296); const FLT x321 = -1 * x45; const FLT x322 = x293 * x300; const FLT x323 = x293 * x308; const FLT x324 = x10 * x305; const FLT x325 = (x20 * x324) + (-1 * x296 * x314); const FLT x326 = (-1 * x289 * x166) + (-1 * x13 * x292) + (-1 * x323) + (x289 * x167) + (-1 * x21 * x313) + x321 + (x21 * x316) + (x289 * x170) + x325 + (x289 * x165) + x322; const FLT x327 = (x311 * sensor_pt[1]) + (-1 * x320 * sensor_pt[0]) + (x326 * sensor_pt[2]); const FLT x328 = (x293 * x316) + (-1 * x293 * x313); const FLT x329 = x10 * x307; const FLT x330 = (-1 * x20 * x329) + (x303 * x314); const FLT x331 = x330 + x42 + (-1 * x21 * x300) + x328 + (-1 * x298 * x142) + (x289 * x182) + (x21 * x308) + (-1 * x289 * x178) + (-1 * x289 * x180) + (-1 * x9 * x292); const FLT x332 = (x320 * sensor_pt[2]) + (-1 * x331 * sensor_pt[1]) + (x326 * sensor_pt[0]); const FLT x333 = (x331 * x187) + (x332 * x189) + (-1 * x311 * x154) + (-1 * x327 * x177); const FLT x334 = (x331 * sensor_pt[0]) + (-1 * x311 * sensor_pt[2]) + (x326 * sensor_pt[1]); const FLT x335 = (x327 * x193) + (-1 * x331 * x192) + (-1 * x334 * x189) + (x320 * x154); const FLT x336 = (-1 * x320 * x187) + (x334 * x177) + (-1 * x332 * x193) + (x311 * x192); const FLT x337 = (x336 * (*lh_p).Rot[2]) + (-1 * x333 * (*lh_p).Rot[3]) + (x335 * (*lh_p).Rot[0]); const FLT x338 = (x335 * (*lh_p).Rot[3]) + (-1 * x336 * (*lh_p).Rot[1]) + (x333 * (*lh_p).Rot[0]); const FLT x339 = x336 + (-1 * x337 * x107) + (x338 * x109); const FLT x340 = (x333 * (*lh_p).Rot[1]) + (-1 * x335 * (*lh_p).Rot[2]) + (x336 * (*lh_p).Rot[0]); const FLT x341 = x333 + (-1 * x340 * x109) + (x337 * x113); const FLT x342 = x335 + (-1 * x338 * x113) + (x340 * x107); const FLT x343 = (-1 * ((x91 * x339) + (-1 * x74 * x342)) * x92) + (-1 * x90 * ((x88 * x341) + (-1 * ((x85 * x339) + (x84 * x342)) * x87))); const FLT x344 = x17 * x290; const FLT x345 = x36 * x344; const FLT x346 = x23 * x345; const FLT x347 = x21 * x284; const FLT x348 = x21 * x282; const FLT x349 = x17 * x286; const FLT x350 = x23 * x23 * x23; const FLT x351 = x17 * x280; const FLT x352 = x288 * ((-1 * x282 * x350) + (-1 * x23 * x348) + (x23 * x351) + (x284 * x350) + (-1 * x23 * x349) + (x23 * x347) + (x23 * x18 * x284) + (-1 * x23 * x283)); const FLT x353 = x13 * x352; const FLT x354 = (x23 * x301) + (-1 * x23 * x309); const FLT x355 = x319 + x354 + x55 + (x24 * x296) + (-1 * x14 * x346) + (x352 * x143) + (-1 * x352 * x145) + (-1 * x352 * x149) + (-1 * x353 * x146) + (-1 * x24 * x305); const FLT x356 = x14 * x352; const FLT x357 = x23 * x10; const FLT x358 = (x357 * x303) + (-1 * x23 * x329); const FLT x359 = x358 + (-1 * x9 * x346) + (-1 * x24 * x313) + (-1 * x353 * x142) + x321 + (-1 * x352 * x178) + (-1 * x352 * x180) + x323 + (-1 * x322) + (x24 * x316) + (x356 * x144); const FLT x360 = -1 * x42; const FLT x361 = x296 * x357; const FLT x362 = x23 * x324; const FLT x363 = x328 + (x356 * x146) + (x352 * x170) + (-1 * x13 * x346) + (x24 * x300) + (-1 * x361) + (-1 * x352 * x166) + (-1 * x24 * x308) + x360 + (x352 * x167) + x362; const FLT x364 = x357 * x313; const FLT x365 = x41 * x344; const FLT x366 = x23 * x317; const FLT x367 = (-1 * x23 * x365) + x51 + x364 + (-1 * x352 * x160) + (-1 * x352 * x157) + (-1 * x297) + (-1 * x24 * x307) + (x352 * x155) + (-1 * x366) + x306 + (x24 * x303) + (-1 * x352 * x161); const FLT x368 = (x367 * sensor_pt[2]) + (-1 * x359 * sensor_pt[1]) + (x363 * sensor_pt[0]); const FLT x369 = (-1 * x367 * sensor_pt[0]) + (x355 * sensor_pt[1]) + (x363 * sensor_pt[2]); const FLT x370 = (-1 * x369 * x177) + (x368 * x189) + (-1 * x355 * x154) + (x359 * x187); const FLT x371 = (x359 * sensor_pt[0]) + (-1 * x355 * sensor_pt[2]) + (x363 * sensor_pt[1]); const FLT x372 = (x367 * x154) + (-1 * x371 * x189) + (x369 * x193) + (-1 * x359 * x192); const FLT x373 = (x371 * x177) + (-1 * x367 * x187) + (-1 * x368 * x193) + (x355 * x192); const FLT x374 = (x373 * (*lh_p).Rot[2]) + (-1 * x370 * (*lh_p).Rot[3]) + (x372 * (*lh_p).Rot[0]); const FLT x375 = (x372 * (*lh_p).Rot[3]) + (x370 * (*lh_p).Rot[0]) + (-1 * x373 * (*lh_p).Rot[1]); const FLT x376 = x373 + (-1 * x374 * x107) + (x375 * x109); const FLT x377 = (-1 * x372 * (*lh_p).Rot[2]) + (x370 * (*lh_p).Rot[1]) + (x373 * (*lh_p).Rot[0]); const FLT x378 = x370 + (-1 * x377 * x109) + (x374 * x113); const FLT x379 = x372 + (-1 * x375 * x113) + (x377 * x107); const FLT x380 = (-1 * ((x91 * x376) + (-1 * x74 * x379)) * x92) + (-1 * x90 * ((x88 * x378) + (-1 * ((x85 * x376) + (x84 * x379)) * x87))); const FLT x381 = x10 * x10 * x10; const FLT x382 = x288 * ((x24 * x10 * x284) + (-1 * x10 * x349) + (x10 * x347) + (x10 * x351) + (-1 * x282 * x381) + (x284 * x381) + (-1 * x10 * x348) + (-1 * x10 * x287)); const FLT x383 = x18 * x295; const FLT x384 = x13 * x382; const FLT x385 = x330 + (-1 * x384 * x146) + (x382 * x143) + (x18 * x300) + (-1 * x345 * x156) + (-1 * x362) + x360 + (-1 * x382 * x149) + (-1 * x54 * x383) + (-1 * x382 * x145) + x361; const FLT x386 = (-1 * x382 * x157) + (-1 * x18 * x316) + (-1 * x10 * x365) + x325 + x45 + (-1 * x382 * x160) + (x50 * x383) + x358 + (-1 * x382 * x161) + (x382 * x155); const FLT x387 = x10 * x345; const FLT x388 = x14 * x382; const FLT x389 = x354 + (x388 * x146) + x318 + (-1 * x13 * x387) + (x18 * x305) + (x382 * x167) + (x382 * x170) + (-1 * x315) + (-1 * x382 * x166) + (-1 * x48 * x383) + x312; const FLT x390 = (x385 * sensor_pt[1]) + (-1 * x386 * sensor_pt[0]) + (x389 * sensor_pt[2]); const FLT x391 = (-1 * x302) + x51 + x366 + (x388 * x144) + x310 + (-1 * x382 * x180) + (x44 * x383) + (-1 * x384 * x142) + (-1 * x18 * x307) + (-1 * x9 * x387) + (-1 * x382 * x178) + (-1 * x364); const FLT x392 = (x386 * sensor_pt[2]) + (-1 * x391 * sensor_pt[1]) + (x389 * sensor_pt[0]); const FLT x393 = (x391 * x187) + (-1 * x385 * x154) + (x392 * x189) + (-1 * x390 * x177); const FLT x394 = (x391 * sensor_pt[0]) + (-1 * x385 * sensor_pt[2]) + (x389 * sensor_pt[1]); const FLT x395 = (x386 * x154) + (x390 * x193) + (-1 * x394 * x189) + (-1 * x391 * x192); const FLT x396 = (-1 * x386 * x187) + (x385 * x192) + (-1 * x392 * x193) + (x394 * x177); const FLT x397 = (-1 * x393 * (*lh_p).Rot[3]) + (x396 * (*lh_p).Rot[2]) + (x395 * (*lh_p).Rot[0]); const FLT x398 = (x395 * (*lh_p).Rot[3]) + (-1 * x396 * (*lh_p).Rot[1]) + (x393 * (*lh_p).Rot[0]); const FLT x399 = x396 + (-1 * x397 * x107) + (x398 * x109); const FLT x400 = (x393 * (*lh_p).Rot[1]) + (-1 * x395 * (*lh_p).Rot[2]) + (x396 * (*lh_p).Rot[0]); const FLT x401 = x393 + (x397 * x113) + (-1 * x400 * x109); const FLT x402 = x395 + (-1 * x398 * x113) + (x400 * x107); const FLT x403 = (-1 * ((x91 * x399) + (-1 * x74 * x402)) * x92) + (-1 * x90 * ((x88 * x401) + (-1 * ((x85 * x399) + (x84 * x402)) * x87))); const FLT x404 = dt * x108; const FLT x405 = dt * x110; const FLT x406 = x405 + (-1 * x404); const FLT x407 = dt * x112; const FLT x408 = dt * x114; const FLT x409 = x408 + x407; const FLT x410 = -1 * dt * x118; const FLT x411 = (-1 * dt * x116) + dt; const FLT x412 = x411 + x410; const FLT x413 = (-1 * ((x91 * x406) + (-1 * x74 * x412)) * x92) + (-1 * x90 * ((x88 * x409) + (-1 * ((x85 * x406) + (x84 * x412)) * x87))); const FLT x414 = dt * x122; const FLT x415 = dt * x123; const FLT x416 = x415 + x414; const FLT x417 = -1 * dt * x125; const FLT x418 = x411 + x417; const FLT x419 = x407 + (-1 * x408); const FLT x420 = (-1 * ((x91 * x416) + (-1 * x74 * x419)) * x92) + (-1 * x90 * ((x88 * x418) + (-1 * ((x85 * x416) + (x84 * x419)) * x87))); const FLT x421 = x410 + dt + x417; const FLT x422 = x414 + (-1 * x415); const FLT x423 = x404 + x405; const FLT x424 = (-1 * ((x91 * x421) + (-1 * x74 * x423)) * x92) + (-1 * x90 * ((x88 * x422) + (-1 * ((x85 * x421) + (x84 * x423)) * x87))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x93 + (x77 * ((-1 * x73 * x74) + (x5 * x70))) + (x93 * x94)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x102 + (x77 * ((-1 * x74 * x100) + (x70 * x97))) + (x94 * x102)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), (((-1 * x74 * x104) + (x70 * x103)) * x77) + x106 + (x94 * x106)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), (((-1 * x74 * x115) + (x70 * x111)) * x77) + x121 + (x94 * x121)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x129 + (((-1 * x74 * x127) + (x70 * x124)) * x77) + (x94 * x129)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x133 + (((-1 * x74 * x131) + (x70 * x130)) * x77) + (x94 * x133)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x202 + (((-1 * x74 * x200) + (x70 * x198)) * x77) + (x94 * x202)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (((-1 * x74 * x226) + (x70 * x224)) * x77) + x228 + (x94 * x228)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x253 + (((-1 * x74 * x251) + (x70 * x249)) * x77) + (x94 * x253)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x273 + (((-1 * x74 * x271) + (x70 * x269)) * x77) + (x94 * x273)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x343 + (((-1 * x74 * x341) + (x70 * x339)) * x77) + (x94 * x343)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x380 + (((-1 * x74 * x378) + (x70 * x376)) * x77) + (x94 * x380)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x403 + (((-1 * x74 * x401) + (x70 * x399)) * x77) + (x94 * x403)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x413 + (((-1 * x74 * x409) + (x70 * x406)) * x77) + (x94 * x413)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x420 + (((-1 * x74 * x418) + (x70 * x416)) * x77) + (x94 * x420)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x424 + (((-1 * x74 * x422) + (x70 * x421)) * x77) + (x94 * x424)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = x0 * (*lh_p).Rot[0]; const FLT x2 = x1 * (*lh_p).Rot[2]; const FLT x3 = x0 * (*lh_p).Rot[1]; const FLT x4 = x3 * (*lh_p).Rot[3]; const FLT x5 = x4 + (-1 * x2); const FLT x6 = 1.0/2.0 * x0; const FLT x7 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x8 = dt * dt; const FLT x9 = x7 * x7; const FLT x10 = x8 * x9; const FLT x11 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x12 = x11 * x11; const FLT x13 = x8 * x12; const FLT x14 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x15 = x14 * x14; const FLT x16 = x8 * x15; const FLT x17 = 1e-10 + x16 + x10 + x13; const FLT x18 = sqrt(x17); const FLT x19 = 0.5 * x18; const FLT x20 = sin(x19); const FLT x21 = x20 * (1. / x18); const FLT x22 = x7 * x21; const FLT x23 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x24 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x25 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x26 = (x25 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x23 * (*error_model).Pose.AxisAngleRot[0]) + (x24 * (*_x0).Pose.Rot[0]); const FLT x27 = x20 * x20; const FLT x28 = 1. / x17; const FLT x29 = x28 * x27; const FLT x30 = cos(x19); const FLT x31 = (x29 * x13) + (x29 * x10) + (x30 * x30) + (x29 * x16); const FLT x32 = 1. / sqrt(x31); const FLT x33 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x34 = (-1 * x24 * (*_x0).Pose.Rot[1]) + (x25 * (*_x0).Pose.Rot[0]) + (x33 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x35 = (-1 * x33 * (*_x0).Pose.Rot[1]) + (-1 * x23 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[0] + (-1 * x24 * (*_x0).Pose.Rot[3]); const FLT x36 = (*_x0).Pose.Rot[1] + (-1 * x25 * (*_x0).Pose.Rot[3]) + (x33 * (*_x0).Pose.Rot[0]) + (x23 * (*error_model).Pose.AxisAngleRot[2]); const FLT x37 = (x36 * x36) + (x35 * x35) + (x26 * x26) + (x34 * x34); const FLT x38 = 1. / sqrt(x37); const FLT x39 = dt * x38; const FLT x40 = x32 * x39; const FLT x41 = x40 * x26; const FLT x42 = x21 * x14; const FLT x43 = x40 * x34; const FLT x44 = x30 * x38; const FLT x45 = x44 * x32; const FLT x46 = x45 * x35; const FLT x47 = x32 * x36; const FLT x48 = x39 * x21; const FLT x49 = x47 * x48; const FLT x50 = x46 + (-1 * x41 * x22) + (-1 * x49 * x11) + (-1 * x42 * x43); const FLT x51 = x40 * x35; const FLT x52 = x45 * x34; const FLT x53 = x47 * x39; const FLT x54 = x48 * x32; const FLT x55 = x54 * x26; const FLT x56 = (-1 * x55 * x11) + (x53 * x22) + (x51 * x42) + x52; const FLT x57 = x45 * x26; const FLT x58 = x54 * x34; const FLT x59 = (x58 * x11) + x57 + (x51 * x22) + (-1 * x53 * x42); const FLT x60 = (-1 * x59 * sensor_pt[1]) + (x50 * sensor_pt[0]) + (x56 * sensor_pt[2]); const FLT x61 = x44 * x47; const FLT x62 = x54 * x35; const FLT x63 = (x62 * x11) + (-1 * x43 * x22) + (x41 * x42) + x61; const FLT x64 = (-1 * x63 * sensor_pt[2]) + (x50 * sensor_pt[1]) + (x59 * sensor_pt[0]); const FLT x65 = (x6 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (*_x0).Pose.Pos[2] + (2 * ((x63 * x64) + (-1 * x60 * x56))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x66 = (-1 * x56 * sensor_pt[0]) + (x50 * sensor_pt[2]) + (x63 * sensor_pt[1]); const FLT x67 = (*_x0).Pose.Pos[1] + (2 * ((x60 * x59) + (-1 * x63 * x66))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x6 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x68 = (2 * ((x66 * x56) + (-1 * x64 * x59))) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (x6 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x69 = (-1 * x68 * (*lh_p).Rot[2]) + (x65 * (*lh_p).Rot[0]) + (x67 * (*lh_p).Rot[1]); const FLT x70 = (-1 * x67 * (*lh_p).Rot[3]) + (x68 * (*lh_p).Rot[0]) + (x65 * (*lh_p).Rot[2]); const FLT x71 = (*lh_p).Pos[1] + x67 + (2 * ((x70 * (*lh_p).Rot[3]) + (-1 * x69 * (*lh_p).Rot[1]))); const FLT x72 = (-1 * x65 * (*lh_p).Rot[1]) + (x67 * (*lh_p).Rot[0]) + (x68 * (*lh_p).Rot[3]); const FLT x73 = x65 + (2 * ((x72 * (*lh_p).Rot[1]) + (-1 * x70 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x74 = x73 * x73; const FLT x75 = 1. / x74; const FLT x76 = x71 * x75; const FLT x77 = x3 * (*lh_p).Rot[2]; const FLT x78 = x1 * (*lh_p).Rot[3]; const FLT x79 = x78 + x77; const FLT x80 = 1. / x73; const FLT x81 = x71 * x71; const FLT x82 = -1 * x73; const FLT x83 = 2 * (1. / (x74 + x81)) * x74 * atan2(x71, x82) * (*bsc0).curve; const FLT x84 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x85 = -1 * x0 * x84; const FLT x86 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x87 = (-1 * x0 * x86) + x6; const FLT x88 = x87 + x85; const FLT x89 = x68 + (*lh_p).Pos[0] + (2 * ((x69 * (*lh_p).Rot[2]) + (-1 * x72 * (*lh_p).Rot[3]))); const FLT x90 = 2 * x89; const FLT x91 = 2 * x73; const FLT x92 = (x89 * x89) + x74; const FLT x93 = 1.0/2.0 * x71 * (1. / (x92 * sqrt(x92))) * (*bsc0).tilt; const FLT x94 = (1. / sqrt(x92)) * (*bsc0).tilt; const FLT x95 = 1. / x92; const FLT x96 = 1. / sqrt(1 + (-1 * x81 * x95 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x97 = x89 * x75; const FLT x98 = x74 * x95; const FLT x99 = (-1 * x98 * ((x5 * x97) + (-1 * x80 * x88))) + (-1 * x96 * ((x79 * x94) + (-1 * x93 * ((x5 * x91) + (x88 * x90))))); const FLT x100 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * asin(x71 * x94)) + (-1 * atan2(x89, x82)) + (-1 * (*bsc0).phase) + (*bsc0).gibpha); const FLT x101 = x0 * (*lh_p).Rot[3] * (*lh_p).Rot[2]; const FLT x102 = x1 * (*lh_p).Rot[1]; const FLT x103 = x102 + x101; const FLT x104 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x105 = -1 * x0 * x104; const FLT x106 = x105 + x85 + x6; const FLT x107 = x77 + (-1 * x78); const FLT x108 = (-1 * ((x97 * x103) + (-1 * x80 * x107)) * x98) + (-1 * x96 * ((x94 * x106) + (-1 * ((x91 * x103) + (x90 * x107)) * x93))); const FLT x109 = x87 + x105; const FLT x110 = x101 + (-1 * x102); const FLT x111 = x2 + x4; const FLT x112 = (-1 * ((x97 * x109) + (-1 * x80 * x111)) * x98) + (-1 * x96 * ((x94 * x110) + (-1 * ((x91 * x109) + (x90 * x111)) * x93))); const FLT x113 = 1.0 * (*_x0).Pose.Rot[1]; const FLT x114 = 1.0 * x26; const FLT x115 = 1.0 * x36; const FLT x116 = 1.0 * x34; const FLT x117 = (x115 * (*_x0).Pose.Rot[0]) + (x116 * (*_x0).Pose.Rot[3]) + (-1 * x35 * x113) + (-1 * x114 * (*_x0).Pose.Rot[2]); const FLT x118 = 1. / (x37 * sqrt(x37)); const FLT x119 = x32 * x118; const FLT x120 = x119 * x117; const FLT x121 = 1.0/2.0 * x35; const FLT x122 = x11 * x121; const FLT x123 = dt * x21; const FLT x124 = x123 * x122; const FLT x125 = dt * x22; const FLT x126 = 1.0/2.0 * x34; const FLT x127 = x119 * x126; const FLT x128 = x117 * x127; const FLT x129 = dt * x42; const FLT x130 = 1.0/2.0 * x26; const FLT x131 = x120 * x130; const FLT x132 = 1.0/2.0 * x47 * x118; const FLT x133 = x30 * x132; const FLT x134 = x40 * x22; const FLT x135 = 0.5 * x134; const FLT x136 = 0.5 * x45; const FLT x137 = x54 * x11; const FLT x138 = 0.5 * x137; const FLT x139 = (-1 * x40 * x42 * x23) + (-1 * x138 * (*_x0).Pose.Rot[1]) + (-1 * x135 * (*_x0).Pose.Rot[3]) + (x136 * (*_x0).Pose.Rot[0]); const FLT x140 = (-1 * x117 * x133) + x139 + (-1 * x129 * x131) + (-1 * x120 * x124) + (x128 * x125); const FLT x141 = 2 * x66; const FLT x142 = x117 * x132; const FLT x143 = x23 * x137; const FLT x144 = 0.5 * x42; const FLT x145 = x40 * x144; const FLT x146 = x145 * (*_x0).Pose.Rot[1]; const FLT x147 = x136 * (*_x0).Pose.Rot[3]; const FLT x148 = x11 * x123; const FLT x149 = x30 * x119; const FLT x150 = x117 * x149; const FLT x151 = x42 * x121; const FLT x152 = dt * x120; const FLT x153 = x40 * (*_x0).Pose.Rot[0]; const FLT x154 = 0.5 * x22 * x153; const FLT x155 = (-1 * x126 * x150) + x143 + x154 + (-1 * x151 * x152) + x147 + (-1 * x125 * x142) + (-1 * x146) + (x131 * x148); const FLT x156 = x145 * (*_x0).Pose.Rot[3]; const FLT x157 = x138 * (*_x0).Pose.Rot[0]; const FLT x158 = x136 * (*_x0).Pose.Rot[1]; const FLT x159 = x23 * x134; const FLT x160 = x159 + (-1 * x158) + (-1 * x156) + (-1 * x157); const FLT x161 = x160 + (x128 * x129) + (-1 * x121 * x150) + (x142 * x148) + (x125 * x131); const FLT x162 = (x140 * sensor_pt[1]) + (-1 * x155 * sensor_pt[0]) + (x161 * sensor_pt[2]); const FLT x163 = 2 * x63; const FLT x164 = x22 * x121; const FLT x165 = x144 * x153; const FLT x166 = x135 * (*_x0).Pose.Rot[1]; const FLT x167 = x45 * x23; const FLT x168 = x138 * (*_x0).Pose.Rot[3]; const FLT x169 = x168 + (-1 * x167) + (-1 * x165) + (-1 * x166); const FLT x170 = x169 + (x129 * x142) + (-1 * x130 * x150) + (-1 * x128 * x148) + (-1 * x164 * x152); const FLT x171 = 2 * x60; const FLT x172 = (x155 * sensor_pt[2]) + (-1 * x170 * sensor_pt[1]) + (x161 * sensor_pt[0]); const FLT x173 = 2 * x59; const FLT x174 = (x172 * x173) + (x170 * x171) + (-1 * x140 * x141) + (-1 * x163 * x162); const FLT x175 = (x170 * sensor_pt[0]) + (-1 * x140 * sensor_pt[2]) + (x161 * sensor_pt[1]); const FLT x176 = 2 * x64; const FLT x177 = 2 * x56; const FLT x178 = (x162 * x177) + (-1 * x175 * x173) + (-1 * x170 * x176) + (x141 * x155); const FLT x179 = (x163 * x175) + (x176 * x140) + (-1 * x177 * x172) + (-1 * x171 * x155); const FLT x180 = (x179 * (*lh_p).Rot[2]) + (-1 * x174 * (*lh_p).Rot[3]) + (x178 * (*lh_p).Rot[0]); const FLT x181 = 2 * (*lh_p).Rot[2]; const FLT x182 = (x178 * (*lh_p).Rot[3]) + (x174 * (*lh_p).Rot[0]) + (-1 * x179 * (*lh_p).Rot[1]); const FLT x183 = 2 * (*lh_p).Rot[1]; const FLT x184 = (-1 * x181 * x180) + x179 + (x183 * x182); const FLT x185 = (x174 * (*lh_p).Rot[1]) + (-1 * x178 * (*lh_p).Rot[2]) + (x179 * (*lh_p).Rot[0]); const FLT x186 = 2 * (*lh_p).Rot[3]; const FLT x187 = x174 + (-1 * x183 * x185) + (x180 * x186); const FLT x188 = x178 + (-1 * x186 * x182) + (x181 * x185); const FLT x189 = (-1 * ((x97 * x184) + (-1 * x80 * x188)) * x98) + (-1 * x96 * ((x94 * x187) + (-1 * ((x91 * x184) + (x90 * x188)) * x93))); const FLT x190 = 1.0 * x35; const FLT x191 = (x116 * (*_x0).Pose.Rot[0]) + (x26 * x113) + (-1 * x115 * (*_x0).Pose.Rot[3]) + (-1 * x190 * (*_x0).Pose.Rot[2]); const FLT x192 = x119 * x191; const FLT x193 = x127 * x191; const FLT x194 = x192 * x130; const FLT x195 = x146 + (-1 * x154) + (-1 * x147) + (-1 * x143); const FLT x196 = x195 + (-1 * x191 * x133) + (-1 * x129 * x194) + (-1 * x124 * x192) + (x125 * x193); const FLT x197 = x191 * x132; const FLT x198 = dt * x192; const FLT x199 = x191 * x149; const FLT x200 = (x194 * x148) + x139 + (-1 * x126 * x199) + (-1 * x125 * x197) + (-1 * x198 * x151); const FLT x201 = x169 + (x129 * x193) + (x125 * x194) + (x197 * x148) + (-1 * x121 * x199); const FLT x202 = (x196 * sensor_pt[1]) + (-1 * x200 * sensor_pt[0]) + (x201 * sensor_pt[2]); const FLT x203 = x158 + x157 + (-1 * x159) + (-1 * x193 * x148) + (-1 * x164 * x198) + (-1 * x199 * x130) + (x129 * x197) + x156; const FLT x204 = (x200 * sensor_pt[2]) + (x201 * sensor_pt[0]) + (-1 * x203 * sensor_pt[1]); const FLT x205 = (x204 * x173) + (x203 * x171) + (-1 * x196 * x141) + (-1 * x202 * x163); const FLT x206 = (x203 * sensor_pt[0]) + (-1 * x196 * sensor_pt[2]) + (x201 * sensor_pt[1]); const FLT x207 = (x200 * x141) + (x202 * x177) + (-1 * x206 * x173) + (-1 * x203 * x176); const FLT x208 = (x206 * x163) + (-1 * x204 * x177) + (x176 * x196) + (-1 * x200 * x171); const FLT x209 = (x208 * (*lh_p).Rot[2]) + (-1 * x205 * (*lh_p).Rot[3]) + (x207 * (*lh_p).Rot[0]); const FLT x210 = (x207 * (*lh_p).Rot[3]) + (-1 * x208 * (*lh_p).Rot[1]) + (x205 * (*lh_p).Rot[0]); const FLT x211 = x208 + (-1 * x209 * x181) + (x210 * x183); const FLT x212 = (x205 * (*lh_p).Rot[1]) + (-1 * x207 * (*lh_p).Rot[2]) + (x208 * (*lh_p).Rot[0]); const FLT x213 = x205 + (-1 * x212 * x183) + (x209 * x186); const FLT x214 = x207 + (x212 * x181) + (-1 * x210 * x186); const FLT x215 = (-1 * ((x97 * x211) + (-1 * x80 * x214)) * x98) + (-1 * x96 * ((x94 * x213) + (-1 * ((x91 * x211) + (x90 * x214)) * x93))); const FLT x216 = (-1 * x34 * x113) + (x114 * (*_x0).Pose.Rot[0]) + (x115 * (*_x0).Pose.Rot[2]) + (-1 * x190 * (*_x0).Pose.Rot[3]); const FLT x217 = x216 * x119; const FLT x218 = x216 * x125; const FLT x219 = x119 * x130; const FLT x220 = x219 * x216; const FLT x221 = x166 + x167 + (x218 * x127) + (-1 * x217 * x124) + (-1 * x168) + (-1 * x216 * x133) + (-1 * x220 * x129) + x165; const FLT x222 = x216 * x149; const FLT x223 = dt * x217; const FLT x224 = x160 + (-1 * x223 * x151) + (-1 * x222 * x126) + (x220 * x148) + (-1 * x218 * x132); const FLT x225 = x216 * x132; const FLT x226 = x216 * x127; const FLT x227 = x195 + (-1 * x222 * x121) + (x225 * x148) + (x226 * x129) + (x219 * x218); const FLT x228 = (x221 * sensor_pt[1]) + (-1 * x224 * sensor_pt[0]) + (x227 * sensor_pt[2]); const FLT x229 = (x225 * x129) + (-1 * x223 * x164) + x139 + (-1 * x226 * x148) + (-1 * x222 * x130); const FLT x230 = (x224 * sensor_pt[2]) + (-1 * x229 * sensor_pt[1]) + (x227 * sensor_pt[0]); const FLT x231 = (x230 * x173) + (x229 * x171) + (-1 * x221 * x141) + (-1 * x228 * x163); const FLT x232 = (x229 * sensor_pt[0]) + (-1 * x221 * sensor_pt[2]) + (x227 * sensor_pt[1]); const FLT x233 = (x224 * x141) + (-1 * x232 * x173) + (-1 * x229 * x176) + (x228 * x177); const FLT x234 = (x221 * x176) + (x232 * x163) + (-1 * x230 * x177) + (-1 * x224 * x171); const FLT x235 = (x234 * (*lh_p).Rot[2]) + (-1 * x231 * (*lh_p).Rot[3]) + (x233 * (*lh_p).Rot[0]); const FLT x236 = (x233 * (*lh_p).Rot[3]) + (-1 * x234 * (*lh_p).Rot[1]) + (x231 * (*lh_p).Rot[0]); const FLT x237 = x234 + (-1 * x235 * x181) + (x236 * x183); const FLT x238 = (x231 * (*lh_p).Rot[1]) + (-1 * x233 * (*lh_p).Rot[2]) + (x234 * (*lh_p).Rot[0]); const FLT x239 = (x235 * x186) + x231 + (-1 * x238 * x183); const FLT x240 = x233 + (x238 * x181) + (-1 * x236 * x186); const FLT x241 = (-1 * ((x97 * x237) + (-1 * x80 * x240)) * x98) + (-1 * x96 * ((x94 * x239) + (-1 * ((x91 * x237) + (x90 * x240)) * x93))); const FLT x242 = x181 * (*lh_p).Rot[0]; const FLT x243 = x186 * (*lh_p).Rot[1]; const FLT x244 = x243 + (-1 * x242); const FLT x245 = x181 * (*lh_p).Rot[1]; const FLT x246 = x186 * (*lh_p).Rot[0]; const FLT x247 = x246 + x245; const FLT x248 = 2 * x84; const FLT x249 = -1 * x248; const FLT x250 = 2 * x86; const FLT x251 = -1 * x250; const FLT x252 = 1 + x251 + x249; const FLT x253 = (-1 * ((x97 * x244) + (-1 * x80 * x252)) * x98) + (-1 * x96 * ((x94 * x247) + (-1 * ((x91 * x244) + (x90 * x252)) * x93))); const FLT x254 = x186 * (*lh_p).Rot[2]; const FLT x255 = x183 * (*lh_p).Rot[0]; const FLT x256 = x255 + x254; const FLT x257 = 2 * x104; const FLT x258 = 1 + (-1 * x257); const FLT x259 = x258 + x249; const FLT x260 = x245 + (-1 * x246); const FLT x261 = (-1 * ((x97 * x256) + (-1 * x80 * x260)) * x98) + (-1 * x96 * ((x94 * x259) + (-1 * ((x91 * x256) + (x90 * x260)) * x93))); const FLT x262 = x258 + x251; const FLT x263 = x254 + (-1 * x255); const FLT x264 = x242 + x243; const FLT x265 = (-1 * ((x97 * x262) + (-1 * x80 * x264)) * x98) + (-1 * x96 * ((x94 * x263) + (-1 * ((x91 * x262) + (x90 * x264)) * x93))); const FLT x266 = x11 * x11 * x11; const FLT x267 = dt * dt * dt * dt; const FLT x268 = 1.0 * x30; const FLT x269 = x20 * (1. / (x17 * sqrt(x17))); const FLT x270 = x268 * x269; const FLT x271 = x270 * x267; const FLT x272 = x8 * x11; const FLT x273 = 2 * x29; const FLT x274 = 2 * x27 * (1. / (x17 * x17)); const FLT x275 = x274 * x267; const FLT x276 = x9 * x11; const FLT x277 = x15 * x271; const FLT x278 = x21 * x272; const FLT x279 = x15 * x275; const FLT x280 = (-1 * x278 * x268) + (x11 * x277) + (x273 * x272) + (-1 * x275 * x266) + (-1 * x276 * x275) + (-1 * x11 * x279) + (x271 * x266) + (x276 * x271); const FLT x281 = 1. / (x31 * sqrt(x31)); const FLT x282 = x44 * x281; const FLT x283 = x282 * x280; const FLT x284 = 1.0/2.0 * x36; const FLT x285 = x47 * x38; const FLT x286 = 0.5 * x285; const FLT x287 = dt * dt * dt; const FLT x288 = 0.5 * x28; const FLT x289 = x287 * x288; const FLT x290 = x14 * x289; const FLT x291 = x57 * x290; const FLT x292 = x11 * x291; const FLT x293 = x39 * x281; const FLT x294 = x293 * x280; const FLT x295 = x42 * x294; const FLT x296 = x22 * x126; const FLT x297 = x48 * x281; const FLT x298 = x297 * x122; const FLT x299 = x32 * x38; const FLT x300 = x34 * x299; const FLT x301 = x11 * x300; const FLT x302 = x269 * x287; const FLT x303 = x7 * x302; const FLT x304 = x301 * x303; const FLT x305 = x12 * x289; const FLT x306 = x14 * x302; const FLT x307 = x26 * x299; const FLT x308 = x11 * x307; const FLT x309 = x308 * x306; const FLT x310 = x35 * x299; const FLT x311 = x12 * x302; const FLT x312 = x7 * x11 * x289; const FLT x313 = x52 * x312; const FLT x314 = (-1 * x313) + x292 + (-1 * x278 * x286) + (-1 * x283 * x284) + (x296 * x294) + (-1 * x295 * x130) + (x46 * x305) + (-1 * x298 * x280) + x304 + x62 + (-1 * x311 * x310) + (-1 * x309); const FLT x315 = 0.5 * x300; const FLT x316 = x61 * x312; const FLT x317 = x11 * x297; const FLT x318 = x317 * x130; const FLT x319 = x11 * x285; const FLT x320 = x303 * x319; const FLT x321 = x22 * x284; const FLT x322 = -1 * x55; const FLT x323 = x46 * x290; const FLT x324 = x11 * x310; const FLT x325 = (-1 * x306 * x324) + (x11 * x323); const FLT x326 = x325 + (x307 * x311) + (-1 * x294 * x321) + x322 + x316 + (-1 * x278 * x315) + (x280 * x318) + (-1 * x57 * x305) + (-1 * x320) + (-1 * x294 * x151) + (-1 * x283 * x126); const FLT x327 = x42 * x126; const FLT x328 = x284 * x317; const FLT x329 = x301 * x306; const FLT x330 = x22 * x130; const FLT x331 = x52 * x290; const FLT x332 = x11 * x331; const FLT x333 = 0.5 * x278; const FLT x334 = -1 * x49; const FLT x335 = (-1 * x57 * x312) + (x308 * x303); const FLT x336 = x334 + (-1 * x283 * x121) + (-1 * x332) + (x294 * x327) + (-1 * x310 * x333) + (x294 * x330) + x335 + (-1 * x61 * x305) + (x280 * x328) + (x285 * x311) + x329; const FLT x337 = (x314 * sensor_pt[1]) + (-1 * x326 * sensor_pt[0]) + (x336 * sensor_pt[2]); const FLT x338 = x317 * x126; const FLT x339 = x61 * x290; const FLT x340 = (x306 * x319) + (-1 * x11 * x339); const FLT x341 = (-1 * x303 * x324) + (x46 * x312); const FLT x342 = x340 + x58 + x341 + (-1 * x283 * x130) + (x295 * x284) + (-1 * x294 * x164) + (x52 * x305) + (-1 * x280 * x338) + (-1 * x300 * x311) + (-1 * x307 * x333); const FLT x343 = (-1 * x342 * sensor_pt[1]) + (x326 * sensor_pt[2]) + (x336 * sensor_pt[0]); const FLT x344 = (x343 * x173) + (x342 * x171) + (-1 * x314 * x141) + (-1 * x337 * x163); const FLT x345 = (x342 * sensor_pt[0]) + (-1 * x314 * sensor_pt[2]) + (x336 * sensor_pt[1]); const FLT x346 = (x337 * x177) + (-1 * x342 * x176) + (-1 * x345 * x173) + (x326 * x141); const FLT x347 = (-1 * x326 * x171) + (x345 * x163) + (-1 * x343 * x177) + (x314 * x176); const FLT x348 = (x347 * (*lh_p).Rot[2]) + (-1 * x344 * (*lh_p).Rot[3]) + (x346 * (*lh_p).Rot[0]); const FLT x349 = (-1 * x347 * (*lh_p).Rot[1]) + (x346 * (*lh_p).Rot[3]) + (x344 * (*lh_p).Rot[0]); const FLT x350 = x347 + (-1 * x348 * x181) + (x349 * x183); const FLT x351 = (x344 * (*lh_p).Rot[1]) + (-1 * x346 * (*lh_p).Rot[2]) + (x347 * (*lh_p).Rot[0]); const FLT x352 = x344 + (-1 * x351 * x183) + (x348 * x186); const FLT x353 = x346 + (-1 * x349 * x186) + (x351 * x181); const FLT x354 = (-1 * ((x97 * x350) + (-1 * x80 * x353)) * x98) + (-1 * x96 * ((x94 * x352) + (-1 * ((x91 * x350) + (x90 * x353)) * x93))); const FLT x355 = x8 * x144; const FLT x356 = x15 * x289; const FLT x357 = x14 * x271; const FLT x358 = x14 * x275; const FLT x359 = x8 * x268; const FLT x360 = (x14 * x14 * x14) * x267; const FLT x361 = x8 * x273; const FLT x362 = (-1 * x274 * x360) + (x14 * x361) + (x12 * x357) + (-1 * x42 * x359) + (-1 * x9 * x358) + (x270 * x360) + (-1 * x12 * x358) + (x9 * x357); const FLT x363 = x293 * x362; const FLT x364 = x42 * x363; const FLT x365 = x15 * x302; const FLT x366 = x282 * x362; const FLT x367 = x7 * x306; const FLT x368 = (x367 * x300) + (-1 * x7 * x331); const FLT x369 = x325 + (x296 * x363) + x55 + (-1 * x284 * x366) + x368 + (-1 * x285 * x355) + (x57 * x356) + (-1 * x364 * x130) + (-1 * x298 * x362) + (-1 * x365 * x307); const FLT x370 = (x7 * x323) + (-1 * x367 * x310); const FLT x371 = x370 + (-1 * x355 * x307) + (-1 * x329) + (-1 * x61 * x356) + (-1 * x366 * x130) + (-1 * x363 * x164) + x332 + (x284 * x364) + (-1 * x362 * x338) + x334 + (x285 * x365); const FLT x372 = -1 * x58; const FLT x373 = x7 * x291; const FLT x374 = x367 * x307; const FLT x375 = x340 + (x362 * x328) + (x363 * x327) + (-1 * x366 * x121) + (x365 * x300) + x372 + (-1 * x355 * x310) + (-1 * x373) + (-1 * x52 * x356) + (x363 * x330) + x374; const FLT x376 = x7 * x339; const FLT x377 = x285 * x367; const FLT x378 = x62 + (x46 * x356) + x376 + (x362 * x318) + x309 + (-1 * x355 * x300) + (-1 * x366 * x126) + (-1 * x363 * x151) + (-1 * x363 * x321) + (-1 * x365 * x310) + (-1 * x292) + (-1 * x377); const FLT x379 = (x378 * sensor_pt[2]) + (-1 * x371 * sensor_pt[1]) + (x375 * sensor_pt[0]); const FLT x380 = (x369 * sensor_pt[1]) + (-1 * x378 * sensor_pt[0]) + (x375 * sensor_pt[2]); const FLT x381 = (-1 * x380 * x163) + (-1 * x369 * x141) + (x379 * x173) + (x371 * x171); const FLT x382 = (x371 * sensor_pt[0]) + (-1 * x369 * sensor_pt[2]) + (x375 * sensor_pt[1]); const FLT x383 = (x378 * x141) + (x380 * x177) + (-1 * x382 * x173) + (-1 * x371 * x176); const FLT x384 = (-1 * x378 * x171) + (x382 * x163) + (-1 * x379 * x177) + (x369 * x176); const FLT x385 = 2 * ((x384 * (*lh_p).Rot[2]) + (-1 * x381 * (*lh_p).Rot[3]) + (x383 * (*lh_p).Rot[0])); const FLT x386 = 2 * ((x383 * (*lh_p).Rot[3]) + (x381 * (*lh_p).Rot[0]) + (-1 * x384 * (*lh_p).Rot[1])); const FLT x387 = x384 + (-1 * x385 * (*lh_p).Rot[2]) + (x386 * (*lh_p).Rot[1]); const FLT x388 = (-1 * x383 * (*lh_p).Rot[2]) + (x381 * (*lh_p).Rot[1]) + (x384 * (*lh_p).Rot[0]); const FLT x389 = x381 + (-1 * x388 * x183) + (x385 * (*lh_p).Rot[3]); const FLT x390 = x383 + (-1 * x386 * (*lh_p).Rot[3]) + (x388 * x181); const FLT x391 = (-1 * ((x97 * x387) + (-1 * x80 * x390)) * x98) + (-1 * x96 * ((x94 * x389) + (-1 * ((x91 * x387) + (x90 * x390)) * x93))); const FLT x392 = x7 * x12; const FLT x393 = x7 * x7 * x7; const FLT x394 = (-1 * x275 * x393) + (-1 * x22 * x359) + (x271 * x393) + (x7 * x361) + (x271 * x392) + (-1 * x7 * x279) + (x7 * x277) + (-1 * x275 * x392); const FLT x395 = x282 * x394; const FLT x396 = x293 * x394; const FLT x397 = x42 * x396; const FLT x398 = x8 * x22; const FLT x399 = x9 * x287; const FLT x400 = x269 * x399; const FLT x401 = x288 * x399; const FLT x402 = x341 + (-1 * x52 * x401) + (x400 * x300) + (-1 * x286 * x398) + (-1 * x374) + x372 + (-1 * x284 * x395) + (-1 * x298 * x394) + (-1 * x397 * x130) + (x296 * x396) + x373; const FLT x403 = x335 + (-1 * x396 * x321) + (-1 * x398 * x315) + (-1 * x400 * x285) + (-1 * x395 * x126) + (-1 * x396 * x151) + (x61 * x401) + x370 + x49 + (x394 * x318); const FLT x404 = 0.5 * x398; const FLT x405 = (x394 * x328) + x368 + x320 + (x396 * x327) + (x400 * x307) + (-1 * x404 * x310) + (-1 * x316) + x322 + (x396 * x330) + (-1 * x57 * x401) + (-1 * x395 * x121); const FLT x406 = (-1 * x403 * sensor_pt[0]) + (x402 * sensor_pt[1]) + (x405 * sensor_pt[2]); const FLT x407 = (-1 * x394 * x338) + (-1 * x396 * x164) + x62 + (x284 * x397) + (x46 * x401) + x313 + (-1 * x404 * x307) + x377 + (-1 * x304) + (-1 * x400 * x310) + (-1 * x395 * x130) + (-1 * x376); const FLT x408 = (x403 * sensor_pt[2]) + (-1 * x407 * sensor_pt[1]) + (x405 * sensor_pt[0]); const FLT x409 = (x408 * x173) + (x407 * x171) + (-1 * x402 * x141) + (-1 * x406 * x163); const FLT x410 = (-1 * x402 * sensor_pt[2]) + (x407 * sensor_pt[0]) + (x405 * sensor_pt[1]); const FLT x411 = (x406 * x177) + (x403 * x141) + (-1 * x410 * x173) + (-1 * x407 * x176); const FLT x412 = (x402 * x176) + (-1 * x408 * x177) + (-1 * x403 * x171) + (x410 * x163); const FLT x413 = (-1 * x409 * (*lh_p).Rot[3]) + (x412 * (*lh_p).Rot[2]) + (x411 * (*lh_p).Rot[0]); const FLT x414 = (x411 * (*lh_p).Rot[3]) + (-1 * x412 * (*lh_p).Rot[1]) + (x409 * (*lh_p).Rot[0]); const FLT x415 = (-1 * x413 * x181) + x412 + (x414 * x183); const FLT x416 = (x409 * (*lh_p).Rot[1]) + (-1 * x411 * (*lh_p).Rot[2]) + (x412 * (*lh_p).Rot[0]); const FLT x417 = x409 + (x413 * x186) + (-1 * x416 * x183); const FLT x418 = x411 + (-1 * x414 * x186) + (x416 * x181); const FLT x419 = (-1 * ((x97 * x415) + (-1 * x80 * x418)) * x98) + (-1 * x96 * ((x94 * x417) + (-1 * ((x91 * x415) + (x90 * x418)) * x93))); const FLT x420 = dt * x242; const FLT x421 = dt * x243; const FLT x422 = x421 + (-1 * x420); const FLT x423 = dt * x245; const FLT x424 = dt * x246; const FLT x425 = x424 + x423; const FLT x426 = -1 * dt * x250; const FLT x427 = (-1 * dt * x248) + dt; const FLT x428 = x427 + x426; const FLT x429 = (-1 * ((x97 * x422) + (-1 * x80 * x428)) * x98) + (-1 * x96 * ((x94 * x425) + (-1 * ((x91 * x422) + (x90 * x428)) * x93))); const FLT x430 = dt * x254; const FLT x431 = dt * x255; const FLT x432 = x431 + x430; const FLT x433 = -1 * dt * x257; const FLT x434 = x427 + x433; const FLT x435 = x423 + (-1 * x424); const FLT x436 = (-1 * ((x97 * x432) + (-1 * x80 * x435)) * x98) + (-1 * x96 * ((x94 * x434) + (-1 * ((x91 * x432) + (x90 * x435)) * x93))); const FLT x437 = x426 + dt + x433; const FLT x438 = x430 + (-1 * x431); const FLT x439 = x420 + x421; const FLT x440 = (-1 * ((x97 * x437) + (-1 * x80 * x439)) * x98) + (-1 * x96 * ((x94 * x438) + (-1 * ((x91 * x437) + (x90 * x439)) * x93))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x99 + (x83 * ((-1 * x80 * x79) + (x5 * x76))) + (x99 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x108 + (((-1 * x80 * x106) + (x76 * x103)) * x83) + (x100 * x108)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), x112 + (((-1 * x80 * x110) + (x76 * x109)) * x83) + (x100 * x112)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x189 + (((-1 * x80 * x187) + (x76 * x184)) * x83) + (x100 * x189)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x215 + (((-1 * x80 * x213) + (x76 * x211)) * x83) + (x215 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x241 + (((-1 * x80 * x239) + (x76 * x237)) * x83) + (x241 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), x253 + (((-1 * x80 * x247) + (x76 * x244)) * x83) + (x253 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), x261 + (((-1 * x80 * x259) + (x76 * x256)) * x83) + (x261 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), x265 + (((-1 * x80 * x263) + (x76 * x262)) * x83) + (x265 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x354 + (((-1 * x80 * x352) + (x76 * x350)) * x83) + (x354 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x391 + (((-1 * x80 * x389) + (x76 * x387)) * x83) + (x391 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x419 + (((-1 * x80 * x417) + (x76 * x415)) * x83) + (x419 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), (((-1 * x80 * x425) + (x76 * x422)) * x83) + x429 + (x429 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), x436 + (((-1 * x80 * x434) + (x76 * x432)) * x83) + (x436 * x100)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), x440 + (((-1 * x80 * x438) + (x76 * x437)) * x83) + (x440 * x100)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*error_model).Pose.AxisAngleRot[2]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x16 = (-1 * x15 * (*_x0).Pose.Rot[1]) + (x2 * (*error_model).Pose.AxisAngleRot[1]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x15 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x4 * x4) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x6 * x19) + (x8 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x7 * x26) + (x25 * x17) + (-1 * x0 * x23) + (-1 * x9 * x24); const FLT x28 = x22 * x17; const FLT x29 = x9 * x28; const FLT x30 = x25 * x16; const FLT x31 = x0 * x26; const FLT x32 = x7 * x23; const FLT x33 = (-1 * x32) + x31 + x29 + x30; const FLT x34 = 2 * x33; const FLT x35 = x34 * x27; const FLT x36 = x0 * x28; const FLT x37 = x9 * x26; const FLT x38 = x4 * x25; const FLT x39 = x7 * x24; const FLT x40 = x38 + x39 + x36 + (-1 * x37); const FLT x41 = x9 * x23; const FLT x42 = x25 * x18; const FLT x43 = x0 * x24; const FLT x44 = x7 * x28; const FLT x45 = x44 + (-1 * x43) + x41 + x42; const FLT x46 = 2 * x45; const FLT x47 = (x40 * x46) + (-1 * x35); const FLT x48 = x32 + (-1 * x30) + (-1 * x31) + (-1 * x29); const FLT x49 = 2 * x40; const FLT x50 = x49 * x27; const FLT x51 = x50 + (-1 * x46 * x48); const FLT x52 = 1 + (x48 * x34) + (-2 * (x40 * x40)); const FLT x53 = (x52 * (*lh_p).Rot[3]) + (-1 * x47 * (*lh_p).Rot[1]) + (x51 * (*lh_p).Rot[0]); const FLT x54 = 2 * (*lh_p).Rot[1]; const FLT x55 = 2 * ((x47 * (*lh_p).Rot[2]) + (-1 * x51 * (*lh_p).Rot[3]) + (x52 * (*lh_p).Rot[0])); const FLT x56 = x47 + (x54 * x53) + (-1 * x55 * (*lh_p).Rot[2]); const FLT x57 = 1.0/2.0 * dt * fabs(dt); const FLT x58 = (x27 * sensor_pt[0]) + (-1 * x40 * sensor_pt[1]) + (x33 * sensor_pt[2]); const FLT x59 = (-1 * x45 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x40 * sensor_pt[0]); const FLT x60 = (*_x0).Pose.Pos[2] + (2 * ((x59 * x45) + (-1 * x58 * x33))) + sensor_pt[2] + (*error_model).Pose.Pos[2] + (x57 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x61 = (-1 * x33 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x45 * sensor_pt[1]); const FLT x62 = (*_x0).Pose.Pos[1] + (2 * ((x58 * x40) + (-1 * x61 * x45))) + (*error_model).Pose.Pos[1] + (x57 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])); const FLT x63 = (x57 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + (2 * ((x61 * x33) + (-1 * x59 * x40))) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x64 = (-1 * x63 * (*lh_p).Rot[2]) + (x60 * (*lh_p).Rot[0]) + (x62 * (*lh_p).Rot[1]); const FLT x65 = (-1 * x62 * (*lh_p).Rot[3]) + (x63 * (*lh_p).Rot[0]) + (x60 * (*lh_p).Rot[2]); const FLT x66 = x62 + (*lh_p).Pos[1] + (2 * ((x65 * (*lh_p).Rot[3]) + (-1 * x64 * (*lh_p).Rot[1]))); const FLT x67 = (-1 * x60 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]) + (x63 * (*lh_p).Rot[3]); const FLT x68 = (2 * ((x67 * (*lh_p).Rot[1]) + (-1 * x65 * (*lh_p).Rot[2]))) + x60 + (*lh_p).Pos[2]; const FLT x69 = x68 * x68; const FLT x70 = 1. / x69; const FLT x71 = x70 * x66; const FLT x72 = 1. / x68; const FLT x73 = (x51 * (*lh_p).Rot[1]) + (-1 * x52 * (*lh_p).Rot[2]) + (x47 * (*lh_p).Rot[0]); const FLT x74 = x51 + (x55 * (*lh_p).Rot[3]) + (-1 * x73 * x54); const FLT x75 = x66 * x66; const FLT x76 = -1 * x68; const FLT x77 = 2 * (1. / (x69 + x75)) * x69 * atan2(x66, x76) * (*bsc0).curve; const FLT x78 = 2 * (*lh_p).Rot[2]; const FLT x79 = 2 * (*lh_p).Rot[3]; const FLT x80 = x52 + (x73 * x78) + (-1 * x79 * x53); const FLT x81 = (*lh_p).Pos[0] + x63 + (2 * ((x64 * (*lh_p).Rot[2]) + (-1 * x67 * (*lh_p).Rot[3]))); const FLT x82 = 2 * x81; const FLT x83 = 2 * x68; const FLT x84 = (x81 * x81) + x69; const FLT x85 = 1.0/2.0 * (1. / (x84 * sqrt(x84))) * x66 * (*bsc0).tilt; const FLT x86 = (1. / sqrt(x84)) * (*bsc0).tilt; const FLT x87 = 1. / x84; const FLT x88 = 1. / sqrt(1 + (-1 * x87 * x75 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x89 = x81 * x70; const FLT x90 = x87 * x69; const FLT x91 = (-1 * ((x89 * x56) + (-1 * x80 * x72)) * x90) + (-1 * x88 * ((x86 * x74) + (-1 * ((x83 * x56) + (x80 * x82)) * x85))); const FLT x92 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * asin(x86 * x66)) + (-1 * atan2(x81, x76)) + (-1 * (*bsc0).phase) + (*bsc0).gibpha); const FLT x93 = (-1 * x36) + (-1 * x39) + x37 + (-1 * x38); const FLT x94 = x46 * x27; const FLT x95 = x94 + (-1 * x93 * x34); const FLT x96 = 1 + (x93 * x49) + (-2 * (x45 * x45)); const FLT x97 = (x46 * x33) + (-1 * x50); const FLT x98 = 2 * ((x97 * (*lh_p).Rot[3]) + (-1 * x95 * (*lh_p).Rot[1]) + (x96 * (*lh_p).Rot[0])); const FLT x99 = (x95 * (*lh_p).Rot[2]) + (-1 * x96 * (*lh_p).Rot[3]) + (x97 * (*lh_p).Rot[0]); const FLT x100 = x95 + (x98 * (*lh_p).Rot[1]) + (-1 * x78 * x99); const FLT x101 = (x96 * (*lh_p).Rot[1]) + (x95 * (*lh_p).Rot[0]) + (-1 * x97 * (*lh_p).Rot[2]); const FLT x102 = (x79 * x99) + x96 + (-1 * x54 * x101); const FLT x103 = (x78 * x101) + x97 + (-1 * x98 * (*lh_p).Rot[3]); const FLT x104 = (-1 * ((x89 * x100) + (-1 * x72 * x103)) * x90) + (-1 * x88 * ((x86 * x102) + (-1 * ((x83 * x100) + (x82 * x103)) * x85))); const FLT x105 = x43 + (-1 * x44) + (-1 * x42) + (-1 * x41); const FLT x106 = 1 + (x46 * x105) + (-2 * (x33 * x33)); const FLT x107 = (x40 * x34) + (-1 * x94); const FLT x108 = x35 + (-1 * x49 * x105); const FLT x109 = 2 * ((x108 * (*lh_p).Rot[3]) + (-1 * x106 * (*lh_p).Rot[1]) + (x107 * (*lh_p).Rot[0])); const FLT x110 = (x106 * (*lh_p).Rot[2]) + (-1 * x107 * (*lh_p).Rot[3]) + (x108 * (*lh_p).Rot[0]); const FLT x111 = x106 + (x109 * (*lh_p).Rot[1]) + (-1 * x78 * x110); const FLT x112 = (-1 * x108 * (*lh_p).Rot[2]) + (x107 * (*lh_p).Rot[1]) + (x106 * (*lh_p).Rot[0]); const FLT x113 = (x79 * x110) + x107 + (-1 * x54 * x112); const FLT x114 = x108 + (x78 * x112) + (-1 * x109 * (*lh_p).Rot[3]); const FLT x115 = (-1 * ((x89 * x111) + (-1 * x72 * x114)) * x90) + (-1 * x88 * ((x86 * x113) + (-1 * ((x83 * x111) + (x82 * x114)) * x85))); cnMatrixOptionalSet(Hx, 0, 0, x91 + (((-1 * x72 * x74) + (x71 * x56)) * x77) + (x92 * x91)); cnMatrixOptionalSet(Hx, 0, 1, x104 + (((-1 * x72 * x102) + (x71 * x100)) * x77) + (x92 * x104)); cnMatrixOptionalSet(Hx, 0, 2, (((-1 * x72 * x113) + (x71 * x111)) * x77) + x115 + (x92 * x115)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x0 * x0) * 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 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x7 * x18) + (x9 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x20 * x19; const FLT x24 = x21 * x15; const FLT x25 = x21 * x16; const FLT x26 = (x8 * x25) + (-1 * x6 * x24) + (x0 * x22) + (x23 * x17); const FLT x27 = x21 * x17; const FLT x28 = (-1 * x8 * x27) + (x23 * x16) + (-1 * x6 * x22) + (-1 * x0 * x24); const FLT x29 = (x6 * x27) + (x0 * x25) + (-1 * x8 * x22) + (x23 * x15); const FLT x30 = (-1 * x29 * sensor_pt[0]) + (x28 * sensor_pt[2]) + (x26 * sensor_pt[1]); const FLT x31 = (x4 * x23) + (x6 * x25) + (x8 * x24) + (-1 * x0 * x27); const FLT x32 = (-1 * x31 * sensor_pt[1]) + (x28 * sensor_pt[0]) + (x29 * sensor_pt[2]); const FLT x33 = 2 * ((x32 * x31) + (-1 * x30 * x26)); const FLT x34 = dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1]); const FLT x35 = 1.0/2.0 * dt * fabs(dt); const FLT x36 = x35 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x37 = (*_x0).Pose.Pos[1] + x33 + (*error_model).Pose.Pos[1] + sensor_pt[1] + x34 + x36; const FLT x38 = x37 * (*lh_p).Rot[0]; const FLT x39 = (-1 * x26 * sensor_pt[2]) + (x28 * sensor_pt[1]) + (x31 * sensor_pt[0]); const FLT x40 = 2 * ((x30 * x29) + (-1 * x31 * x39)); const FLT x41 = dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0]); const FLT x42 = x35 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x43 = x42 + x40 + (*error_model).Pose.Pos[0] + x41 + (*_x0).Pose.Pos[0] + sensor_pt[0]; const FLT x44 = x43 * (*lh_p).Rot[3]; const FLT x45 = x35 * ((*_x0).Acc[2] + (*error_model).Acc[2]); const FLT x46 = 2 * ((x39 * x26) + (-1 * x32 * x29)); const FLT x47 = dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2]); const FLT x48 = (*error_model).Pose.Pos[2] + x46 + x45 + sensor_pt[2] + (*_x0).Pose.Pos[2] + x47; const FLT x49 = x48 * (*lh_p).Rot[1]; const FLT x50 = (-1 * x49) + x38 + x44; const FLT x51 = x48 * (*lh_p).Rot[0]; const FLT x52 = x37 * (*lh_p).Rot[1]; const FLT x53 = x43 * (*lh_p).Rot[2]; const FLT x54 = (-1 * x53) + x51 + x52; const FLT x55 = x43 + (*lh_p).Pos[0] + (2 * ((x54 * (*lh_p).Rot[2]) + (-1 * x50 * (*lh_p).Rot[3]))); const FLT x56 = x43 * (*lh_p).Rot[0]; const FLT x57 = x48 * (*lh_p).Rot[2]; const FLT x58 = x37 * (*lh_p).Rot[3]; const FLT x59 = (-1 * x58) + x56 + x57; const FLT x60 = x37 + (*lh_p).Pos[1] + (2 * ((x59 * (*lh_p).Rot[3]) + (-1 * x54 * (*lh_p).Rot[1]))); const FLT x61 = x60 * x60; const FLT x62 = x48 + (2 * ((x50 * (*lh_p).Rot[1]) + (-1 * x59 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x63 = x62 * x62; const FLT x64 = (x55 * x55) + x63; const FLT x65 = 1. / x64; const FLT x66 = 1. / sqrt(1 + (-1 * x61 * x65 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x67 = x60 * (1. / (x64 * sqrt(x64))) * (*bsc0).tilt; const FLT x68 = x67 * x66; const FLT x69 = (x62 * x65) + (x68 * x55); const FLT x70 = -1 * x62; const FLT x71 = (1. / sqrt(x64)) * (*bsc0).tilt; const FLT x72 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * asin(x71 * x60)) + (-1 * atan2(x55, x70)) + (-1 * (*bsc0).phase) + (*bsc0).gibpha); const FLT x73 = x71 * x66; const FLT x74 = 2 * x62; const FLT x75 = (1. / (x63 + x61)) * atan2(x60, x70) * (*bsc0).curve; const FLT x76 = 2 * x75; const FLT x77 = (-1 * x65 * x55) + (x62 * x68); const FLT x78 = 2 * x53; const FLT x79 = (2 * x52) + (-1 * x78); const FLT x80 = 1. / x63; const FLT x81 = x80 * x60; const FLT x82 = 2 * x49; const FLT x83 = (2 * x44) + (-1 * x82); const FLT x84 = 1. / x62; const FLT x85 = x76 * x63; const FLT x86 = 2 * x58; const FLT x87 = (2 * x57) + (-1 * x86); const FLT x88 = 2 * x55; const FLT x89 = 1.0/2.0 * x67; const FLT x90 = x80 * x55; const FLT x91 = x63 * x65; const FLT x92 = (-1 * ((x79 * x90) + (-1 * x84 * x87)) * x91) + (-1 * x66 * ((x83 * x71) + (-1 * ((x79 * x74) + (x88 * x87)) * x89))); const FLT x93 = 2 * ((-1 * x46) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x47) + (-1 * sensor_pt[2]) + (-1 * x45) + (-1 * (*error_model).Pose.Pos[2])); const FLT x94 = 2 * x38; const FLT x95 = x83 + (x93 * (*lh_p).Rot[1]) + x94; const FLT x96 = x80 * x95; const FLT x97 = 2 * x51; const FLT x98 = (-1 * x97) + x78 + (-4 * x52); const FLT x99 = 2 * (*lh_p).Rot[2]; const FLT x100 = (x99 * x37) + (-1 * x93 * (*lh_p).Rot[3]); const FLT x101 = (-1 * x91 * ((x55 * x96) + (-1 * x84 * x100))) + (-1 * x66 * ((x71 * x98) + (-1 * x89 * ((x74 * x95) + (x88 * x100))))); const FLT x102 = 2 * x56; const FLT x103 = x86 + (-1 * x102) + (-4 * x57); const FLT x104 = (-1 * sensor_pt[0]) + (-1 * (*_x0).Pose.Pos[0]) + (-1 * x42) + (-1 * x41) + (-1 * (*error_model).Pose.Pos[0]) + (-1 * x40); const FLT x105 = 2 * (*lh_p).Rot[1]; const FLT x106 = 2 * (*lh_p).Rot[3]; const FLT x107 = (x48 * x106) + (-1 * x105 * x104); const FLT x108 = x79 + (x99 * x104) + x97; const FLT x109 = (-1 * ((x90 * x103) + (-1 * x84 * x108)) * x91) + (-1 * x66 * ((x71 * x107) + (-1 * ((x74 * x103) + (x88 * x108)) * x89))); const FLT x110 = (-1 * x34) + (-1 * x33) + (-1 * x36) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * (*error_model).Pose.Pos[1]) + (-1 * sensor_pt[1]); const FLT x111 = (x43 * x105) + (-1 * x99 * x110); const FLT x112 = x87 + x102 + (x106 * x110); const FLT x113 = (-1 * x94) + x82 + (-4 * x44); const FLT x114 = (-1 * ((x90 * x111) + (-1 * x84 * x113)) * x91) + (-1 * x66 * ((x71 * x112) + (-1 * ((x74 * x111) + (x88 * x113)) * x89))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), x69 + (x72 * x69)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x73 * x72) + (-1 * x73) + (-1 * x75 * x74)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x77 + (x76 * x60) + (x72 * x77)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x92 + (((-1 * x83 * x84) + (x81 * x79)) * x85) + (x72 * x92)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x101 + (((-1 * x84 * x98) + (x60 * x96)) * x85) + (x72 * x101)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), (((-1 * x84 * x107) + (x81 * x103)) * x85) + x109 + (x72 * x109)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), x114 + (((-1 * x84 * x112) + (x81 * x111)) * x85) + (x72 * x114)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen1_jac_lh_p(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x5 = (*_x0).Pose.Rot[3] + (x4 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (x3 * (*_x0).Pose.Rot[0]); const FLT x6 = dt * dt; const FLT x7 = (x1 * x1) * x6; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x6 * (x8 * x8); const FLT x10 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x11 = x6 * (x10 * x10); const FLT x12 = 1e-10 + x11 + x7 + x9; const FLT x13 = sqrt(x12); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = (-1 * x3 * (*_x0).Pose.Rot[1]) + (x4 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (-1 * x4 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x3 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x4 * (*_x0).Pose.Rot[3]) + (x3 * (*_x0).Pose.Rot[2]); const FLT x19 = (x15 * x15) * (1. / x12); const FLT x20 = cos(x14); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x5 * x5) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x11 * x19))); const FLT x22 = dt * x21 * x15 * (1. / x13); const FLT x23 = x5 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x8 * x26) + (x25 * x17) + (-1 * x1 * x23) + (-1 * x24 * x10); const FLT x28 = x22 * x17; const FLT x29 = (-1 * x8 * x23) + (x1 * x26) + (x28 * x10) + (x25 * x16); const FLT x30 = (x5 * x25) + (x8 * x24) + (x1 * x28) + (-1 * x26 * x10); const FLT x31 = (-1 * x30 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x29 * sensor_pt[2]); const FLT x32 = (x8 * x28) + (-1 * x1 * x24) + (x23 * x10) + (x25 * x18); const FLT x33 = (-1 * x32 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x30 * sensor_pt[0]); const FLT x34 = (*_x0).Pose.Pos[2] + (x0 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + (2 * ((x32 * x33) + (-1 * x31 * x29))) + sensor_pt[2]; const FLT x35 = (-1 * x29 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x32 * sensor_pt[1]); const FLT x36 = (*_x0).Pose.Pos[1] + (2 * ((x30 * x31) + (-1 * x32 * x35))) + (x0 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + (*error_model).Pose.Pos[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + sensor_pt[1]; const FLT x37 = (x0 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (2 * ((x35 * x29) + (-1 * x30 * x33))) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x38 = (-1 * x37 * (*lh_p).Rot[2]) + (x34 * (*lh_p).Rot[0]) + (x36 * (*lh_p).Rot[1]); const FLT x39 = (-1 * x36 * (*lh_p).Rot[3]) + (x37 * (*lh_p).Rot[0]) + (x34 * (*lh_p).Rot[2]); const FLT x40 = x36 + (*lh_p).Pos[1] + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x38 * (*lh_p).Rot[1]))); const FLT x41 = (-1 * x34 * (*lh_p).Rot[1]) + (x36 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[3]); const FLT x42 = x34 + (2 * ((x41 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x43 = -1 * x42; const FLT x44 = x37 + (*lh_p).Pos[0] + (2 * ((x38 * (*lh_p).Rot[2]) + (-1 * x41 * (*lh_p).Rot[3]))); const FLT x45 = (x44 * x44) + (x42 * x42); const FLT x46 = x40 * (1. / sqrt(x45)); const FLT x47 = 1.5707963267949 + (-1 * atan2(x44, x43)) + (-1 * (*bsc0).phase) + (-1 * asin(x46 * (*bsc0).tilt)) + (*bsc0).gibpha; const FLT x48 = sin(x47) * (*bsc0).gibmag; const FLT x49 = x46 * (1. / sqrt(1 + (-1 * (x40 * x40) * (1. / x45) * ((*bsc0).tilt * (*bsc0).tilt)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), atan2(x40, x43) * atan2(x40, x43)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * cos(x47)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x48); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1 + (-1 * x48)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x48 * x49) + (-1 * x49)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_x_gen1_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen1_jac_bsc0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanErrorModel_LightMeas_y_gen1(const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x8 * x18) + (x6 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x21 * x15; const FLT x24 = x20 * x19; const FLT x25 = x21 * x17; const FLT x26 = (-1 * x7 * x25) + (-1 * x0 * x22) + (x24 * x16) + (-1 * x9 * x23); const FLT x27 = x21 * x16; const FLT x28 = (x7 * x23) + (x4 * x24) + (x0 * x27) + (-1 * x9 * x25); const FLT x29 = (x7 * x27) + (-1 * x0 * x23) + (x9 * x22) + (x24 * x17); const FLT x30 = (x26 * sensor_pt[1]) + (-1 * x29 * sensor_pt[2]) + (x28 * sensor_pt[0]); const FLT x31 = (x9 * x27) + (-1 * x7 * x22) + (x0 * x25) + (x24 * x15); const FLT x32 = (-1 * x31 * sensor_pt[0]) + (x26 * sensor_pt[2]) + (x29 * sensor_pt[1]); const FLT x33 = 1.0/2.0 * dt * fabs(dt); const FLT x34 = (x33 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x32 * x31) + (-1 * x30 * x28))) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0]; const FLT x35 = (-1 * x28 * sensor_pt[1]) + (x26 * sensor_pt[0]) + (x31 * sensor_pt[2]); const FLT x36 = (*_x0).Pose.Pos[2] + (*error_model).Pose.Pos[2] + (2 * ((x30 * x29) + (-1 * x31 * x35))) + (x33 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + sensor_pt[2]; const FLT x37 = (*_x0).Pose.Pos[1] + (2 * ((x35 * x28) + (-1 * x32 * x29))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x33 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x38 = (-1 * x37 * (*lh_p).Rot[3]) + (x34 * (*lh_p).Rot[0]) + (x36 * (*lh_p).Rot[2]); const FLT x39 = (-1 * x36 * (*lh_p).Rot[1]) + (x37 * (*lh_p).Rot[0]) + (x34 * (*lh_p).Rot[3]); const FLT x40 = x36 + (2 * ((x39 * (*lh_p).Rot[1]) + (-1 * x38 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x41 = (-1 * x34 * (*lh_p).Rot[2]) + (x36 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[1]); const FLT x42 = x37 + (*lh_p).Pos[1] + (2 * ((x38 * (*lh_p).Rot[3]) + (-1 * x41 * (*lh_p).Rot[1]))); const FLT x43 = x34 + (*lh_p).Pos[0] + (2 * ((x41 * (*lh_p).Rot[2]) + (-1 * x39 * (*lh_p).Rot[3]))); const FLT x44 = -1 * x40; const FLT x45 = (-1 * (*bsc0).phase) + (-1 * asin((1. / sqrt((x42 * x42) + (x40 * x40))) * x43 * (*bsc0).tilt)) + (-1 * atan2(-1 * x42, x44)); return x45 + (-1 * cos(1.5707963267949 + x45 + (*bsc0).gibpha) * (*bsc0).gibmag) + ((atan2(x43, x44) * atan2(x43, x44)) * (*bsc0).curve); } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x3 = (x2 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x0 * (*_x0).Pose.Rot[2]) + (x1 * (*_x0).Pose.Rot[0]); const FLT x4 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x5 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (x0 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[2]; const FLT x6 = (-1 * x0 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]); const FLT x7 = (*_x0).Pose.Rot[1] + (x0 * (*_x0).Pose.Rot[0]) + (-1 * x2 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5); const FLT x9 = 1. / sqrt(x8); const FLT x10 = dt * dt; const FLT x11 = x4 * x4; const FLT x12 = x11 * x10; const FLT x13 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x14 = x13 * x13; const FLT x15 = x14 * x10; const FLT x16 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x17 = x16 * x16; const FLT x18 = x10 * x17; const FLT x19 = 1e-10 + x18 + x12 + x15; const FLT x20 = sqrt(x19); const FLT x21 = 0.5 * x20; const FLT x22 = sin(x21); const FLT x23 = x22 * x22; const FLT x24 = 1. / x19; const FLT x25 = x24 * x23; const FLT x26 = cos(x21); const FLT x27 = (x25 * x15) + (x26 * x26) + (x25 * x12) + (x25 * x18); const FLT x28 = 1. / sqrt(x27); const FLT x29 = x9 * x28; const FLT x30 = x22 * (1. / x20); const FLT x31 = dt * x30; const FLT x32 = x31 * x29; const FLT x33 = x4 * x32; const FLT x34 = x32 * x16; const FLT x35 = x28 * x26; const FLT x36 = x9 * x35; const FLT x37 = x6 * x36; const FLT x38 = x29 * x13; const FLT x39 = x31 * x38; const FLT x40 = x37 + (-1 * x7 * x39) + (-1 * x3 * x33) + (-1 * x5 * x34); const FLT x41 = x3 * x36; const FLT x42 = x41 + (x6 * x33) + (x5 * x39) + (-1 * x7 * x34); const FLT x43 = x7 * x36; const FLT x44 = (x6 * x39) + (-1 * x5 * x33) + (x3 * x34) + x43; const FLT x45 = (-1 * x44 * sensor_pt[2]) + (x40 * sensor_pt[1]) + (x42 * sensor_pt[0]); const FLT x46 = x5 * x36; const FLT x47 = (-1 * x3 * x39) + (x6 * x34) + (x7 * x33) + x46; const FLT x48 = (-1 * x47 * sensor_pt[0]) + (x40 * sensor_pt[2]) + (x44 * sensor_pt[1]); const FLT x49 = dt * fabs(dt); const FLT x50 = 1.0/2.0 * x49; const FLT x51 = (2 * ((x47 * x48) + (-1 * x42 * x45))) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (x50 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x52 = (-1 * x42 * sensor_pt[1]) + (x40 * sensor_pt[0]) + (x47 * sensor_pt[2]); const FLT x53 = (*_x0).Pose.Pos[2] + (*error_model).Pose.Pos[2] + (x50 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (2 * ((x44 * x45) + (-1 * x52 * x47))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x54 = (*_x0).Pose.Pos[1] + (2 * ((x52 * x42) + (-1 * x44 * x48))) + (*error_model).Pose.Pos[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + sensor_pt[1] + (x50 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x55 = (-1 * x54 * (*lh_p).Rot[3]) + (x51 * (*lh_p).Rot[0]) + (x53 * (*lh_p).Rot[2]); const FLT x56 = (-1 * x53 * (*lh_p).Rot[1]) + (x54 * (*lh_p).Rot[0]) + (x51 * (*lh_p).Rot[3]); const FLT x57 = x53 + (2 * ((x56 * (*lh_p).Rot[1]) + (-1 * x55 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x58 = 1. / x57; const FLT x59 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x60 = -1 * x59 * x49; const FLT x61 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x62 = (-1 * x61 * x49) + x50; const FLT x63 = x62 + x60; const FLT x64 = x49 * (*lh_p).Rot[2]; const FLT x65 = x64 * (*lh_p).Rot[0]; const FLT x66 = x49 * (*lh_p).Rot[1]; const FLT x67 = x66 * (*lh_p).Rot[3]; const FLT x68 = x67 + (-1 * x65); const FLT x69 = x57 * x57; const FLT x70 = 1. / x69; const FLT x71 = (-1 * x51 * (*lh_p).Rot[2]) + (x53 * (*lh_p).Rot[0]) + (x54 * (*lh_p).Rot[1]); const FLT x72 = x51 + (*lh_p).Pos[0] + (2 * ((x71 * (*lh_p).Rot[2]) + (-1 * x56 * (*lh_p).Rot[3]))); const FLT x73 = x70 * x72; const FLT x74 = -1 * x57; const FLT x75 = x72 * x72; const FLT x76 = 2 * (1. / (x69 + x75)) * x69 * atan2(x72, x74) * (*bsc0).curve; const FLT x77 = x54 + (*lh_p).Pos[1] + (2 * ((x55 * (*lh_p).Rot[3]) + (-1 * x71 * (*lh_p).Rot[1]))); const FLT x78 = x70 * x77; const FLT x79 = x66 * (*lh_p).Rot[2]; const FLT x80 = x49 * (*lh_p).Rot[3] * (*lh_p).Rot[0]; const FLT x81 = x80 + x79; const FLT x82 = x69 + (x77 * x77); const FLT x83 = 1. / x82; const FLT x84 = x83 * x69; const FLT x85 = (1. / sqrt(x82)) * (*bsc0).tilt; const FLT x86 = 2 * x77; const FLT x87 = 2 * x57; const FLT x88 = 1.0/2.0 * (1. / (x82 * sqrt(x82))) * x72 * (*bsc0).tilt; const FLT x89 = 1. / sqrt(1 + (-1 * x83 * x75 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x90 = (-1 * x89 * ((-1 * ((x87 * x68) + (x81 * x86)) * x88) + (x85 * x63))) + (-1 * ((x81 * x58) + (-1 * x78 * x68)) * x84); const FLT x91 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x85 * x72)) + (-1 * atan2(-1 * x77, x74))); const FLT x92 = x79 + (-1 * x80); const FLT x93 = x64 * (*lh_p).Rot[3]; const FLT x94 = x66 * (*lh_p).Rot[0]; const FLT x95 = x94 + x93; const FLT x96 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x97 = -1 * x96 * x49; const FLT x98 = x97 + x60 + x50; const FLT x99 = (-1 * x89 * ((-1 * ((x87 * x95) + (x86 * x98)) * x88) + (x85 * x92))) + (-1 * ((x58 * x98) + (-1 * x78 * x95)) * x84); const FLT x100 = x65 + x67; const FLT x101 = x62 + x97; const FLT x102 = x93 + (-1 * x94); const FLT x103 = (-1 * x89 * ((-1 * ((x87 * x101) + (x86 * x102)) * x88) + (x85 * x100))) + (-1 * ((x58 * x102) + (-1 * x78 * x101)) * x84); const FLT x104 = 2 * x59; const FLT x105 = -1 * x104; const FLT x106 = 2 * x61; const FLT x107 = -1 * x106; const FLT x108 = 1 + x107 + x105; const FLT x109 = 2 * (*lh_p).Rot[2]; const FLT x110 = x109 * (*lh_p).Rot[0]; const FLT x111 = 2 * (*lh_p).Rot[1]; const FLT x112 = x111 * (*lh_p).Rot[3]; const FLT x113 = x112 + (-1 * x110); const FLT x114 = x111 * (*lh_p).Rot[2]; const FLT x115 = 2 * (*lh_p).Rot[3]; const FLT x116 = x115 * (*lh_p).Rot[0]; const FLT x117 = x116 + x114; const FLT x118 = (-1 * x89 * ((-1 * ((x87 * x113) + (x86 * x117)) * x88) + (x85 * x108))) + (-1 * ((x58 * x117) + (-1 * x78 * x113)) * x84); const FLT x119 = x114 + (-1 * x116); const FLT x120 = x109 * (*lh_p).Rot[3]; const FLT x121 = x111 * (*lh_p).Rot[0]; const FLT x122 = x121 + x120; const FLT x123 = 2 * x96; const FLT x124 = 1 + (-1 * x123); const FLT x125 = x124 + x105; const FLT x126 = (-1 * x89 * ((-1 * ((x87 * x122) + (x86 * x125)) * x88) + (x85 * x119))) + (-1 * ((x58 * x125) + (-1 * x78 * x122)) * x84); const FLT x127 = x110 + x112; const FLT x128 = x124 + x107; const FLT x129 = x120 + (-1 * x121); const FLT x130 = (-1 * x89 * ((-1 * ((x87 * x128) + (x86 * x129)) * x88) + (x85 * x127))) + (-1 * ((x58 * x129) + (-1 * x78 * x128)) * x84); const FLT x131 = x1 * x34; const FLT x132 = 1.0/2.0 * x5; const FLT x133 = x4 * x31; const FLT x134 = x133 * x132; const FLT x135 = 1.0 * (*error_model).Pose.AxisAngleRot[2]; const FLT x136 = 1.0 * x7; const FLT x137 = 1.0 * x5; const FLT x138 = (x136 * (*error_model).Pose.AxisAngleRot[0]) + (2 * x6) + (x137 * (*error_model).Pose.AxisAngleRot[1]) + (x3 * x135); const FLT x139 = 1. / (x8 * sqrt(x8)); const FLT x140 = x28 * x139; const FLT x141 = x138 * x140; const FLT x142 = 1.0/2.0 * x3; const FLT x143 = x31 * x16; const FLT x144 = x140 * x143; const FLT x145 = x142 * x144; const FLT x146 = x31 * x13; const FLT x147 = x140 * x146; const FLT x148 = 1.0/2.0 * x6; const FLT x149 = x138 * x148; const FLT x150 = 1.0/2.0 * x7; const FLT x151 = x35 * x139; const FLT x152 = x138 * x151; const FLT x153 = -1 * x2 * x33; const FLT x154 = x0 * x36; const FLT x155 = x154 + x153; const FLT x156 = x155 + (-1 * x150 * x152) + x131 + x39 + (x134 * x141) + (-1 * x138 * x145) + (-1 * x147 * x149); const FLT x157 = 2 * x48; const FLT x158 = x0 * x33; const FLT x159 = x2 * x36; const FLT x160 = x138 * x150; const FLT x161 = x160 * x140; const FLT x162 = -1 * x1 * x39; const FLT x163 = x34 + x162; const FLT x164 = x163 + x159 + (-1 * x161 * x133) + (-1 * x132 * x152) + (x138 * x142 * x147) + x158 + (-1 * x144 * x149); const FLT x165 = x133 * x142; const FLT x166 = x0 * x39; const FLT x167 = -1 * x166; const FLT x168 = x132 * x144; const FLT x169 = x1 * x33; const FLT x170 = -1 * x169; const FLT x171 = x2 * x34; const FLT x172 = x36 + (-1 * x171); const FLT x173 = x172 + (-1 * x148 * x152) + (x161 * x146) + (x165 * x141) + x170 + x167 + (x168 * x138); const FLT x174 = (-1 * x164 * sensor_pt[0]) + (x156 * sensor_pt[1]) + (x173 * sensor_pt[2]); const FLT x175 = 2 * x44; const FLT x176 = x132 * x146; const FLT x177 = x1 * x36; const FLT x178 = x2 * x39; const FLT x179 = -1 * x0 * x34; const FLT x180 = x179 + x178; const FLT x181 = (x160 * x144) + (-1 * x133 * x140 * x149) + (-1 * x142 * x152) + (-1 * x176 * x141) + x180 + x33 + x177; const FLT x182 = 2 * x52; const FLT x183 = (x164 * sensor_pt[2]) + (x173 * sensor_pt[0]) + (-1 * x181 * sensor_pt[1]); const FLT x184 = 2 * x42; const FLT x185 = (x183 * x184) + (x181 * x182) + (-1 * x156 * x157) + (-1 * x174 * x175); const FLT x186 = 2 * x47; const FLT x187 = (x181 * sensor_pt[0]) + (-1 * x156 * sensor_pt[2]) + (x173 * sensor_pt[1]); const FLT x188 = 2 * x45; const FLT x189 = (x175 * x187) + (x188 * x156) + (-1 * x186 * x183) + (-1 * x164 * x182); const FLT x190 = (x164 * x157) + (x174 * x186) + (-1 * x187 * x184) + (-1 * x181 * x188); const FLT x191 = (x190 * (*lh_p).Rot[3]) + (x185 * (*lh_p).Rot[0]) + (-1 * x189 * (*lh_p).Rot[1]); const FLT x192 = (-1 * x190 * (*lh_p).Rot[2]) + (x185 * (*lh_p).Rot[1]) + (x189 * (*lh_p).Rot[0]); const FLT x193 = x190 + (-1 * x115 * x191) + (x109 * x192); const FLT x194 = (-1 * x185 * (*lh_p).Rot[3]) + (x189 * (*lh_p).Rot[2]) + (x190 * (*lh_p).Rot[0]); const FLT x195 = x189 + (-1 * x109 * x194) + (x111 * x191); const FLT x196 = x185 + (-1 * x111 * x192) + (x115 * x194); const FLT x197 = (-1 * x89 * ((-1 * ((x87 * x195) + (x86 * x196)) * x88) + (x85 * x193))) + (-1 * ((x58 * x196) + (-1 * x78 * x195)) * x84); const FLT x198 = 1.0 * x6; const FLT x199 = 1.0 * x3; const FLT x200 = (-1 * x5 * x135) + (x199 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x7) + (-1 * x198 * (*error_model).Pose.AxisAngleRot[0]); const FLT x201 = x200 * x140; const FLT x202 = x201 * x148; const FLT x203 = x200 * x151; const FLT x204 = x171 + x36; const FLT x205 = x204 + (-1 * x203 * x150) + (x201 * x134) + (-1 * x200 * x145) + x169 + x167 + (-1 * x202 * x146); const FLT x206 = 1.0/2.0 * x203; const FLT x207 = -1 * x158; const FLT x208 = x200 * x144; const FLT x209 = (-1 * x34) + x162; const FLT x210 = x209 + x159 + (-1 * x3 * x206) + (x208 * x150) + (-1 * x201 * x176) + (-1 * x202 * x133) + x207; const FLT x211 = -1 * x39; const FLT x212 = x201 * x150; const FLT x213 = (-1 * x154) + x153; const FLT x214 = x213 + (x200 * x168) + x211 + (x212 * x146) + x131 + (x201 * x165) + (-1 * x203 * x148); const FLT x215 = -1 * x177; const FLT x216 = x142 * x146; const FLT x217 = (-1 * x178) + x179; const FLT x218 = x217 + (-1 * x5 * x206) + x33 + x215 + (-1 * x212 * x133) + (-1 * x208 * x148) + (x216 * x201); const FLT x219 = (x218 * sensor_pt[2]) + (-1 * x210 * sensor_pt[1]) + (x214 * sensor_pt[0]); const FLT x220 = (x205 * sensor_pt[1]) + (x214 * sensor_pt[2]) + (-1 * x218 * sensor_pt[0]); const FLT x221 = (-1 * x220 * x175) + (x210 * x182) + (-1 * x205 * x157) + (x219 * x184); const FLT x222 = (x210 * sensor_pt[0]) + (-1 * x205 * sensor_pt[2]) + (x214 * sensor_pt[1]); const FLT x223 = (x222 * x175) + (-1 * x218 * x182) + (-1 * x219 * x186) + (x205 * x188); const FLT x224 = (x218 * x157) + (x220 * x186) + (-1 * x222 * x184) + (-1 * x210 * x188); const FLT x225 = (x224 * (*lh_p).Rot[3]) + (x221 * (*lh_p).Rot[0]) + (-1 * x223 * (*lh_p).Rot[1]); const FLT x226 = (x223 * (*lh_p).Rot[2]) + (x224 * (*lh_p).Rot[0]) + (-1 * x221 * (*lh_p).Rot[3]); const FLT x227 = x223 + (x225 * x111) + (-1 * x226 * x109); const FLT x228 = (x221 * (*lh_p).Rot[1]) + (-1 * x224 * (*lh_p).Rot[2]) + (x223 * (*lh_p).Rot[0]); const FLT x229 = (x226 * x115) + x221 + (-1 * x228 * x111); const FLT x230 = x224 + (-1 * x225 * x115) + (x228 * x109); const FLT x231 = (-1 * x89 * ((-1 * ((x87 * x227) + (x86 * x229)) * x88) + (x85 * x230))) + (-1 * ((x58 * x229) + (-1 * x78 * x227)) * x84); const FLT x232 = -1 * x33; const FLT x233 = (-1 * x199 * (*error_model).Pose.AxisAngleRot[0]) + (x7 * x135) + (2 * x5) + (-1 * x198 * (*error_model).Pose.AxisAngleRot[1]); const FLT x234 = x233 * x140; const FLT x235 = x234 * x148; const FLT x236 = x234 * x132; const FLT x237 = x142 * x143; const FLT x238 = x233 * x151; const FLT x239 = x177 + (-1 * x238 * x150) + (x236 * x133) + x232 + x217 + (-1 * x235 * x146) + (-1 * x234 * x237); const FLT x240 = x234 * x150; const FLT x241 = -1 * x159; const FLT x242 = x241 + (-1 * x238 * x148) + (x234 * x165) + x158 + x209 + (x240 * x146) + (x236 * x143); const FLT x243 = x169 + x172 + (-1 * x235 * x143) + (-1 * x240 * x133) + x166 + (-1 * x238 * x132) + (x216 * x234); const FLT x244 = (x242 * sensor_pt[2]) + (x239 * sensor_pt[1]) + (-1 * x243 * sensor_pt[0]); const FLT x245 = -1 * x131; const FLT x246 = x213 + x245 + (-1 * x235 * x133) + (x240 * x143) + (-1 * x238 * x142) + x39 + (-1 * x234 * x176); const FLT x247 = (x243 * sensor_pt[2]) + (-1 * x246 * sensor_pt[1]) + (x242 * sensor_pt[0]); const FLT x248 = (x247 * x184) + (-1 * x239 * x157) + (x246 * x182) + (-1 * x244 * x175); const FLT x249 = (x246 * sensor_pt[0]) + (-1 * x239 * sensor_pt[2]) + (x242 * sensor_pt[1]); const FLT x250 = (x249 * x175) + (x239 * x188) + (-1 * x243 * x182) + (-1 * x247 * x186); const FLT x251 = (x243 * x157) + (x244 * x186) + (-1 * x249 * x184) + (-1 * x246 * x188); const FLT x252 = (x251 * (*lh_p).Rot[3]) + (x248 * (*lh_p).Rot[0]) + (-1 * x250 * (*lh_p).Rot[1]); const FLT x253 = (x248 * (*lh_p).Rot[1]) + (-1 * x251 * (*lh_p).Rot[2]) + (x250 * (*lh_p).Rot[0]); const FLT x254 = x251 + (-1 * x252 * x115) + (x253 * x109); const FLT x255 = (x250 * (*lh_p).Rot[2]) + (-1 * x248 * (*lh_p).Rot[3]) + (x251 * (*lh_p).Rot[0]); const FLT x256 = x250 + (x252 * x111) + (-1 * x255 * x109); const FLT x257 = (-1 * x253 * x111) + x248 + (x255 * x115); const FLT x258 = (-1 * x89 * ((-1 * ((x87 * x256) + (x86 * x257)) * x88) + (x85 * x254))) + (-1 * ((x58 * x257) + (-1 * x78 * x256)) * x84); const FLT x259 = (x137 * (*error_model).Pose.AxisAngleRot[0]) + (2 * x3) + (-1 * x136 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x6 * x135); const FLT x260 = x259 * x140; const FLT x261 = x259 * x151; const FLT x262 = x259 * x148; const FLT x263 = x262 * x140; const FLT x264 = x259 * x150; const FLT x265 = x204 + (x264 * x144) + x170 + (-1 * x260 * x176) + (-1 * x261 * x142) + x166 + (-1 * x263 * x133); const FLT x266 = (x259 * x168) + (x260 * x165) + x180 + (x264 * x147) + x232 + (-1 * x261 * x148) + x215; const FLT x267 = x259 * x142; const FLT x268 = x260 * x133; const FLT x269 = x155 + (x267 * x147) + x211 + x245 + (-1 * x268 * x150) + (-1 * x261 * x132) + (-1 * x262 * x144); const FLT x270 = (x269 * sensor_pt[2]) + (-1 * x265 * sensor_pt[1]) + (x266 * sensor_pt[0]); const FLT x271 = x163 + x207 + (x268 * x132) + x241 + (-1 * x263 * x146) + (-1 * x261 * x150) + (-1 * x267 * x144); const FLT x272 = (x265 * sensor_pt[0]) + (x266 * sensor_pt[1]) + (-1 * x271 * sensor_pt[2]); const FLT x273 = (x271 * x188) + (-1 * x270 * x186) + (x272 * x175) + (-1 * x269 * x182); const FLT x274 = (x271 * sensor_pt[1]) + (-1 * x269 * sensor_pt[0]) + (x266 * sensor_pt[2]); const FLT x275 = (x265 * x182) + (x270 * x184) + (-1 * x274 * x175) + (-1 * x271 * x157); const FLT x276 = (x269 * x157) + (x274 * x186) + (-1 * x272 * x184) + (-1 * x265 * x188); const FLT x277 = 2 * ((x276 * (*lh_p).Rot[3]) + (-1 * x273 * (*lh_p).Rot[1]) + (x275 * (*lh_p).Rot[0])); const FLT x278 = 2 * ((x275 * (*lh_p).Rot[1]) + (-1 * x276 * (*lh_p).Rot[2]) + (x273 * (*lh_p).Rot[0])); const FLT x279 = (-1 * x277 * (*lh_p).Rot[3]) + x276 + (x278 * (*lh_p).Rot[2]); const FLT x280 = (x273 * (*lh_p).Rot[2]) + (-1 * x275 * (*lh_p).Rot[3]) + (x276 * (*lh_p).Rot[0]); const FLT x281 = x273 + (-1 * x280 * x109) + (x277 * (*lh_p).Rot[1]); const FLT x282 = x275 + (x280 * x115) + (-1 * x278 * (*lh_p).Rot[1]); const FLT x283 = (-1 * x89 * ((-1 * ((x87 * x281) + (x86 * x282)) * x88) + (x85 * x279))) + (-1 * ((x58 * x282) + (-1 * x78 * x281)) * x84); const FLT x284 = dt * dt * dt; const FLT x285 = 0.5 * x24 * x284; const FLT x286 = x46 * x285; const FLT x287 = dt * dt * dt * dt; const FLT x288 = (x13 * x13 * x13) * x287; const FLT x289 = x22 * (1. / (x19 * sqrt(x19))); const FLT x290 = 1.0 * x26; const FLT x291 = x290 * x289; const FLT x292 = x13 * x10; const FLT x293 = 2 * x25; const FLT x294 = 2 * x23 * (1. / (x19 * x19)); const FLT x295 = x294 * x287; const FLT x296 = x13 * x295; const FLT x297 = x291 * x287; const FLT x298 = x13 * x297; const FLT x299 = x30 * x290; const FLT x300 = (-1 * x17 * x296) + (x293 * x292) + (x291 * x288) + (-1 * x294 * x288) + (-1 * x11 * x296) + (-1 * x299 * x292) + (x17 * x298) + (x11 * x298); const FLT x301 = x9 * (1. / (x27 * sqrt(x27))); const FLT x302 = x301 * x300; const FLT x303 = x302 * x150; const FLT x304 = x301 * x148; const FLT x305 = x300 * x304; const FLT x306 = x3 * x29; const FLT x307 = 0.5 * x30; const FLT x308 = x292 * x307; const FLT x309 = x5 * x29; const FLT x310 = x289 * x284; const FLT x311 = x14 * x310; const FLT x312 = x26 * x301; const FLT x313 = x300 * x312; const FLT x314 = x5 * x32; const FLT x315 = x37 * x285; const FLT x316 = x4 * x13; const FLT x317 = x38 * x310; const FLT x318 = x4 * x317; const FLT x319 = (-1 * x6 * x318) + (x315 * x316); const FLT x320 = x43 * x285; const FLT x321 = x13 * x16; const FLT x322 = x16 * x317; const FLT x323 = (x7 * x322) + (-1 * x320 * x321); const FLT x324 = x314 + (-1 * x313 * x142) + (-1 * x309 * x311) + (x14 * x286) + x323 + x319 + (-1 * x302 * x176) + (-1 * x305 * x133) + (x303 * x143) + (-1 * x308 * x306); const FLT x325 = x132 * x143; const FLT x326 = x5 * x322; const FLT x327 = x286 * x321; const FLT x328 = x7 * x29; const FLT x329 = x6 * x29; const FLT x330 = x7 * x32; const FLT x331 = -1 * x330; const FLT x332 = x41 * x285; const FLT x333 = (x3 * x318) + (-1 * x332 * x316); const FLT x334 = (-1 * x313 * x148) + (-1 * x327) + (x302 * x325) + (x328 * x311) + (-1 * x14 * x320) + x331 + (x303 * x146) + (-1 * x308 * x329) + x326 + x333 + (x302 * x165); const FLT x335 = x320 * x316; const FLT x336 = x7 * x318; const FLT x337 = x3 * x32; const FLT x338 = -1 * x337; const FLT x339 = (-1 * x6 * x322) + (x315 * x321); const FLT x340 = (x306 * x311) + (-1 * x336) + (-1 * x305 * x143) + x335 + x338 + (-1 * x303 * x133) + (-1 * x14 * x332) + x339 + (-1 * x309 * x308) + (x216 * x302) + (-1 * x313 * x132); const FLT x341 = (x340 * sensor_pt[2]) + (-1 * x324 * sensor_pt[1]) + (x334 * sensor_pt[0]); const FLT x342 = x312 * x150; const FLT x343 = x6 * x32; const FLT x344 = x307 * x328; const FLT x345 = x16 * x332; const FLT x346 = x13 * x345; const FLT x347 = x304 * x146; const FLT x348 = x5 * x318; const FLT x349 = x3 * x322; const FLT x350 = x286 * x316; const FLT x351 = (-1 * x350) + (-1 * x237 * x302) + x346 + (-1 * x292 * x344) + (-1 * x300 * x342) + (x302 * x134) + x343 + (-1 * x300 * x347) + (-1 * x349) + x348 + (-1 * x329 * x311) + (x14 * x315); const FLT x352 = (x324 * sensor_pt[0]) + (-1 * x351 * sensor_pt[2]) + (x334 * sensor_pt[1]); const FLT x353 = (-1 * x340 * x182) + (x352 * x175) + (-1 * x341 * x186) + (x351 * x188); const FLT x354 = (x351 * sensor_pt[1]) + (-1 * x340 * sensor_pt[0]) + (x334 * sensor_pt[2]); const FLT x355 = (x341 * x184) + (-1 * x351 * x157) + (x324 * x182) + (-1 * x354 * x175); const FLT x356 = (x354 * x186) + (-1 * x324 * x188) + (-1 * x352 * x184) + (x340 * x157); const FLT x357 = (-1 * x353 * (*lh_p).Rot[1]) + (x356 * (*lh_p).Rot[3]) + (x355 * (*lh_p).Rot[0]); const FLT x358 = (x355 * (*lh_p).Rot[1]) + (-1 * x356 * (*lh_p).Rot[2]) + (x353 * (*lh_p).Rot[0]); const FLT x359 = x356 + (-1 * x357 * x115) + (x358 * x109); const FLT x360 = (-1 * x355 * (*lh_p).Rot[3]) + (x353 * (*lh_p).Rot[2]) + (x356 * (*lh_p).Rot[0]); const FLT x361 = x353 + (-1 * x360 * x109) + (x357 * x111); const FLT x362 = x355 + (-1 * x358 * x111) + (x360 * x115); const FLT x363 = (-1 * x89 * ((-1 * ((x87 * x361) + (x86 * x362)) * x88) + (x85 * x359))) + (-1 * ((x58 * x362) + (-1 * x78 * x361)) * x84); const FLT x364 = x10 * x344; const FLT x365 = x14 * x297; const FLT x366 = x14 * x295; const FLT x367 = x10 * x299; const FLT x368 = x11 * x16; const FLT x369 = x16 * x16 * x16; const FLT x370 = x10 * x293; const FLT x371 = (x16 * x370) + (-1 * x16 * x366) + (-1 * x295 * x369) + (-1 * x295 * x368) + (-1 * x16 * x367) + (x297 * x369) + (x16 * x365) + (x297 * x368); const FLT x372 = x371 * x301; const FLT x373 = x17 * x310; const FLT x374 = x16 * x310; const FLT x375 = x4 * x374; const FLT x376 = x4 * x16; const FLT x377 = (-1 * x286 * x376) + (x375 * x309); const FLT x378 = x377 + x339 + (x372 * x134) + (-1 * x371 * x342) + x337 + (x17 * x332) + (-1 * x373 * x306) + (-1 * x16 * x364) + (-1 * x237 * x372) + (-1 * x371 * x347); const FLT x379 = x372 * x150; const FLT x380 = x371 * x312; const FLT x381 = x371 * x304; const FLT x382 = x17 * x285; const FLT x383 = x10 * x307; const FLT x384 = (x376 * x315) + (-1 * x375 * x329); const FLT x385 = (-1 * x43 * x382) + (-1 * x381 * x133) + (-1 * x16 * x383 * x306) + (-1 * x326) + (-1 * x380 * x142) + x327 + x384 + (x379 * x143) + (x373 * x328) + x331 + (-1 * x372 * x176); const FLT x386 = -1 * x314; const FLT x387 = x4 * x345; const FLT x388 = x4 * x306; const FLT x389 = x374 * x388; const FLT x390 = x383 * x329; const FLT x391 = x323 + (x379 * x146) + (x373 * x309) + (-1 * x17 * x286) + (x372 * x325) + (-1 * x380 * x148) + x386 + (-1 * x16 * x390) + (x372 * x165) + (-1 * x387) + x389; const FLT x392 = x376 * x320; const FLT x393 = x383 * x309; const FLT x394 = x375 * x328; const FLT x395 = (-1 * x16 * x393) + x392 + (-1 * x373 * x329) + x343 + (-1 * x381 * x143) + (-1 * x346) + (x37 * x382) + x349 + (-1 * x379 * x133) + (-1 * x394) + (x216 * x372) + (-1 * x380 * x132); const FLT x396 = (x395 * sensor_pt[2]) + (-1 * x385 * sensor_pt[1]) + (x391 * sensor_pt[0]); const FLT x397 = (x378 * sensor_pt[1]) + (-1 * x395 * sensor_pt[0]) + (x391 * sensor_pt[2]); const FLT x398 = (-1 * x397 * x175) + (x396 * x184) + (-1 * x378 * x157) + (x385 * x182); const FLT x399 = (x385 * sensor_pt[0]) + (-1 * x378 * sensor_pt[2]) + (x391 * sensor_pt[1]); const FLT x400 = (-1 * x395 * x182) + (x399 * x175) + (-1 * x396 * x186) + (x378 * x188); const FLT x401 = (-1 * x399 * x184) + (x395 * x157) + (x397 * x186) + (-1 * x385 * x188); const FLT x402 = (x401 * (*lh_p).Rot[3]) + (x398 * (*lh_p).Rot[0]) + (-1 * x400 * (*lh_p).Rot[1]); const FLT x403 = (x398 * (*lh_p).Rot[1]) + (-1 * x401 * (*lh_p).Rot[2]) + (x400 * (*lh_p).Rot[0]); const FLT x404 = (-1 * x402 * x115) + x401 + (x403 * x109); const FLT x405 = (x400 * (*lh_p).Rot[2]) + (-1 * x398 * (*lh_p).Rot[3]) + (x401 * (*lh_p).Rot[0]); const FLT x406 = (-1 * x405 * x109) + x400 + (x402 * x111); const FLT x407 = x398 + (-1 * x403 * x111) + (x405 * x115); const FLT x408 = (-1 * x89 * ((-1 * ((x87 * x406) + (x86 * x407)) * x88) + (x85 * x404))) + (-1 * ((x58 * x407) + (-1 * x78 * x406)) * x84); const FLT x409 = x4 * x4 * x4; const FLT x410 = x4 * x17; const FLT x411 = (x410 * x297) + (-1 * x409 * x295) + (x409 * x297) + (-1 * x410 * x295) + (x4 * x365) + (x4 * x370) + (-1 * x4 * x367) + (-1 * x4 * x366); const FLT x412 = x411 * x301; const FLT x413 = x412 * x150; const FLT x414 = x411 * x304; const FLT x415 = x11 * x310; const FLT x416 = x411 * x312; const FLT x417 = (-1 * x412 * x176) + (-1 * x348) + (-1 * x414 * x133) + x350 + x343 + (x11 * x315) + (-1 * x383 * x388) + (-1 * x415 * x329) + (-1 * x392) + x394 + (x413 * x143) + (-1 * x416 * x142); const FLT x418 = (x412 * x325) + x377 + (-1 * x335) + x336 + (x412 * x165) + (-1 * x4 * x390) + x338 + (x413 * x146) + (-1 * x416 * x148) + (x415 * x306) + (-1 * x11 * x332); const FLT x419 = x384 + (-1 * x413 * x133) + (-1 * x415 * x328) + x330 + x333 + (-1 * x416 * x132) + (-1 * x414 * x143) + (-1 * x4 * x393) + (x11 * x320) + (x412 * x216); const FLT x420 = (x419 * sensor_pt[2]) + (-1 * x417 * sensor_pt[1]) + (x418 * sensor_pt[0]); const FLT x421 = (-1 * x11 * x286) + (-1 * x411 * x347) + (x412 * x134) + (x415 * x309) + x386 + (-1 * x389) + x387 + (-1 * x411 * x342) + x319 + (-1 * x412 * x237) + (-1 * x4 * x364); const FLT x422 = (x417 * sensor_pt[0]) + (-1 * x421 * sensor_pt[2]) + (x418 * sensor_pt[1]); const FLT x423 = (-1 * x419 * x182) + (x421 * x188) + (-1 * x420 * x186) + (x422 * x175); const FLT x424 = (-1 * x419 * sensor_pt[0]) + (x421 * sensor_pt[1]) + (x418 * sensor_pt[2]); const FLT x425 = (x420 * x184) + (x417 * x182) + (-1 * x421 * x157) + (-1 * x424 * x175); const FLT x426 = (x419 * x157) + (x424 * x186) + (-1 * x422 * x184) + (-1 * x417 * x188); const FLT x427 = (-1 * x423 * (*lh_p).Rot[1]) + (x426 * (*lh_p).Rot[3]) + (x425 * (*lh_p).Rot[0]); const FLT x428 = (-1 * x426 * (*lh_p).Rot[2]) + (x425 * (*lh_p).Rot[1]) + (x423 * (*lh_p).Rot[0]); const FLT x429 = x426 + (-1 * x427 * x115) + (x428 * x109); const FLT x430 = (x423 * (*lh_p).Rot[2]) + (-1 * x425 * (*lh_p).Rot[3]) + (x426 * (*lh_p).Rot[0]); const FLT x431 = x423 + (-1 * x430 * x109) + (x427 * x111); const FLT x432 = (x430 * x115) + x425 + (-1 * x428 * x111); const FLT x433 = (-1 * x89 * ((-1 * ((x87 * x431) + (x86 * x432)) * x88) + (x85 * x429))) + (-1 * ((x58 * x432) + (-1 * x78 * x431)) * x84); const FLT x434 = -1 * dt * x106; const FLT x435 = dt + (-1 * dt * x104); const FLT x436 = x435 + x434; const FLT x437 = dt * x110; const FLT x438 = dt * x112; const FLT x439 = x438 + (-1 * x437); const FLT x440 = dt * x114; const FLT x441 = dt * x116; const FLT x442 = x441 + x440; const FLT x443 = (-1 * x89 * ((-1 * ((x87 * x439) + (x86 * x442)) * x88) + (x85 * x436))) + (-1 * ((x58 * x442) + (-1 * x78 * x439)) * x84); const FLT x444 = x440 + (-1 * x441); const FLT x445 = dt * x120; const FLT x446 = dt * x121; const FLT x447 = x446 + x445; const FLT x448 = -1 * dt * x123; const FLT x449 = x435 + x448; const FLT x450 = (-1 * x89 * ((-1 * ((x87 * x447) + (x86 * x449)) * x88) + (x85 * x444))) + (-1 * ((x58 * x449) + (-1 * x78 * x447)) * x84); const FLT x451 = x437 + x438; const FLT x452 = x434 + dt + x448; const FLT x453 = x445 + (-1 * x446); const FLT x454 = (-1 * x89 * ((-1 * ((x87 * x452) + (x86 * x453)) * x88) + (x85 * x451))) + (-1 * ((x58 * x453) + (-1 * x78 * x452)) * x84); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x90 + (((x73 * x68) + (-1 * x63 * x58)) * x76) + (x91 * x90)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x99 + (((x73 * x95) + (-1 * x58 * x92)) * x76) + (x91 * x99)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x103 + (((x73 * x101) + (-1 * x58 * x100)) * x76) + (x91 * x103)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x118 + (((x73 * x113) + (-1 * x58 * x108)) * x76) + (x91 * x118)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x126 + (((x73 * x122) + (-1 * x58 * x119)) * x76) + (x91 * x126)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x130 + (((x73 * x128) + (-1 * x58 * x127)) * x76) + (x91 * x130)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x197 + (((x73 * x195) + (-1 * x58 * x193)) * x76) + (x91 * x197)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x91 * x231) + x231 + (((x73 * x227) + (-1 * x58 * x230)) * x76)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (((x73 * x256) + (-1 * x58 * x254)) * x76) + x258 + (x91 * x258)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x283 + (((x73 * x281) + (-1 * x58 * x279)) * x76) + (x91 * x283)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x363 + (((x73 * x361) + (-1 * x58 * x359)) * x76) + (x91 * x363)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x408 + (((x73 * x406) + (-1 * x58 * x404)) * x76) + (x91 * x408)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x433 + (((x73 * x431) + (-1 * x58 * x429)) * x76) + (x91 * x433)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x443 + (((x73 * x439) + (-1 * x58 * x436)) * x76) + (x91 * x443)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x450 + (((x73 * x447) + (-1 * x58 * x444)) * x76) + (x91 * x450)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x454 + (((x73 * x452) + (-1 * x58 * x451)) * x76) + (x91 * x454)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = dt * dt; const FLT x2 = x0 * x0; const FLT x3 = x2 * x1; const FLT x4 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x5 = x4 * x4; const FLT x6 = x1 * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x8 = x7 * x7; const FLT x9 = x1 * x8; const FLT x10 = 1e-10 + x3 + x9 + x6; const FLT x11 = sqrt(x10); const FLT x12 = 0.5 * x11; const FLT x13 = sin(x12); const FLT x14 = x13 * x13; const FLT x15 = 1. / x10; const FLT x16 = x15 * x14; const FLT x17 = cos(x12); const FLT x18 = (x6 * x16) + (x17 * x17) + (x3 * x16) + (x9 * x16); const FLT x19 = 1. / sqrt(x18); const FLT x20 = x13 * (1. / x11); const FLT x21 = x20 * x19; const FLT x22 = dt * x21; const FLT x23 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x24 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x25 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x26 = (x25 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x23 * (*_x0).Pose.Rot[2]) + (x24 * (*_x0).Pose.Rot[0]); const FLT x27 = (-1 * x24 * (*_x0).Pose.Rot[1]) + (x25 * (*_x0).Pose.Rot[0]) + (x23 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x28 = (-1 * x23 * (*_x0).Pose.Rot[1]) + (-1 * x25 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x24 * (*_x0).Pose.Rot[3]); const FLT x29 = (x23 * (*_x0).Pose.Rot[0]) + (-1 * x25 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[1] + (x24 * (*_x0).Pose.Rot[2]); const FLT x30 = (x29 * x29) + (x28 * x28) + (x26 * x26) + (x27 * x27); const FLT x31 = 1. / sqrt(x30); const FLT x32 = x31 * x26; const FLT x33 = x32 * x22; const FLT x34 = x31 * x27; const FLT x35 = x34 * x22; const FLT x36 = x19 * x17; const FLT x37 = x31 * x36; const FLT x38 = x37 * x28; const FLT x39 = x31 * x29; const FLT x40 = x39 * x22; const FLT x41 = (-1 * x4 * x40) + x38 + (-1 * x0 * x33) + (-1 * x7 * x35); const FLT x42 = x31 * x28; const FLT x43 = x42 * x22; const FLT x44 = x37 * x26; const FLT x45 = x44 + (x0 * x43) + (x4 * x35) + (-1 * x7 * x40); const FLT x46 = x36 * x39; const FLT x47 = (x4 * x43) + (-1 * x0 * x35) + (x7 * x33) + x46; const FLT x48 = (-1 * x47 * sensor_pt[2]) + (x41 * sensor_pt[1]) + (x45 * sensor_pt[0]); const FLT x49 = x37 * x27; const FLT x50 = (-1 * x4 * x33) + (x0 * x40) + (x7 * x43) + x49; const FLT x51 = (-1 * x50 * sensor_pt[0]) + (x41 * sensor_pt[2]) + (x47 * sensor_pt[1]); const FLT x52 = dt * fabs(dt); const FLT x53 = 1.0/2.0 * x52; const FLT x54 = (x53 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x50 * x51) + (-1 * x45 * x48))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x55 = (-1 * x45 * sensor_pt[1]) + (x41 * sensor_pt[0]) + (x50 * sensor_pt[2]); const FLT x56 = (*_x0).Pose.Pos[2] + (x53 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x47 * x48) + (-1 * x50 * x55))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x57 = (*_x0).Pose.Pos[1] + (2 * ((x55 * x45) + (-1 * x51 * x47))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x53 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x58 = (-1 * x57 * (*lh_p).Rot[3]) + (x54 * (*lh_p).Rot[0]) + (x56 * (*lh_p).Rot[2]); const FLT x59 = (x57 * (*lh_p).Rot[0]) + (-1 * x56 * (*lh_p).Rot[1]) + (x54 * (*lh_p).Rot[3]); const FLT x60 = x56 + (2 * ((x59 * (*lh_p).Rot[1]) + (-1 * x58 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x61 = 1. / x60; const FLT x62 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x63 = -1 * x62 * x52; const FLT x64 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x65 = (-1 * x64 * x52) + x53; const FLT x66 = x65 + x63; const FLT x67 = x52 * (*lh_p).Rot[0]; const FLT x68 = x67 * (*lh_p).Rot[2]; const FLT x69 = x52 * (*lh_p).Rot[3]; const FLT x70 = x69 * (*lh_p).Rot[1]; const FLT x71 = x70 + (-1 * x68); const FLT x72 = x60 * x60; const FLT x73 = 1. / x72; const FLT x74 = (x56 * (*lh_p).Rot[0]) + (-1 * x54 * (*lh_p).Rot[2]) + (x57 * (*lh_p).Rot[1]); const FLT x75 = (*lh_p).Pos[0] + x54 + (2 * ((x74 * (*lh_p).Rot[2]) + (-1 * x59 * (*lh_p).Rot[3]))); const FLT x76 = x73 * x75; const FLT x77 = -1 * x60; const FLT x78 = x75 * x75; const FLT x79 = 2 * (1. / (x72 + x78)) * x72 * atan2(x75, x77) * (*bsc0).curve; const FLT x80 = x57 + (*lh_p).Pos[1] + (2 * ((x58 * (*lh_p).Rot[3]) + (-1 * x74 * (*lh_p).Rot[1]))); const FLT x81 = x80 * x73; const FLT x82 = x52 * (*lh_p).Rot[1] * (*lh_p).Rot[2]; const FLT x83 = x67 * (*lh_p).Rot[3]; const FLT x84 = x83 + x82; const FLT x85 = x72 + (x80 * x80); const FLT x86 = 1. / x85; const FLT x87 = x86 * x72; const FLT x88 = (1. / sqrt(x85)) * (*bsc0).tilt; const FLT x89 = 2 * x80; const FLT x90 = 2 * x60; const FLT x91 = 1.0/2.0 * (1. / (x85 * sqrt(x85))) * x75 * (*bsc0).tilt; const FLT x92 = 1. / sqrt(1 + (-1 * x86 * x78 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x93 = (-1 * x92 * ((-1 * ((x71 * x90) + (x89 * x84)) * x91) + (x88 * x66))) + (-1 * ((x84 * x61) + (-1 * x81 * x71)) * x87); const FLT x94 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x88 * x75)) + (-1 * atan2(-1 * x80, x77))); const FLT x95 = x82 + (-1 * x83); const FLT x96 = x69 * (*lh_p).Rot[2]; const FLT x97 = x67 * (*lh_p).Rot[1]; const FLT x98 = x97 + x96; const FLT x99 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x100 = -1 * x52 * x99; const FLT x101 = x100 + x63 + x53; const FLT x102 = (-1 * x92 * ((-1 * x91 * ((x90 * x98) + (x89 * x101))) + (x88 * x95))) + (-1 * x87 * ((x61 * x101) + (-1 * x81 * x98))); const FLT x103 = x68 + x70; const FLT x104 = x65 + x100; const FLT x105 = x96 + (-1 * x97); const FLT x106 = (-1 * x92 * ((-1 * ((x90 * x104) + (x89 * x105)) * x91) + (x88 * x103))) + (-1 * ((x61 * x105) + (-1 * x81 * x104)) * x87); const FLT x107 = x4 * x22; const FLT x108 = 1.0 * x28; const FLT x109 = 1.0 * (*_x0).Pose.Rot[2]; const FLT x110 = 1.0 * x29; const FLT x111 = 1.0 * x27; const FLT x112 = (-1 * x108 * (*_x0).Pose.Rot[1]) + (x111 * (*_x0).Pose.Rot[3]) + (x110 * (*_x0).Pose.Rot[0]) + (-1 * x26 * x109); const FLT x113 = 1.0/2.0 * (1. / (x30 * sqrt(x30))); const FLT x114 = x112 * x113; const FLT x115 = x28 * x114; const FLT x116 = x0 * x22; const FLT x117 = x27 * x114; const FLT x118 = x7 * x22; const FLT x119 = x26 * x118; const FLT x120 = x36 * x113; const FLT x121 = x112 * x120; const FLT x122 = 0.5 * x31; const FLT x123 = x22 * x122; const FLT x124 = x4 * x123; const FLT x125 = x7 * x123; const FLT x126 = 0.5 * x37; const FLT x127 = x116 * x122; const FLT x128 = (-1 * x127 * (*_x0).Pose.Rot[3]) + (x126 * (*_x0).Pose.Rot[0]) + (-1 * x124 * (*_x0).Pose.Rot[1]) + (-1 * x125 * (*_x0).Pose.Rot[2]); const FLT x129 = (-1 * x119 * x114) + x128 + (-1 * x29 * x121) + (-1 * x107 * x115) + (x116 * x117); const FLT x130 = 2 * x51; const FLT x131 = x29 * x114; const FLT x132 = x124 * (*_x0).Pose.Rot[2]; const FLT x133 = x125 * (*_x0).Pose.Rot[1]; const FLT x134 = x126 * (*_x0).Pose.Rot[3]; const FLT x135 = x107 * x114; const FLT x136 = x127 * (*_x0).Pose.Rot[0]; const FLT x137 = (-1 * x118 * x115) + (-1 * x27 * x121) + x132 + (-1 * x133) + x136 + x134 + (-1 * x116 * x131) + (x26 * x135); const FLT x138 = x26 * x116; const FLT x139 = x127 * (*_x0).Pose.Rot[2]; const FLT x140 = x125 * (*_x0).Pose.Rot[3]; const FLT x141 = x124 * (*_x0).Pose.Rot[0]; const FLT x142 = x126 * (*_x0).Pose.Rot[1]; const FLT x143 = (-1 * x142) + (-1 * x141) + x139 + (-1 * x140); const FLT x144 = (x107 * x131) + x143 + (x118 * x117) + (-1 * x28 * x121) + (x114 * x138); const FLT x145 = (x129 * sensor_pt[1]) + (-1 * x137 * sensor_pt[0]) + (x144 * sensor_pt[2]); const FLT x146 = 2 * x47; const FLT x147 = x124 * (*_x0).Pose.Rot[3]; const FLT x148 = x125 * (*_x0).Pose.Rot[0]; const FLT x149 = x127 * (*_x0).Pose.Rot[1]; const FLT x150 = x126 * (*_x0).Pose.Rot[2]; const FLT x151 = (-1 * x149) + (-1 * x150) + x147 + (-1 * x148); const FLT x152 = (x118 * x131) + x151 + (-1 * x26 * x121) + (-1 * x27 * x135) + (-1 * x115 * x116); const FLT x153 = 2 * x55; const FLT x154 = (x137 * sensor_pt[2]) + (-1 * x152 * sensor_pt[1]) + (x144 * sensor_pt[0]); const FLT x155 = 2 * x45; const FLT x156 = (x154 * x155) + (x152 * x153) + (-1 * x129 * x130) + (-1 * x146 * x145); const FLT x157 = 2 * x50; const FLT x158 = (x152 * sensor_pt[0]) + (-1 * x129 * sensor_pt[2]) + (x144 * sensor_pt[1]); const FLT x159 = 2 * x48; const FLT x160 = (x129 * x159) + (x146 * x158) + (-1 * x154 * x157) + (-1 * x137 * x153); const FLT x161 = (x145 * x157) + (-1 * x152 * x159) + (-1 * x155 * x158) + (x130 * x137); const FLT x162 = (x156 * (*lh_p).Rot[0]) + (x161 * (*lh_p).Rot[3]) + (-1 * x160 * (*lh_p).Rot[1]); const FLT x163 = 2 * (*lh_p).Rot[3]; const FLT x164 = (-1 * x161 * (*lh_p).Rot[2]) + (x156 * (*lh_p).Rot[1]) + (x160 * (*lh_p).Rot[0]); const FLT x165 = 2 * (*lh_p).Rot[2]; const FLT x166 = x161 + (-1 * x163 * x162) + (x165 * x164); const FLT x167 = (x160 * (*lh_p).Rot[2]) + (-1 * x156 * (*lh_p).Rot[3]) + (x161 * (*lh_p).Rot[0]); const FLT x168 = 2 * (*lh_p).Rot[1]; const FLT x169 = x160 + (-1 * x167 * x165) + (x168 * x162); const FLT x170 = x156 + (-1 * x168 * x164) + (x167 * x163); const FLT x171 = (-1 * x92 * ((-1 * ((x90 * x169) + (x89 * x170)) * x91) + (x88 * x166))) + (-1 * ((x61 * x170) + (-1 * x81 * x169)) * x87); const FLT x172 = 1.0 * x26; const FLT x173 = (x172 * (*_x0).Pose.Rot[1]) + (-1 * x110 * (*_x0).Pose.Rot[3]) + (x111 * (*_x0).Pose.Rot[0]) + (-1 * x28 * x109); const FLT x174 = x113 * x173; const FLT x175 = x28 * x174; const FLT x176 = x27 * x174; const FLT x177 = x120 * x173; const FLT x178 = (-1 * x134) + x133 + (-1 * x136) + (-1 * x132); const FLT x179 = x178 + (-1 * x119 * x174) + (-1 * x29 * x177) + (-1 * x107 * x175) + (x116 * x176); const FLT x180 = x107 * x174; const FLT x181 = x151 + (x174 * x138) + (x118 * x176) + (x29 * x180) + (-1 * x28 * x177); const FLT x182 = x29 * x174; const FLT x183 = x142 + (-1 * x139) + (-1 * x26 * x177) + x141 + (-1 * x107 * x176) + (x118 * x182) + (-1 * x116 * x175) + x140; const FLT x184 = (x183 * sensor_pt[0]) + (-1 * x179 * sensor_pt[2]) + (x181 * sensor_pt[1]); const FLT x185 = x128 + (x26 * x180) + (-1 * x27 * x177) + (-1 * x116 * x182) + (-1 * x118 * x175); const FLT x186 = (x179 * sensor_pt[1]) + (-1 * x185 * sensor_pt[0]) + (x181 * sensor_pt[2]); const FLT x187 = (x185 * x130) + (x186 * x157) + (-1 * x184 * x155) + (-1 * x183 * x159); const FLT x188 = (x185 * sensor_pt[2]) + (x181 * sensor_pt[0]) + (-1 * x183 * sensor_pt[1]); const FLT x189 = (x179 * x159) + (x184 * x146) + (-1 * x188 * x157) + (-1 * x185 * x153); const FLT x190 = (x188 * x155) + (x183 * x153) + (-1 * x179 * x130) + (-1 * x186 * x146); const FLT x191 = (x190 * (*lh_p).Rot[1]) + (-1 * x187 * (*lh_p).Rot[2]) + (x189 * (*lh_p).Rot[0]); const FLT x192 = (-1 * x189 * (*lh_p).Rot[1]) + (x187 * (*lh_p).Rot[3]) + (x190 * (*lh_p).Rot[0]); const FLT x193 = x187 + (x165 * x191) + (-1 * x163 * x192); const FLT x194 = (x189 * (*lh_p).Rot[2]) + (-1 * x190 * (*lh_p).Rot[3]) + (x187 * (*lh_p).Rot[0]); const FLT x195 = x189 + (-1 * x165 * x194) + (x168 * x192); const FLT x196 = x190 + (-1 * x168 * x191) + (x163 * x194); const FLT x197 = (-1 * x92 * ((-1 * ((x90 * x195) + (x89 * x196)) * x91) + (x88 * x193))) + (-1 * ((x61 * x196) + (-1 * x81 * x195)) * x87); const FLT x198 = (-1 * x111 * (*_x0).Pose.Rot[1]) + (x29 * x109) + (x172 * (*_x0).Pose.Rot[0]) + (-1 * x108 * (*_x0).Pose.Rot[3]); const FLT x199 = x113 * x198; const FLT x200 = x28 * x199; const FLT x201 = x27 * x199; const FLT x202 = x120 * x198; const FLT x203 = x149 + (-1 * x29 * x202) + (x201 * x116) + x150 + (-1 * x200 * x107) + x148 + (-1 * x147) + (-1 * x119 * x199); const FLT x204 = x107 * x199; const FLT x205 = x29 * x199; const FLT x206 = x143 + (-1 * x200 * x118) + (-1 * x27 * x202) + (x26 * x204) + (-1 * x205 * x116); const FLT x207 = x178 + (x29 * x204) + (-1 * x28 * x202) + (x201 * x118) + (x199 * x138); const FLT x208 = (x203 * sensor_pt[1]) + (-1 * x206 * sensor_pt[0]) + (x207 * sensor_pt[2]); const FLT x209 = x128 + (-1 * x200 * x116) + (-1 * x27 * x204) + (x205 * x118) + (-1 * x26 * x202); const FLT x210 = (-1 * x209 * sensor_pt[1]) + (x206 * sensor_pt[2]) + (x207 * sensor_pt[0]); const FLT x211 = (x210 * x155) + (x209 * x153) + (-1 * x203 * x130) + (-1 * x208 * x146); const FLT x212 = (x209 * sensor_pt[0]) + (-1 * x203 * sensor_pt[2]) + (x207 * sensor_pt[1]); const FLT x213 = (x206 * x130) + (-1 * x212 * x155) + (-1 * x209 * x159) + (x208 * x157); const FLT x214 = (x203 * x159) + (x212 * x146) + (-1 * x210 * x157) + (-1 * x206 * x153); const FLT x215 = (-1 * x211 * (*lh_p).Rot[3]) + (x214 * (*lh_p).Rot[2]) + (x213 * (*lh_p).Rot[0]); const FLT x216 = (x213 * (*lh_p).Rot[3]) + (-1 * x214 * (*lh_p).Rot[1]) + (x211 * (*lh_p).Rot[0]); const FLT x217 = (-1 * x215 * x165) + x214 + (x216 * x168); const FLT x218 = (x211 * (*lh_p).Rot[1]) + (-1 * x213 * (*lh_p).Rot[2]) + (x214 * (*lh_p).Rot[0]); const FLT x219 = x211 + (x215 * x163) + (-1 * x218 * x168); const FLT x220 = x213 + (x218 * x165) + (-1 * x216 * x163); const FLT x221 = (-1 * x92 * ((-1 * ((x90 * x217) + (x89 * x219)) * x91) + (x88 * x220))) + (-1 * ((x61 * x219) + (-1 * x81 * x217)) * x87); const FLT x222 = 2 * x64; const FLT x223 = -1 * x222; const FLT x224 = 2 * x62; const FLT x225 = 1 + (-1 * x224); const FLT x226 = x225 + x223; const FLT x227 = x165 * (*lh_p).Rot[0]; const FLT x228 = x168 * (*lh_p).Rot[3]; const FLT x229 = x228 + (-1 * x227); const FLT x230 = x165 * (*lh_p).Rot[1]; const FLT x231 = x163 * (*lh_p).Rot[0]; const FLT x232 = x231 + x230; const FLT x233 = (-1 * x92 * ((-1 * ((x90 * x229) + (x89 * x232)) * x91) + (x88 * x226))) + (-1 * ((x61 * x232) + (-1 * x81 * x229)) * x87); const FLT x234 = x230 + (-1 * x231); const FLT x235 = x165 * (*lh_p).Rot[3]; const FLT x236 = x168 * (*lh_p).Rot[0]; const FLT x237 = x236 + x235; const FLT x238 = 2 * x99; const FLT x239 = -1 * x238; const FLT x240 = x225 + x239; const FLT x241 = (-1 * x92 * ((-1 * ((x90 * x237) + (x89 * x240)) * x91) + (x88 * x234))) + (-1 * ((x61 * x240) + (-1 * x81 * x237)) * x87); const FLT x242 = x227 + x228; const FLT x243 = 1 + x239 + x223; const FLT x244 = x235 + (-1 * x236); const FLT x245 = (-1 * x92 * ((-1 * ((x90 * x243) + (x89 * x244)) * x91) + (x88 * x242))) + (-1 * ((x61 * x244) + (-1 * x81 * x243)) * x87); const FLT x246 = dt * dt * dt; const FLT x247 = x5 * x246; const FLT x248 = 0.5 * x15; const FLT x249 = x247 * x248; const FLT x250 = x4 * x4 * x4; const FLT x251 = dt * dt * dt * dt; const FLT x252 = 1.0 * x17; const FLT x253 = x13 * (1. / (x10 * sqrt(x10))); const FLT x254 = x252 * x253; const FLT x255 = x251 * x254; const FLT x256 = 2 * x16; const FLT x257 = x1 * x256; const FLT x258 = 2 * x14 * (1. / (x10 * x10)); const FLT x259 = x251 * x258; const FLT x260 = x2 * x4; const FLT x261 = x8 * x255; const FLT x262 = x20 * x252; const FLT x263 = x1 * x4; const FLT x264 = x8 * x259; const FLT x265 = (x4 * x261) + (x4 * x257) + (-1 * x260 * x259) + (-1 * x4 * x264) + (-1 * x262 * x263) + (x250 * x255) + (-1 * x250 * x259) + (x260 * x255); const FLT x266 = 1.0/2.0 * (1. / (x18 * sqrt(x18))); const FLT x267 = dt * x20 * x266; const FLT x268 = x267 * x265; const FLT x269 = x7 * x39; const FLT x270 = x4 * x268; const FLT x271 = x42 * x268; const FLT x272 = 0.5 * x21; const FLT x273 = x32 * x272; const FLT x274 = x19 * x253; const FLT x275 = x34 * x274; const FLT x276 = x17 * x266; const FLT x277 = x276 * x265; const FLT x278 = x39 * x274; const FLT x279 = x4 * x7; const FLT x280 = x279 * x246; const FLT x281 = x46 * x248; const FLT x282 = (-1 * x280 * x281) + (x278 * x280); const FLT x283 = x42 * x274; const FLT x284 = x0 * x4; const FLT x285 = x284 * x246; const FLT x286 = x38 * x248; const FLT x287 = (x286 * x285) + (-1 * x283 * x285); const FLT x288 = (-1 * x32 * x277) + (-1 * x275 * x247) + x282 + (x268 * x269) + (-1 * x34 * x270) + x287 + x35 + (x49 * x249) + (-1 * x0 * x271) + (-1 * x273 * x263); const FLT x289 = x7 * x34; const FLT x290 = x275 * x246; const FLT x291 = x279 * x290; const FLT x292 = x0 * x268; const FLT x293 = x49 * x248; const FLT x294 = x293 * x280; const FLT x295 = x42 * x272; const FLT x296 = -1 * x40; const FLT x297 = x32 * x274; const FLT x298 = x44 * x248; const FLT x299 = (-1 * x298 * x285) + (x297 * x285); const FLT x300 = x299 + (-1 * x295 * x263) + x296 + (x278 * x247) + (-1 * x42 * x277) + (-1 * x294) + (-1 * x46 * x249) + (x268 * x289) + x291 + (x39 * x270) + (x32 * x292); const FLT x301 = x34 * x272; const FLT x302 = x285 * x281; const FLT x303 = x32 * x268; const FLT x304 = x278 * x285; const FLT x305 = -1 * x33; const FLT x306 = (-1 * x283 * x280) + (x286 * x280); const FLT x307 = (-1 * x39 * x292) + (-1 * x304) + (-1 * x263 * x301) + x306 + (x4 * x303) + x305 + (-1 * x7 * x271) + (-1 * x298 * x247) + (x297 * x247) + x302 + (-1 * x34 * x277); const FLT x308 = (x307 * sensor_pt[2]) + (-1 * x288 * sensor_pt[1]) + (x300 * sensor_pt[0]); const FLT x309 = x39 * x272; const FLT x310 = x298 * x280; const FLT x311 = x290 * x284; const FLT x312 = x297 * x280; const FLT x313 = x293 * x285; const FLT x314 = x310 + (-1 * x263 * x309) + x43 + (-1 * x7 * x303) + (-1 * x39 * x277) + (x34 * x292) + (-1 * x283 * x247) + (-1 * x313) + (-1 * x4 * x271) + x311 + (x38 * x249) + (-1 * x312); const FLT x315 = (x288 * sensor_pt[0]) + (-1 * x314 * sensor_pt[2]) + (x300 * sensor_pt[1]); const FLT x316 = (x315 * x146) + (-1 * x307 * x153) + (-1 * x308 * x157) + (x314 * x159); const FLT x317 = (x314 * sensor_pt[1]) + (-1 * x307 * sensor_pt[0]) + (x300 * sensor_pt[2]); const FLT x318 = (x308 * x155) + (x288 * x153) + (-1 * x314 * x130) + (-1 * x317 * x146); const FLT x319 = (x317 * x157) + (-1 * x288 * x159) + (-1 * x315 * x155) + (x307 * x130); const FLT x320 = (x319 * (*lh_p).Rot[3]) + (-1 * x316 * (*lh_p).Rot[1]) + (x318 * (*lh_p).Rot[0]); const FLT x321 = (x318 * (*lh_p).Rot[1]) + (-1 * x319 * (*lh_p).Rot[2]) + (x316 * (*lh_p).Rot[0]); const FLT x322 = x319 + (-1 * x320 * x163) + (x321 * x165); const FLT x323 = (x316 * (*lh_p).Rot[2]) + (-1 * x318 * (*lh_p).Rot[3]) + (x319 * (*lh_p).Rot[0]); const FLT x324 = (-1 * x323 * x165) + x316 + (x320 * x168); const FLT x325 = x318 + (-1 * x321 * x168) + (x323 * x163); const FLT x326 = (-1 * x92 * ((-1 * ((x90 * x324) + (x89 * x325)) * x91) + (x88 * x322))) + (-1 * ((x61 * x325) + (-1 * x81 * x324)) * x87); const FLT x327 = x1 * x7; const FLT x328 = x8 * x246; const FLT x329 = x7 * x255; const FLT x330 = x7 * x259; const FLT x331 = (x7 * x7 * x7) * x251; const FLT x332 = (x256 * x327) + (-1 * x258 * x331) + (x254 * x331) + (-1 * x5 * x330) + (x5 * x329) + (-1 * x262 * x327) + (-1 * x2 * x330) + (x2 * x329); const FLT x333 = x32 * x332; const FLT x334 = x267 * x333; const FLT x335 = x4 * x267; const FLT x336 = x42 * x332; const FLT x337 = x276 * x332; const FLT x338 = x267 * x332; const FLT x339 = x0 * x7; const FLT x340 = x246 * x339; const FLT x341 = (x290 * x339) + (-1 * x293 * x340); const FLT x342 = x341 + x33 + (-1 * x39 * x337) + (x298 * x328) + (-1 * x336 * x335) + (-1 * x309 * x327) + (x0 * x34 * x338) + (-1 * x7 * x334) + x306 + (-1 * x297 * x328); const FLT x343 = x39 * x332; const FLT x344 = x267 * x343; const FLT x345 = x267 * x336; const FLT x346 = (x286 * x340) + (-1 * x283 * x340); const FLT x347 = (-1 * x281 * x328) + (-1 * x0 * x345) + x346 + (-1 * x32 * x337) + x294 + (-1 * x273 * x327) + (-1 * x291) + (x7 * x344) + (-1 * x34 * x335 * x332) + x296 + (x278 * x328); const FLT x348 = -1 * x35; const FLT x349 = x298 * x340; const FLT x350 = x297 * x340; const FLT x351 = x282 + (x335 * x343) + (x289 * x338) + (-1 * x42 * x337) + (-1 * x295 * x327) + (x8 * x290) + (-1 * x293 * x328) + x348 + (x0 * x334) + (-1 * x349) + x350; const FLT x352 = x281 * x340; const FLT x353 = x278 * x340; const FLT x354 = (x335 * x333) + (-1 * x301 * x327) + (x286 * x328) + x43 + (-1 * x353) + (-1 * x34 * x337) + (-1 * x7 * x345) + (-1 * x0 * x344) + (-1 * x283 * x328) + x352 + (-1 * x310) + x312; const FLT x355 = (x354 * sensor_pt[2]) + (-1 * x347 * sensor_pt[1]) + (x351 * sensor_pt[0]); const FLT x356 = (x342 * sensor_pt[1]) + (-1 * x354 * sensor_pt[0]) + (x351 * sensor_pt[2]); const FLT x357 = (-1 * x356 * x146) + (x355 * x155) + (-1 * x342 * x130) + (x347 * x153); const FLT x358 = (x347 * sensor_pt[0]) + (-1 * x342 * sensor_pt[2]) + (x351 * sensor_pt[1]); const FLT x359 = (-1 * x354 * x153) + (x358 * x146) + (-1 * x355 * x157) + (x342 * x159); const FLT x360 = (x354 * x130) + (x356 * x157) + (-1 * x358 * x155) + (-1 * x347 * x159); const FLT x361 = (x360 * (*lh_p).Rot[3]) + (x357 * (*lh_p).Rot[0]) + (-1 * x359 * (*lh_p).Rot[1]); const FLT x362 = (x357 * (*lh_p).Rot[1]) + (-1 * x360 * (*lh_p).Rot[2]) + (x359 * (*lh_p).Rot[0]); const FLT x363 = x360 + (-1 * x361 * x163) + (x362 * x165); const FLT x364 = (-1 * x357 * (*lh_p).Rot[3]) + (x359 * (*lh_p).Rot[2]) + (x360 * (*lh_p).Rot[0]); const FLT x365 = x359 + (-1 * x364 * x165) + (x361 * x168); const FLT x366 = (-1 * x362 * x168) + x357 + (x364 * x163); const FLT x367 = (-1 * x92 * ((-1 * ((x90 * x365) + (x89 * x366)) * x91) + (x88 * x363))) + (-1 * ((x61 * x366) + (-1 * x81 * x365)) * x87); const FLT x368 = x2 * x246; const FLT x369 = x0 * x5; const FLT x370 = x0 * x0 * x0; const FLT x371 = x0 * x1; const FLT x372 = (-1 * x262 * x371) + (-1 * x259 * x370) + (x255 * x370) + (-1 * x0 * x264) + (x0 * x261) + (x255 * x369) + (x0 * x257) + (-1 * x259 * x369); const FLT x373 = x267 * x372; const FLT x374 = x42 * x373; const FLT x375 = x276 * x372; const FLT x376 = x372 * x335; const FLT x377 = (-1 * x34 * x376) + (x269 * x373) + x43 + x353 + x313 + (x286 * x368) + (-1 * x0 * x374) + (-1 * x273 * x371) + (-1 * x311) + (-1 * x283 * x368) + (-1 * x352) + (-1 * x32 * x375); const FLT x378 = x32 * x373; const FLT x379 = x341 + (x39 * x376) + x304 + (-1 * x302) + (x289 * x373) + x305 + (-1 * x295 * x371) + (x0 * x378) + (x297 * x368) + (-1 * x42 * x375) + (-1 * x298 * x368); const FLT x380 = x0 * x373; const FLT x381 = x346 + (-1 * x39 * x380) + x40 + (-1 * x34 * x375) + (-1 * x7 * x374) + x299 + (-1 * x371 * x301) + (-1 * x278 * x368) + (x281 * x368) + (x32 * x376); const FLT x382 = (x381 * sensor_pt[2]) + (-1 * x377 * sensor_pt[1]) + (x379 * sensor_pt[0]); const FLT x383 = (-1 * x293 * x368) + (x34 * x380) + (-1 * x371 * x309) + x287 + (-1 * x350) + x349 + x348 + (-1 * x42 * x376) + (-1 * x39 * x375) + (x2 * x290) + (-1 * x7 * x378); const FLT x384 = (x377 * sensor_pt[0]) + (-1 * x383 * sensor_pt[2]) + (x379 * sensor_pt[1]); const FLT x385 = (-1 * x381 * x153) + (x383 * x159) + (-1 * x382 * x157) + (x384 * x146); const FLT x386 = (x383 * sensor_pt[1]) + (-1 * x381 * sensor_pt[0]) + (x379 * sensor_pt[2]); const FLT x387 = (x382 * x155) + (x377 * x153) + (-1 * x383 * x130) + (-1 * x386 * x146); const FLT x388 = (x381 * x130) + (x386 * x157) + (-1 * x384 * x155) + (-1 * x377 * x159); const FLT x389 = (-1 * x385 * (*lh_p).Rot[1]) + (x388 * (*lh_p).Rot[3]) + (x387 * (*lh_p).Rot[0]); const FLT x390 = (x387 * (*lh_p).Rot[1]) + (-1 * x388 * (*lh_p).Rot[2]) + (x385 * (*lh_p).Rot[0]); const FLT x391 = x388 + (-1 * x389 * x163) + (x390 * x165); const FLT x392 = (x385 * (*lh_p).Rot[2]) + (-1 * x387 * (*lh_p).Rot[3]) + (x388 * (*lh_p).Rot[0]); const FLT x393 = x385 + (-1 * x392 * x165) + (x389 * x168); const FLT x394 = x387 + (x392 * x163) + (-1 * x390 * x168); const FLT x395 = (-1 * x92 * ((-1 * ((x90 * x393) + (x89 * x394)) * x91) + (x88 * x391))) + (-1 * ((x61 * x394) + (-1 * x81 * x393)) * x87); const FLT x396 = -1 * dt * x222; const FLT x397 = (-1 * dt * x224) + dt; const FLT x398 = x397 + x396; const FLT x399 = dt * x227; const FLT x400 = dt * x228; const FLT x401 = x400 + (-1 * x399); const FLT x402 = dt * x230; const FLT x403 = dt * x231; const FLT x404 = x403 + x402; const FLT x405 = (-1 * x92 * ((-1 * ((x90 * x401) + (x89 * x404)) * x91) + (x88 * x398))) + (-1 * ((x61 * x404) + (-1 * x81 * x401)) * x87); const FLT x406 = x402 + (-1 * x403); const FLT x407 = dt * x235; const FLT x408 = dt * x236; const FLT x409 = x408 + x407; const FLT x410 = -1 * dt * x238; const FLT x411 = x397 + x410; const FLT x412 = (-1 * x92 * ((-1 * ((x90 * x409) + (x89 * x411)) * x91) + (x88 * x406))) + (-1 * ((x61 * x411) + (-1 * x81 * x409)) * x87); const FLT x413 = x399 + x400; const FLT x414 = dt + x396 + x410; const FLT x415 = x407 + (-1 * x408); const FLT x416 = (-1 * x92 * ((-1 * ((x90 * x414) + (x89 * x415)) * x91) + (x88 * x413))) + (-1 * ((x61 * x415) + (-1 * x81 * x414)) * x87); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x93 + (((x71 * x76) + (-1 * x61 * x66)) * x79) + (x93 * x94)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x102 + (((x76 * x98) + (-1 * x61 * x95)) * x79) + (x94 * x102)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), (((x76 * x104) + (-1 * x61 * x103)) * x79) + x106 + (x94 * x106)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x171 + (((x76 * x169) + (-1 * x61 * x166)) * x79) + (x94 * x171)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x197 + (((x76 * x195) + (-1 * x61 * x193)) * x79) + (x94 * x197)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x221 + (x94 * x221) + (((x76 * x217) + (-1 * x61 * x220)) * x79)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), x233 + (((x76 * x229) + (-1 * x61 * x226)) * x79) + (x94 * x233)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), x241 + (((x76 * x237) + (-1 * x61 * x234)) * x79) + (x94 * x241)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), x245 + (((x76 * x243) + (-1 * x61 * x242)) * x79) + (x94 * x245)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (((x76 * x324) + (-1 * x61 * x322)) * x79) + x326 + (x94 * x326)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (((x76 * x365) + (-1 * x61 * x363)) * x79) + x367 + (x94 * x367)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (((x76 * x393) + (-1 * x61 * x391)) * x79) + x395 + (x94 * x395)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), x405 + (((x76 * x401) + (-1 * x61 * x398)) * x79) + (x94 * x405)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), x412 + (((x76 * x409) + (-1 * x61 * x406)) * x79) + (x94 * x412)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), x416 + (((x76 * x414) + (-1 * x61 * x413)) * x79) + (x94 * x416)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x8 * x18) + (x6 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x21 * x15; const FLT x24 = x20 * x19; const FLT x25 = x21 * x17; const FLT x26 = (-1 * x7 * x25) + (-1 * x0 * x22) + (x24 * x16) + (-1 * x9 * x23); const FLT x27 = x21 * x16; const FLT x28 = x0 * x27; const FLT x29 = x9 * x25; const FLT x30 = x4 * x24; const FLT x31 = x7 * x23; const FLT x32 = x30 + x28 + x31 + (-1 * x29); const FLT x33 = x9 * x22; const FLT x34 = x24 * x17; const FLT x35 = x0 * x23; const FLT x36 = x7 * x27; const FLT x37 = x36 + (-1 * x35) + x33 + x34; const FLT x38 = (-1 * x37 * sensor_pt[2]) + (x26 * sensor_pt[1]) + (x32 * sensor_pt[0]); const FLT x39 = x9 * x27; const FLT x40 = x24 * x15; const FLT x41 = x0 * x25; const FLT x42 = x7 * x22; const FLT x43 = (-1 * x42) + x41 + x39 + x40; const FLT x44 = (-1 * x43 * sensor_pt[0]) + (x26 * sensor_pt[2]) + (x37 * sensor_pt[1]); const FLT x45 = 1.0/2.0 * dt * fabs(dt); const FLT x46 = (x45 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x43 * x44) + (-1 * x32 * x38))) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x47 = (-1 * x32 * sensor_pt[1]) + (x26 * sensor_pt[0]) + (x43 * sensor_pt[2]); const FLT x48 = (*_x0).Pose.Pos[2] + (x45 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + sensor_pt[2] + (*error_model).Pose.Pos[2] + (2 * ((x38 * x37) + (-1 * x43 * x47))) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x49 = (*_x0).Pose.Pos[1] + (2 * ((x47 * x32) + (-1 * x44 * x37))) + (*error_model).Pose.Pos[1] + (x45 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])); const FLT x50 = (-1 * x49 * (*lh_p).Rot[3]) + (x46 * (*lh_p).Rot[0]) + (x48 * (*lh_p).Rot[2]); const FLT x51 = (-1 * x48 * (*lh_p).Rot[1]) + (x49 * (*lh_p).Rot[0]) + (x46 * (*lh_p).Rot[3]); const FLT x52 = x48 + (2 * ((x51 * (*lh_p).Rot[1]) + (-1 * x50 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x53 = 1. / x52; const FLT x54 = (-1 * x41) + x42 + (-1 * x40) + (-1 * x39); const FLT x55 = 2 * x43; const FLT x56 = 1 + (x54 * x55) + (-2 * (x32 * x32)); const FLT x57 = x55 * x26; const FLT x58 = 2 * x37; const FLT x59 = (x58 * x32) + (-1 * x57); const FLT x60 = 2 * x32; const FLT x61 = x60 * x26; const FLT x62 = x61 + (-1 * x54 * x58); const FLT x63 = (x62 * (*lh_p).Rot[1]) + (-1 * x56 * (*lh_p).Rot[2]) + (x59 * (*lh_p).Rot[0]); const FLT x64 = 2 * (*lh_p).Rot[2]; const FLT x65 = (x56 * (*lh_p).Rot[3]) + (-1 * x59 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]); const FLT x66 = 2 * (*lh_p).Rot[3]; const FLT x67 = x56 + (x63 * x64) + (-1 * x65 * x66); const FLT x68 = 2 * (*lh_p).Rot[1]; const FLT x69 = (x59 * (*lh_p).Rot[2]) + (-1 * x62 * (*lh_p).Rot[3]) + (x56 * (*lh_p).Rot[0]); const FLT x70 = x59 + (x68 * x65) + (-1 * x64 * x69); const FLT x71 = x52 * x52; const FLT x72 = 1. / x71; const FLT x73 = (-1 * x46 * (*lh_p).Rot[2]) + (x48 * (*lh_p).Rot[0]) + (x49 * (*lh_p).Rot[1]); const FLT x74 = x46 + (*lh_p).Pos[0] + (2 * ((x73 * (*lh_p).Rot[2]) + (-1 * x51 * (*lh_p).Rot[3]))); const FLT x75 = x72 * x74; const FLT x76 = -1 * x52; const FLT x77 = x74 * x74; const FLT x78 = 2 * (1. / (x71 + x77)) * x71 * atan2(x74, x76) * (*bsc0).curve; const FLT x79 = x49 + (*lh_p).Pos[1] + (2 * ((x50 * (*lh_p).Rot[3]) + (-1 * x73 * (*lh_p).Rot[1]))); const FLT x80 = x72 * x79; const FLT x81 = x62 + (x66 * x69) + (-1 * x63 * x68); const FLT x82 = x71 + (x79 * x79); const FLT x83 = 1. / x82; const FLT x84 = x83 * x71; const FLT x85 = (1. / sqrt(x82)) * (*bsc0).tilt; const FLT x86 = 2 * x79; const FLT x87 = 2 * x52; const FLT x88 = 1.0/2.0 * (1. / (x82 * sqrt(x82))) * x74 * (*bsc0).tilt; const FLT x89 = 1. / sqrt(1 + (-1 * x83 * x77 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x90 = (-1 * x89 * ((-1 * ((x87 * x70) + (x81 * x86)) * x88) + (x85 * x67))) + (-1 * ((x81 * x53) + (-1 * x80 * x70)) * x84); const FLT x91 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x85 * x74)) + (-1 * atan2(-1 * x79, x76))); const FLT x92 = (-1 * x28) + (-1 * x31) + x29 + (-1 * x30); const FLT x93 = x58 * x26; const FLT x94 = x93 + (-1 * x55 * x92); const FLT x95 = 1 + (x60 * x92) + (-2 * (x37 * x37)); const FLT x96 = (x55 * x37) + (-1 * x61); const FLT x97 = (x96 * (*lh_p).Rot[3]) + (-1 * x94 * (*lh_p).Rot[1]) + (x95 * (*lh_p).Rot[0]); const FLT x98 = (x94 * (*lh_p).Rot[2]) + (-1 * x95 * (*lh_p).Rot[3]) + (x96 * (*lh_p).Rot[0]); const FLT x99 = (x68 * x97) + x94 + (-1 * x64 * x98); const FLT x100 = (x95 * (*lh_p).Rot[1]) + (x94 * (*lh_p).Rot[0]) + (-1 * x96 * (*lh_p).Rot[2]); const FLT x101 = x95 + (x66 * x98) + (-1 * x68 * x100); const FLT x102 = x96 + (x64 * x100) + (-1 * x66 * x97); const FLT x103 = (-1 * x89 * ((-1 * x88 * ((x87 * x99) + (x86 * x101))) + (x85 * x102))) + (-1 * x84 * ((x53 * x101) + (-1 * x80 * x99))); const FLT x104 = x35 + (-1 * x36) + (-1 * x34) + (-1 * x33); const FLT x105 = x57 + (-1 * x60 * x104); const FLT x106 = 1 + (x58 * x104) + (-2 * (x43 * x43)); const FLT x107 = (x55 * x32) + (-1 * x93); const FLT x108 = (x107 * (*lh_p).Rot[1]) + (-1 * x105 * (*lh_p).Rot[2]) + (x106 * (*lh_p).Rot[0]); const FLT x109 = (-1 * x106 * (*lh_p).Rot[1]) + (x105 * (*lh_p).Rot[3]) + (x107 * (*lh_p).Rot[0]); const FLT x110 = x105 + (x64 * x108) + (-1 * x66 * x109); const FLT x111 = (x106 * (*lh_p).Rot[2]) + (-1 * x107 * (*lh_p).Rot[3]) + (x105 * (*lh_p).Rot[0]); const FLT x112 = (x68 * x109) + x106 + (-1 * x64 * x111); const FLT x113 = x107 + (x66 * x111) + (-1 * x68 * x108); const FLT x114 = (-1 * x89 * ((-1 * ((x87 * x112) + (x86 * x113)) * x88) + (x85 * x110))) + (-1 * ((x53 * x113) + (-1 * x80 * x112)) * x84); cnMatrixOptionalSet(Hx, 0, 0, (((x70 * x75) + (-1 * x67 * x53)) * x78) + x90 + (x91 * x90)); cnMatrixOptionalSet(Hx, 0, 1, x103 + (x91 * x103) + (x78 * ((x75 * x99) + (-1 * x53 * x102)))); cnMatrixOptionalSet(Hx, 0, 2, x114 + (((x75 * x112) + (-1 * x53 * x110)) * x78) + (x91 * x114)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x1 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x3 = 0.5 * (*_x0).Pose.Rot[1]; const FLT x4 = (x3 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*error_model).Pose.AxisAngleRot[0]) + (x2 * (*error_model).Pose.AxisAngleRot[2]); const FLT x5 = dt * dt; const FLT x6 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x0 * x0) * 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).Pose.Rot[3]; const FLT x16 = (x2 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x3 * (*error_model).Pose.AxisAngleRot[2]) + (x15 * (*error_model).Pose.AxisAngleRot[0]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x3 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[0] + (-1 * x15 * (*error_model).Pose.AxisAngleRot[2]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x15 * (*error_model).Pose.AxisAngleRot[1]) + (x1 * (*error_model).Pose.AxisAngleRot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x4 * x4) + (x16 * x16))) * (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 = x20 * x21; const FLT x25 = x22 * x16; const FLT x26 = x8 * x22; const FLT x27 = (-1 * x6 * x25) + (x26 * x17) + (x0 * x23) + (x24 * x18); const FLT x28 = (-1 * x26 * x18) + (x24 * x17) + (-1 * x6 * x23) + (-1 * x0 * x25); const FLT x29 = x22 * x17; const FLT x30 = x22 * x18; const FLT x31 = (-1 * x4 * x26) + (x6 * x30) + (x0 * x29) + (x24 * x16); const FLT x32 = (-1 * x31 * sensor_pt[0]) + (x28 * sensor_pt[2]) + (x27 * sensor_pt[1]); const FLT x33 = (x26 * x16) + (x4 * x24) + (x6 * x29) + (-1 * x0 * x30); const FLT x34 = (-1 * x33 * sensor_pt[1]) + (x28 * sensor_pt[0]) + (x31 * sensor_pt[2]); const FLT x35 = 2 * ((x34 * x33) + (-1 * x32 * x27)); const FLT x36 = dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1]); const FLT x37 = 1.0/2.0 * dt * fabs(dt); const FLT x38 = x37 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x39 = (*_x0).Pose.Pos[1] + x35 + (*error_model).Pose.Pos[1] + sensor_pt[1] + x36 + x38; const FLT x40 = x39 * (*lh_p).Rot[0]; const FLT x41 = (-1 * x27 * sensor_pt[2]) + (x28 * sensor_pt[1]) + (x33 * sensor_pt[0]); const FLT x42 = 2 * ((x32 * x31) + (-1 * x41 * x33)); const FLT x43 = dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0]); const FLT x44 = x37 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x45 = x44 + x42 + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + x43; const FLT x46 = x45 * (*lh_p).Rot[3]; const FLT x47 = x37 * ((*_x0).Acc[2] + (*error_model).Acc[2]); const FLT x48 = 2 * ((x41 * x27) + (-1 * x31 * x34)); const FLT x49 = dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2]); const FLT x50 = (*_x0).Pose.Pos[2] + x47 + sensor_pt[2] + (*error_model).Pose.Pos[2] + x49 + x48; const FLT x51 = x50 * (*lh_p).Rot[1]; const FLT x52 = (-1 * x51) + x40 + x46; const FLT x53 = x50 * (*lh_p).Rot[0]; const FLT x54 = x39 * (*lh_p).Rot[1]; const FLT x55 = x45 * (*lh_p).Rot[2]; const FLT x56 = (-1 * x55) + x53 + x54; const FLT x57 = x45 + (*lh_p).Pos[0] + (2 * ((x56 * (*lh_p).Rot[2]) + (-1 * x52 * (*lh_p).Rot[3]))); const FLT x58 = x45 * (*lh_p).Rot[0]; const FLT x59 = x50 * (*lh_p).Rot[2]; const FLT x60 = x39 * (*lh_p).Rot[3]; const FLT x61 = (-1 * x60) + x58 + x59; const FLT x62 = x50 + (2 * ((x52 * (*lh_p).Rot[1]) + (-1 * x61 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x63 = x62 * x62; const FLT x64 = x39 + (*lh_p).Pos[1] + (2 * ((x61 * (*lh_p).Rot[3]) + (-1 * x56 * (*lh_p).Rot[1]))); const FLT x65 = (x64 * x64) + x63; const FLT x66 = (1. / sqrt(x65)) * (*bsc0).tilt; const FLT x67 = -1 * x62; const FLT x68 = (*bsc0).gibmag * sin(1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x66 * x57)) + (-1 * atan2(-1 * x64, x67))); const FLT x69 = 1. / x65; const FLT x70 = x57 * x57; const FLT x71 = 1. / sqrt(1 + (-1 * x70 * x69 * ((*bsc0).tilt * (*bsc0).tilt))); const FLT x72 = x71 * x66; const FLT x73 = 2 * x62; const FLT x74 = (1. / (x63 + x70)) * atan2(x57, x67) * (*bsc0).curve; const FLT x75 = (1. / (x65 * sqrt(x65))) * x57 * (*bsc0).tilt; const FLT x76 = x71 * x75; const FLT x77 = (x76 * x64) + (-1 * x62 * x69); const FLT x78 = 2 * x74; const FLT x79 = (x76 * x62) + (x64 * x69); const FLT x80 = 1. / x62; const FLT x81 = 2 * x60; const FLT x82 = (2 * x59) + (-1 * x81); const FLT x83 = 2 * x55; const FLT x84 = (2 * x54) + (-1 * x83); const FLT x85 = 1. / x63; const FLT x86 = x85 * x57; const FLT x87 = x78 * x63; const FLT x88 = x85 * x64; const FLT x89 = 2 * x51; const FLT x90 = (2 * x46) + (-1 * x89); const FLT x91 = x63 * x69; const FLT x92 = 2 * x64; const FLT x93 = 1.0/2.0 * x75; const FLT x94 = (-1 * x71 * ((-1 * ((x84 * x73) + (x92 * x90)) * x93) + (x82 * x66))) + (-1 * ((x80 * x90) + (-1 * x88 * x84)) * x91); const FLT x95 = 2 * ((-1 * sensor_pt[2]) + (-1 * x49) + (-1 * (*error_model).Pose.Pos[2]) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x47) + (-1 * x48)); const FLT x96 = 2 * x40; const FLT x97 = x90 + (x95 * (*lh_p).Rot[1]) + x96; const FLT x98 = 2 * x53; const FLT x99 = (-1 * x98) + x83 + (-4 * x54); const FLT x100 = 2 * (*lh_p).Rot[2]; const FLT x101 = (x39 * x100) + (-1 * x95 * (*lh_p).Rot[3]); const FLT x102 = (-1 * x71 * ((-1 * ((x73 * x97) + (x92 * x99)) * x93) + (x66 * x101))) + (-1 * ((x80 * x99) + (-1 * x88 * x97)) * x91); const FLT x103 = (-1 * sensor_pt[0]) + (-1 * (*_x0).Pose.Pos[0]) + (-1 * x42) + (-1 * x43) + (-1 * x44) + (-1 * (*error_model).Pose.Pos[0]); const FLT x104 = x84 + (x100 * x103) + x98; const FLT x105 = 2 * x58; const FLT x106 = x81 + (-1 * x105) + (-4 * x59); const FLT x107 = 2 * (*lh_p).Rot[1]; const FLT x108 = 2 * (*lh_p).Rot[3]; const FLT x109 = (x50 * x108) + (-1 * x103 * x107); const FLT x110 = (-1 * x71 * ((-1 * ((x73 * x106) + (x92 * x109)) * x93) + (x66 * x104))) + (-1 * ((x80 * x109) + (-1 * x88 * x106)) * x91); const FLT x111 = (-1 * x96) + x89 + (-4 * x46); const FLT x112 = (-1 * x36) + (-1 * (*error_model).Pose.Pos[1]) + (-1 * sensor_pt[1]) + (-1 * x38) + (-1 * x35) + (-1 * (*_x0).Pose.Pos[1]); const FLT x113 = (x45 * x107) + (-1 * x100 * x112); const FLT x114 = x82 + x105 + (x108 * x112); const FLT x115 = (-1 * x71 * ((-1 * ((x73 * x113) + (x92 * x114)) * x93) + (x66 * x111))) + (-1 * ((x80 * x114) + (-1 * x88 * x113)) * x91); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), (-1 * x72) + (-1 * x72 * x68) + (-1 * x73 * x74)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), x77 + (x77 * x68)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x79 + (x78 * x57) + (x79 * x68)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x94 + (((x84 * x86) + (-1 * x80 * x82)) * x87) + (x68 * x94)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x102 + (x68 * x102) + (x87 * ((x86 * x97) + (-1 * x80 * x101)))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x110 + (((x86 * x106) + (-1 * x80 * x104)) * x87) + (x68 * x110)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (((x86 * x113) + (-1 * x80 * x111)) * x87) + x115 + (x68 * x115)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen1_jac_lh_p(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x0 * x0) * 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 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x7 * x18) + (x9 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x20 * x19; const FLT x24 = x21 * x15; const FLT x25 = x21 * x16; const FLT x26 = (x8 * x25) + (-1 * x6 * x24) + (x0 * x22) + (x23 * x17); const FLT x27 = x21 * x17; const FLT x28 = (-1 * x8 * x27) + (x23 * x16) + (-1 * x6 * x22) + (-1 * x0 * x24); const FLT x29 = (x6 * x27) + (x0 * x25) + (-1 * x8 * x22) + (x23 * x15); const FLT x30 = (-1 * x29 * sensor_pt[0]) + (x28 * sensor_pt[2]) + (x26 * sensor_pt[1]); const FLT x31 = (x4 * x23) + (x6 * x25) + (x8 * x24) + (-1 * x0 * x27); const FLT x32 = (-1 * x31 * sensor_pt[1]) + (x28 * sensor_pt[0]) + (x29 * sensor_pt[2]); const FLT x33 = 1.0/2.0 * dt * fabs(dt); const FLT x34 = (*_x0).Pose.Pos[1] + (2 * ((x32 * x31) + (-1 * x30 * x26))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x33 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x35 = (-1 * x26 * sensor_pt[2]) + (x28 * sensor_pt[1]) + (x31 * sensor_pt[0]); const FLT x36 = (x33 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x30 * x29) + (-1 * x31 * x35))) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x37 = (*_x0).Pose.Pos[2] + (*error_model).Pose.Pos[2] + (2 * ((x35 * x26) + (-1 * x32 * x29))) + sensor_pt[2] + (x33 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x38 = (x34 * (*lh_p).Rot[0]) + (-1 * x37 * (*lh_p).Rot[1]) + (x36 * (*lh_p).Rot[3]); const FLT x39 = (-1 * x36 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[0]) + (x34 * (*lh_p).Rot[1]); const FLT x40 = x36 + (*lh_p).Pos[0] + (2 * ((x39 * (*lh_p).Rot[2]) + (-1 * x38 * (*lh_p).Rot[3]))); const FLT x41 = (-1 * x34 * (*lh_p).Rot[3]) + (x36 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[2]); const FLT x42 = (2 * ((x38 * (*lh_p).Rot[1]) + (-1 * x41 * (*lh_p).Rot[2]))) + x37 + (*lh_p).Pos[2]; const FLT x43 = -1 * x42; const FLT x44 = x34 + (*lh_p).Pos[1] + (2 * ((x41 * (*lh_p).Rot[3]) + (-1 * x39 * (*lh_p).Rot[1]))); const FLT x45 = (x44 * x44) + (x42 * x42); const FLT x46 = x40 * (1. / sqrt(x45)); const FLT x47 = 1.5707963267949 + (-1 * (*bsc0).phase) + (*bsc0).gibpha + (-1 * asin(x46 * (*bsc0).tilt)) + (-1 * atan2(-1 * x44, x43)); const FLT x48 = sin(x47) * (*bsc0).gibmag; const FLT x49 = x46 * (1. / sqrt(1 + (-1 * (x40 * x40) * (1. / x45) * ((*bsc0).tilt * (*bsc0).tilt)))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), atan2(x40, x43) * atan2(x40, x43)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * cos(x47)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x48); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1 + (-1 * x48)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x48 * x49) + (-1 * x49)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen1 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_y_gen1_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen1_jac_bsc0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanErrorModel_LightMeas_x_gen2(const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*error_model).Pose.AxisAngleRot[2]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x16 = (-1 * x15 * (*_x0).Pose.Rot[1]) + (x2 * (*error_model).Pose.AxisAngleRot[1]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x15 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x15 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x4 * x4) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x6 * x19) + (x8 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x7 * x26) + (x25 * x17) + (-1 * x0 * x23) + (-1 * x9 * x24); const FLT x28 = x22 * x17; const FLT x29 = (x7 * x24) + (x4 * x25) + (x0 * x28) + (-1 * x9 * x26); const FLT x30 = (x7 * x28) + (-1 * x0 * x24) + (x9 * x23) + (x25 * x18); const FLT x31 = (-1 * x30 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x29 * sensor_pt[0]); const FLT x32 = (-1 * x7 * x23) + (x0 * x26) + (x9 * x28) + (x25 * x16); const FLT x33 = (-1 * x32 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x30 * sensor_pt[1]); const FLT x34 = 1.0/2.0 * dt * fabs(dt); const FLT x35 = (x34 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x32 * x33) + (-1 * x31 * x29))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x36 = (-1 * x29 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x32 * sensor_pt[2]); const FLT x37 = (*_x0).Pose.Pos[2] + (x34 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x30 * x31) + (-1 * x32 * x36))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x38 = (*_x0).Pose.Pos[1] + (2 * ((x36 * x29) + (-1 * x30 * x33))) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (*error_model).Pose.Pos[1] + (x34 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x39 = (-1 * x38 * (*lh_p).Rot[3]) + (x35 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[2]); const FLT x40 = (-1 * x37 * (*lh_p).Rot[1]) + (x38 * (*lh_p).Rot[0]) + (x35 * (*lh_p).Rot[3]); const FLT x41 = (2 * ((x40 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + x37 + (*lh_p).Pos[2]; const FLT x42 = (-1 * x35 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[0]) + (x38 * (*lh_p).Rot[1]); const FLT x43 = (*lh_p).Pos[0] + x35 + (2 * ((x42 * (*lh_p).Rot[2]) + (-1 * x40 * (*lh_p).Rot[3]))); const FLT x44 = atan2(-1 * x41, x43); const FLT x45 = (*lh_p).Pos[1] + x38 + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x42 * (*lh_p).Rot[1]))); const FLT x46 = (x43 * x43) + (x41 * x41); const FLT x47 = 0.523598775598299 + (*bsc0).tilt; const FLT x48 = cos(x47); const FLT x49 = asin(x45 * (1. / x48) * (1. / sqrt(x46 + (x45 * x45)))); const FLT x50 = 0.0028679863 + (x49 * (-8.0108022e-06 + (-8.0108022e-06 * x49))); const FLT x51 = 5.3685255e-06 + (x50 * x49); const FLT x52 = 0.0076069798 + (x51 * x49); const FLT x53 = x45 * (1. / sqrt(x46)) * tan(x47); const FLT x54 = ((*bsc0).ogeemag * sin((*bsc0).ogeephase + (-1 * asin(x53)) + x44)) + (*bsc0).curve; const FLT x55 = asin(x53 + (x54 * x52 * (x49 * x49) * (1. / (x48 + (-1 * x54 * sin(x47) * ((x49 * (x52 + (x49 * (x51 + (x49 * (x50 + (x49 * (-8.0108022e-06 + (-1.60216044e-05 * x49))))))))) + (x52 * x49))))))); return -1.5707963267949 + (-1 * x55) + (-1 * sin(x55 + (-1 * x44) + (-1 * (*bsc0).gibpha)) * (*bsc0).gibmag) + x44 + (-1 * (*bsc0).phase); } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x1 = dt * fabs(dt); const FLT x2 = -1 * x0 * x1; const FLT x3 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x4 = 1.0/2.0 * x1; const FLT x5 = x4 + (-1 * x1 * x3); const FLT x6 = x5 + x2; const FLT x7 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x8 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x9 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x10 = (x9 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x7 * (*error_model).Pose.AxisAngleRot[0]) + (x8 * (*_x0).Pose.Rot[0]); const FLT x11 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x12 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x13 = (-1 * x8 * (*_x0).Pose.Rot[1]) + (x9 * (*_x0).Pose.Rot[0]) + (x12 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x14 = (-1 * x12 * (*_x0).Pose.Rot[1]) + (-1 * x7 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[0] + (-1 * x8 * (*_x0).Pose.Rot[3]); const FLT x15 = (x12 * (*_x0).Pose.Rot[0]) + (-1 * x9 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[1] + (x7 * (*error_model).Pose.AxisAngleRot[2]); const FLT x16 = (x15 * x15) + (x14 * x14) + (x10 * x10) + (x13 * x13); const FLT x17 = 1. / sqrt(x16); const FLT x18 = dt * dt; const FLT x19 = x11 * x11; const FLT x20 = x19 * x18; const FLT x21 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x22 = x21 * x21; const FLT x23 = x22 * x18; const FLT x24 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).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 * x30; const FLT x32 = 1. / x27; const FLT x33 = x32 * x31; const FLT x34 = cos(x29); const FLT x35 = (x33 * x23) + (x34 * x34) + (x33 * x20) + (x33 * x26); const FLT x36 = 1. / sqrt(x35); const FLT x37 = x36 * x17; const FLT x38 = x37 * x11; const FLT x39 = x30 * (1. / x28); const FLT x40 = dt * x39; const FLT x41 = x40 * x38; const FLT x42 = x40 * x37; const FLT x43 = x42 * x24; const FLT x44 = x34 * x37; const FLT x45 = x44 * x14; const FLT x46 = x42 * x15; const FLT x47 = (-1 * x46 * x21) + x45 + (-1 * x41 * x10) + (-1 * x43 * x13); const FLT x48 = x44 * x10; const FLT x49 = x42 * x13; const FLT x50 = x48 + (x41 * x14) + (x49 * x21) + (-1 * x43 * x15); const FLT x51 = x44 * x15; const FLT x52 = x42 * x14; const FLT x53 = (-1 * x41 * x13) + (x43 * x10) + (x52 * x21) + x51; const FLT x54 = (x47 * sensor_pt[1]) + (-1 * x53 * sensor_pt[2]) + (x50 * sensor_pt[0]); const FLT x55 = x44 * x13; const FLT x56 = x42 * x10; const FLT x57 = (-1 * x56 * x21) + (x41 * x15) + (x43 * x14) + x55; const FLT x58 = (x47 * sensor_pt[2]) + (-1 * x57 * sensor_pt[0]) + (x53 * sensor_pt[1]); const FLT x59 = (x4 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x58 * x57) + (-1 * x50 * x54))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x60 = (-1 * x50 * sensor_pt[1]) + (x47 * sensor_pt[0]) + (x57 * sensor_pt[2]); const FLT x61 = (*_x0).Pose.Pos[2] + (x4 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + (2 * ((x54 * x53) + (-1 * x60 * x57))) + sensor_pt[2]; const FLT x62 = (*_x0).Pose.Pos[1] + (x4 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + (2 * ((x60 * x50) + (-1 * x53 * x58))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])); const FLT x63 = (-1 * x62 * (*lh_p).Rot[3]) + (x59 * (*lh_p).Rot[0]) + (x61 * (*lh_p).Rot[2]); const FLT x64 = (-1 * x61 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]) + (x59 * (*lh_p).Rot[3]); const FLT x65 = x61 + (2 * ((x64 * (*lh_p).Rot[1]) + (-1 * x63 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x66 = (x61 * (*lh_p).Rot[0]) + (-1 * x59 * (*lh_p).Rot[2]) + (x62 * (*lh_p).Rot[1]); const FLT x67 = x59 + (*lh_p).Pos[0] + (2 * ((x66 * (*lh_p).Rot[2]) + (-1 * x64 * (*lh_p).Rot[3]))); const FLT x68 = x67 * x67; const FLT x69 = (1. / x68) * x65; const FLT x70 = x1 * (*lh_p).Rot[0]; const FLT x71 = x70 * (*lh_p).Rot[2]; const FLT x72 = x1 * (*lh_p).Rot[1]; const FLT x73 = x72 * (*lh_p).Rot[3]; const FLT x74 = x73 + (-1 * x71); const FLT x75 = 1. / x67; const FLT x76 = x68 + (x65 * x65); const FLT x77 = 1. / x76; const FLT x78 = x77 * x68; const FLT x79 = x78 * ((-1 * x75 * x74) + (x6 * x69)); const FLT x80 = 2 * x67; const FLT x81 = 2 * x65; const FLT x82 = (x81 * x74) + (x6 * x80); const FLT x83 = 0.523598775598299 + (*bsc0).tilt; const FLT x84 = tan(x83); const FLT x85 = x62 + (*lh_p).Pos[1] + (2 * ((x63 * (*lh_p).Rot[3]) + (-1 * x66 * (*lh_p).Rot[1]))); const FLT x86 = 1.0/2.0 * x85; const FLT x87 = x84 * x86 * (1. / (x76 * sqrt(x76))); const FLT x88 = x72 * (*lh_p).Rot[2]; const FLT x89 = x70 * (*lh_p).Rot[3]; const FLT x90 = x89 + x88; const FLT x91 = x84 * (1. / sqrt(x76)); const FLT x92 = (x91 * x90) + (-1 * x82 * x87); const FLT x93 = x85 * x85; const FLT x94 = 1. / sqrt(1 + (-1 * (x84 * x84) * x77 * x93)); const FLT x95 = x79 + (-1 * x92 * x94); const FLT x96 = atan2(-1 * x65, x67); const FLT x97 = x85 * x91; const FLT x98 = (*bsc0).ogeephase + (-1 * asin(x97)) + x96; const FLT x99 = cos(x98) * (*bsc0).ogeemag; const FLT x100 = x76 + x93; const FLT x101 = cos(x83); const FLT x102 = 1. / x101; const FLT x103 = (1. / sqrt(x100)) * x102; const FLT x104 = asin(x85 * x103); const FLT x105 = 8.0108022e-06 * x104; const FLT x106 = -8.0108022e-06 + (-1 * x105); const FLT x107 = 0.0028679863 + (x104 * x106); const FLT x108 = 5.3685255e-06 + (x104 * x107); const FLT x109 = 0.0076069798 + (x108 * x104); const FLT x110 = x109 * x104; const FLT x111 = -8.0108022e-06 + (-1.60216044e-05 * x104); const FLT x112 = x107 + (x104 * x111); const FLT x113 = x108 + (x104 * x112); const FLT x114 = x109 + (x104 * x113); const FLT x115 = (x104 * x114) + x110; const FLT x116 = sin(x83); const FLT x117 = (sin(x98) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x118 = x116 * x117; const FLT x119 = x101 + (-1 * x118 * x115); const FLT x120 = 1. / x119; const FLT x121 = x104 * x104; const FLT x122 = x109 * x121; const FLT x123 = x120 * x122; const FLT x124 = x99 * x123; const FLT x125 = x115 * x116; const FLT x126 = x99 * x125; const FLT x127 = 2 * x85; const FLT x128 = x86 * (1. / (x100 * sqrt(x100))) * x102; const FLT x129 = (x90 * x103) + (-1 * x128 * (x82 + (x90 * x127))); const FLT x130 = 1. / sqrt(1 + (-1 * x93 * (1. / (x101 * x101)) * (1. / x100))); const FLT x131 = x113 * x130; const FLT x132 = x112 * x130; const FLT x133 = 2.40324066e-05 * x104; const FLT x134 = x130 * x133; const FLT x135 = x106 * x130; const FLT x136 = x129 * x135; const FLT x137 = x111 * x130; const FLT x138 = x105 * x130; const FLT x139 = x107 * x130; const FLT x140 = (x129 * x139) + (x104 * ((-1 * x129 * x138) + x136)); const FLT x141 = x108 * x130; const FLT x142 = (x104 * x140) + (x129 * x141); const FLT x143 = x114 * x130; const FLT x144 = x109 * x130; const FLT x145 = (1. / (x119 * x119)) * x117 * x122; const FLT x146 = x117 * x120; const FLT x147 = 2 * x110 * x146; const FLT x148 = x130 * x147; const FLT x149 = x121 * x146; const FLT x150 = x97 + (x109 * x149); const FLT x151 = 1. / sqrt(1 + (-1 * (x150 * x150))); const FLT x152 = x151 * ((x142 * x149) + (x129 * x148) + (x95 * x124) + x92 + (-1 * x145 * ((-1 * x118 * ((x129 * x144) + (x104 * x142) + (x104 * ((x129 * x131) + x142 + (x104 * (x140 + (x129 * x132) + (x104 * ((x129 * x137) + (-1 * x129 * x134) + x136)))))) + (x129 * x143))) + (-1 * x95 * x126)))); const FLT x153 = cos(x96 + (-1 * asin(x150)) + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x154 = x88 + (-1 * x89); const FLT x155 = x1 * (*lh_p).Rot[3] * (*lh_p).Rot[2]; const FLT x156 = x70 * (*lh_p).Rot[1]; const FLT x157 = x156 + x155; const FLT x158 = (x81 * x157) + (x80 * x154); const FLT x159 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x160 = -1 * x1 * x159; const FLT x161 = x5 + x160; const FLT x162 = (x91 * x161) + (-1 * x87 * x158); const FLT x163 = ((-1 * x75 * x157) + (x69 * x154)) * x78; const FLT x164 = x99 * (x163 + (-1 * x94 * x162)); const FLT x165 = (x103 * x161) + (-1 * x128 * (x158 + (x127 * x161))); const FLT x166 = x165 * x135; const FLT x167 = (x165 * x139) + (x104 * ((-1 * x165 * x138) + x166)); const FLT x168 = (x104 * x167) + (x165 * x141); const FLT x169 = x151 * (x162 + (x168 * x149) + (x165 * x148) + (-1 * x145 * ((-1 * x118 * ((x165 * x144) + (x165 * x143) + (x104 * ((x165 * x131) + x168 + (x104 * (x167 + (x165 * x132) + (x104 * ((x165 * x137) + (-1 * x165 * x134) + x166)))))) + (x104 * x168))) + (-1 * x125 * x164))) + (x123 * x164)); const FLT x170 = x155 + (-1 * x156); const FLT x171 = x71 + x73; const FLT x172 = x2 + x4 + x160; const FLT x173 = (x81 * x172) + (x80 * x171); const FLT x174 = (x103 * x170) + (-1 * x128 * (x173 + (x127 * x170))); const FLT x175 = x174 * x130; const FLT x176 = (x91 * x170) + (-1 * x87 * x173); const FLT x177 = ((-1 * x75 * x172) + (x69 * x171)) * x78; const FLT x178 = x177 + (-1 * x94 * x176); const FLT x179 = x174 * x135; const FLT x180 = (x174 * x139) + (x104 * ((-1 * x174 * x138) + x179)); const FLT x181 = (x104 * x180) + (x174 * x141); const FLT x182 = x151 * (x176 + (x181 * x149) + (x124 * x178) + (x175 * x147) + (-1 * x145 * ((-1 * x118 * ((x104 * (x181 + (x174 * x131) + (x104 * (x180 + (x174 * x132) + (x104 * ((x174 * x137) + (-1 * x175 * x133) + x179)))))) + (x174 * x144) + (x104 * x181) + (x174 * x143))) + (-1 * x126 * x178)))); const FLT x183 = 2 * x3; const FLT x184 = -1 * x183; const FLT x185 = 2 * x0; const FLT x186 = -1 * x185; const FLT x187 = 1 + x186 + x184; const FLT x188 = 2 * (*lh_p).Rot[2]; const FLT x189 = x188 * (*lh_p).Rot[0]; const FLT x190 = 2 * (*lh_p).Rot[3]; const FLT x191 = x190 * (*lh_p).Rot[1]; const FLT x192 = x191 + (-1 * x189); const FLT x193 = (x81 * x192) + (x80 * x187); const FLT x194 = x188 * (*lh_p).Rot[1]; const FLT x195 = x190 * (*lh_p).Rot[0]; const FLT x196 = x195 + x194; const FLT x197 = (x91 * x196) + (-1 * x87 * x193); const FLT x198 = ((-1 * x75 * x192) + (x69 * x187)) * x78; const FLT x199 = x198 + (-1 * x94 * x197); const FLT x200 = (x103 * x196) + (-1 * x128 * (x193 + (x127 * x196))); const FLT x201 = x200 * x135; const FLT x202 = (x200 * x139) + (x104 * ((-1 * x200 * x138) + x201)); const FLT x203 = (x202 * x104) + (x200 * x141); const FLT x204 = x151 * ((-1 * x145 * ((-1 * x118 * ((x200 * x144) + (x203 * x104) + (x104 * (x203 + (x200 * x131) + (x104 * (x202 + (x200 * x132) + (x104 * ((x200 * x137) + x201 + (-1 * x200 * x134))))))) + (x200 * x143))) + (-1 * x126 * x199))) + x197 + (x203 * x149) + (x124 * x199) + (x200 * x148)); const FLT x205 = 2 * x159; const FLT x206 = 1 + (-1 * x205); const FLT x207 = x206 + x184; const FLT x208 = x194 + (-1 * x195); const FLT x209 = x190 * (*lh_p).Rot[2]; const FLT x210 = 2 * (*lh_p).Rot[1]; const FLT x211 = x210 * (*lh_p).Rot[0]; const FLT x212 = x211 + x209; const FLT x213 = (x81 * x212) + (x80 * x208); const FLT x214 = (x207 * x103) + (-1 * x128 * (x213 + (x207 * x127))); const FLT x215 = (x91 * x207) + (-1 * x87 * x213); const FLT x216 = ((-1 * x75 * x212) + (x69 * x208)) * x78; const FLT x217 = x216 + (-1 * x94 * x215); const FLT x218 = x214 * x135; const FLT x219 = (x214 * x139) + (x104 * ((-1 * x214 * x138) + x218)); const FLT x220 = (x219 * x104) + (x214 * x141); const FLT x221 = x151 * (x215 + (-1 * x145 * ((-1 * x118 * ((x220 * x104) + (x104 * (x220 + (x214 * x131) + (x104 * ((x214 * x132) + x219 + (x104 * ((x214 * x137) + x218 + (-1 * x214 * x134))))))) + (x214 * x144) + (x214 * x143))) + (-1 * x217 * x126))) + (x214 * x148) + (x220 * x149) + (x217 * x124)); const FLT x222 = x189 + x191; const FLT x223 = x206 + x186; const FLT x224 = ((-1 * x75 * x223) + (x69 * x222)) * x78; const FLT x225 = (x81 * x223) + (x80 * x222); const FLT x226 = x209 + (-1 * x211); const FLT x227 = (x91 * x226) + (-1 * x87 * x225); const FLT x228 = x224 + (-1 * x94 * x227); const FLT x229 = (x226 * x103) + (-1 * x128 * (x225 + (x226 * x127))); const FLT x230 = x229 * x135; const FLT x231 = (x229 * x139) + (x104 * ((-1 * x229 * x138) + x230)); const FLT x232 = (x231 * x104) + (x229 * x141); const FLT x233 = x151 * (x227 + (x232 * x149) + (x228 * x124) + (x229 * x148) + (-1 * x145 * ((-1 * x118 * ((x229 * x144) + (x232 * x104) + (x229 * x143) + (x104 * (x232 + (x229 * x131) + (x104 * (x231 + (x229 * x132) + (x104 * ((x229 * x137) + x230 + (-1 * x229 * x134))))))))) + (-1 * x228 * x126)))); const FLT x234 = x8 * x43; const FLT x235 = x44 * x12; const FLT x236 = 1.0/2.0 * x40; const FLT x237 = x11 * x236; const FLT x238 = x13 * x237; const FLT x239 = 1.0 * x10; const FLT x240 = 1.0 * x15; const FLT x241 = 1.0 * x13; const FLT x242 = (x240 * (*error_model).Pose.AxisAngleRot[0]) + (x241 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x14) + (x239 * (*error_model).Pose.AxisAngleRot[2]); const FLT x243 = x36 * (1. / (x16 * sqrt(x16))); const FLT x244 = x242 * x243; const FLT x245 = x24 * x10; const FLT x246 = x236 * x243; const FLT x247 = x242 * x246; const FLT x248 = x14 * x236; const FLT x249 = x21 * x248; const FLT x250 = 1.0/2.0 * x34; const FLT x251 = x250 * x243; const FLT x252 = x15 * x251; const FLT x253 = -1 * x9 * x41; const FLT x254 = x42 * x21; const FLT x255 = x254 + x253; const FLT x256 = x255 + (-1 * x252 * x242) + x234 + x235 + (x238 * x244) + (-1 * x245 * x247) + (-1 * x244 * x249); const FLT x257 = 2 * x58; const FLT x258 = x41 * x12; const FLT x259 = x21 * x236; const FLT x260 = x10 * x259; const FLT x261 = x15 * x237; const FLT x262 = x24 * x14; const FLT x263 = x262 * x246; const FLT x264 = x13 * x251; const FLT x265 = -1 * x8 * x254; const FLT x266 = x9 * x44; const FLT x267 = x266 + x265; const FLT x268 = x267 + (-1 * x264 * x242) + (x260 * x244) + x258 + (-1 * x261 * x244) + (-1 * x263 * x242) + x43; const FLT x269 = x12 * x254; const FLT x270 = -1 * x269; const FLT x271 = x9 * x43; const FLT x272 = -1 * x271; const FLT x273 = x15 * x259; const FLT x274 = x14 * x250; const FLT x275 = x274 * x243; const FLT x276 = x11 * x10; const FLT x277 = x24 * x13; const FLT x278 = x236 * x277; const FLT x279 = x8 * x41; const FLT x280 = (-1 * x279) + x44; const FLT x281 = x280 + (x278 * x244) + x272 + (x276 * x247) + x270 + (x273 * x244) + (-1 * x275 * x242); const FLT x282 = (x256 * sensor_pt[1]) + (-1 * x268 * sensor_pt[0]) + (x281 * sensor_pt[2]); const FLT x283 = 2 * x53; const FLT x284 = x10 * x251; const FLT x285 = x11 * x248; const FLT x286 = x13 * x259; const FLT x287 = x8 * x44; const FLT x288 = x24 * x15; const FLT x289 = x9 * x254; const FLT x290 = -1 * x43 * x12; const FLT x291 = x290 + x289; const FLT x292 = x291 + (-1 * x284 * x242) + x41 + (x288 * x247) + (-1 * x286 * x244) + (-1 * x285 * x244) + x287; const FLT x293 = 2 * x60; const FLT x294 = (x268 * sensor_pt[2]) + (x281 * sensor_pt[0]) + (-1 * x292 * sensor_pt[1]); const FLT x295 = 2 * x50; const FLT x296 = (x293 * x292) + (x295 * x294) + (-1 * x256 * x257) + (-1 * x282 * x283); const FLT x297 = 2 * x57; const FLT x298 = (-1 * x256 * sensor_pt[2]) + (x292 * sensor_pt[0]) + (x281 * sensor_pt[1]); const FLT x299 = 2 * x54; const FLT x300 = (x299 * x256) + (x298 * x283) + (-1 * x297 * x294) + (-1 * x293 * x268); const FLT x301 = (x268 * x257) + (x297 * x282) + (-1 * x298 * x295) + (-1 * x299 * x292); const FLT x302 = (x301 * (*lh_p).Rot[3]) + (x296 * (*lh_p).Rot[0]) + (-1 * x300 * (*lh_p).Rot[1]); const FLT x303 = (x296 * (*lh_p).Rot[1]) + (-1 * x301 * (*lh_p).Rot[2]) + (x300 * (*lh_p).Rot[0]); const FLT x304 = x301 + (-1 * x302 * x190) + (x303 * x188); const FLT x305 = (x300 * (*lh_p).Rot[2]) + (-1 * x296 * (*lh_p).Rot[3]) + (x301 * (*lh_p).Rot[0]); const FLT x306 = x300 + (-1 * x305 * x188) + (x210 * x302); const FLT x307 = (x81 * x306) + (x80 * x304); const FLT x308 = x296 + (-1 * x210 * x303) + (x305 * x190); const FLT x309 = (x91 * x308) + (-1 * x87 * x307); const FLT x310 = ((-1 * x75 * x306) + (x69 * x304)) * x78; const FLT x311 = x310 + (-1 * x94 * x309); const FLT x312 = (x308 * x103) + (-1 * x128 * (x307 + (x308 * x127))); const FLT x313 = x312 * x135; const FLT x314 = (x312 * x139) + (x104 * ((-1 * x312 * x138) + x313)); const FLT x315 = (x314 * x104) + (x312 * x141); const FLT x316 = x151 * (x309 + (x311 * x124) + (-1 * x145 * ((-1 * x118 * ((x315 * x104) + (x312 * x144) + (x104 * (x315 + (x312 * x131) + (x104 * (x314 + (x312 * x132) + (x104 * ((x312 * x137) + x313 + (-1 * x312 * x134))))))) + (x312 * x143))) + (-1 * x311 * x126))) + (x315 * x149) + (x312 * x148)); const FLT x317 = 1.0 * x14; const FLT x318 = (-1 * x241 * (*error_model).Pose.AxisAngleRot[2]) + (x239 * (*error_model).Pose.AxisAngleRot[1]) + (2 * x15) + (-1 * x317 * (*error_model).Pose.AxisAngleRot[0]); const FLT x319 = x243 * x318; const FLT x320 = x246 * x318; const FLT x321 = x44 + x279; const FLT x322 = x321 + (-1 * x252 * x318) + (-1 * x249 * x319) + x271 + x270 + (x238 * x319) + (-1 * x245 * x320); const FLT x323 = x15 * x319; const FLT x324 = -1 * x235; const FLT x325 = x253 + (-1 * x254); const FLT x326 = x325 + x234 + (x276 * x320) + x324 + (x259 * x323) + (-1 * x275 * x318) + (x278 * x319); const FLT x327 = -1 * x258; const FLT x328 = -1 * x43; const FLT x329 = x267 + x328 + (-1 * x284 * x318) + x327 + (x288 * x320) + (-1 * x286 * x319) + (-1 * x285 * x319); const FLT x330 = (x329 * sensor_pt[0]) + (-1 * x322 * sensor_pt[2]) + (x326 * sensor_pt[1]); const FLT x331 = -1 * x287; const FLT x332 = (-1 * x289) + x290; const FLT x333 = x332 + (-1 * x264 * x318) + x41 + (-1 * x237 * x323) + x331 + (-1 * x262 * x320) + (x260 * x319); const FLT x334 = (x322 * sensor_pt[1]) + (x326 * sensor_pt[2]) + (-1 * x333 * sensor_pt[0]); const FLT x335 = (x257 * x333) + (x297 * x334) + (-1 * x295 * x330) + (-1 * x299 * x329); const FLT x336 = (-1 * x329 * sensor_pt[1]) + (x333 * sensor_pt[2]) + (x326 * sensor_pt[0]); const FLT x337 = (x293 * x329) + (-1 * x283 * x334) + (-1 * x257 * x322) + (x295 * x336); const FLT x338 = (-1 * x293 * x333) + (x283 * x330) + (-1 * x297 * x336) + (x299 * x322); const FLT x339 = (x338 * (*lh_p).Rot[2]) + (x335 * (*lh_p).Rot[0]) + (-1 * x337 * (*lh_p).Rot[3]); const FLT x340 = (x337 * (*lh_p).Rot[1]) + (-1 * x335 * (*lh_p).Rot[2]) + (x338 * (*lh_p).Rot[0]); const FLT x341 = x337 + (x339 * x190) + (-1 * x210 * x340); const FLT x342 = (x337 * (*lh_p).Rot[0]) + (x335 * (*lh_p).Rot[3]) + (-1 * x338 * (*lh_p).Rot[1]); const FLT x343 = x335 + (-1 * x342 * x190) + (x340 * x188); const FLT x344 = x338 + (x210 * x342) + (-1 * x339 * x188); const FLT x345 = (x81 * x344) + (x80 * x343); const FLT x346 = (x341 * x103) + (-1 * x128 * (x345 + (x341 * x127))); const FLT x347 = (x91 * x341) + (-1 * x87 * x345); const FLT x348 = ((-1 * x75 * x344) + (x69 * x343)) * x78; const FLT x349 = x348 + (-1 * x94 * x347); const FLT x350 = x346 * x135; const FLT x351 = (x346 * x139) + (x104 * ((-1 * x346 * x138) + x350)); const FLT x352 = (x351 * x104) + (x346 * x141); const FLT x353 = x151 * (x347 + (x352 * x149) + (x346 * x148) + (-1 * x145 * ((-1 * x118 * ((x346 * x144) + (x352 * x104) + (x104 * (x352 + (x346 * x131) + (x104 * ((x346 * x132) + x351 + (x104 * ((x346 * x137) + x350 + (-1 * x346 * x134))))))) + (x346 * x143))) + (-1 * x349 * x126))) + (x349 * x124)); const FLT x354 = -1 * x41; const FLT x355 = (-1 * x239 * (*error_model).Pose.AxisAngleRot[0]) + (2 * x13) + (x240 * (*error_model).Pose.AxisAngleRot[2]) + (-1 * x317 * (*error_model).Pose.AxisAngleRot[1]); const FLT x356 = x243 * x355; const FLT x357 = x246 * x355; const FLT x358 = x332 + x287 + x354 + (-1 * x249 * x356) + (-1 * x252 * x355) + (x238 * x356) + (-1 * x245 * x357); const FLT x359 = x265 + (-1 * x266); const FLT x360 = x328 + (-1 * x275 * x355) + x359 + (x273 * x356) + (x276 * x357) + (x278 * x356) + x258; const FLT x361 = -1 * x234; const FLT x362 = x251 * x355; const FLT x363 = (-1 * x10 * x362) + x324 + (-1 * x285 * x356) + x255 + (-1 * x286 * x356) + x361 + (x288 * x357); const FLT x364 = (x363 * sensor_pt[0]) + (-1 * x358 * sensor_pt[2]) + (x360 * sensor_pt[1]); const FLT x365 = (x260 * x356) + x269 + (-1 * x263 * x355) + (-1 * x13 * x362) + (-1 * x261 * x356) + x321 + x272; const FLT x366 = (x360 * sensor_pt[2]) + (x358 * sensor_pt[1]) + (-1 * x365 * sensor_pt[0]); const FLT x367 = (x257 * x365) + (x297 * x366) + (-1 * x295 * x364) + (-1 * x299 * x363); const FLT x368 = (x365 * sensor_pt[2]) + (-1 * x363 * sensor_pt[1]) + (x360 * sensor_pt[0]); const FLT x369 = (x283 * x364) + (x299 * x358) + (-1 * x293 * x365) + (-1 * x297 * x368); const FLT x370 = (x295 * x368) + (x293 * x363) + (-1 * x257 * x358) + (-1 * x283 * x366); const FLT x371 = (x370 * (*lh_p).Rot[1]) + (-1 * x367 * (*lh_p).Rot[2]) + (x369 * (*lh_p).Rot[0]); const FLT x372 = (x369 * (*lh_p).Rot[2]) + (-1 * x370 * (*lh_p).Rot[3]) + (x367 * (*lh_p).Rot[0]); const FLT x373 = x370 + (-1 * x210 * x371) + (x372 * x190); const FLT x374 = (x367 * (*lh_p).Rot[3]) + (x370 * (*lh_p).Rot[0]) + (-1 * x369 * (*lh_p).Rot[1]); const FLT x375 = x367 + (-1 * x374 * x190) + (x371 * x188); const FLT x376 = x369 + (x210 * x374) + (-1 * x372 * x188); const FLT x377 = (x81 * x376) + (x80 * x375); const FLT x378 = (x373 * x103) + (-1 * x128 * (x377 + (x373 * x127))); const FLT x379 = (x91 * x373) + (-1 * x87 * x377); const FLT x380 = ((-1 * x75 * x376) + (x69 * x375)) * x78; const FLT x381 = x380 + (-1 * x94 * x379); const FLT x382 = x378 * x135; const FLT x383 = (x378 * x139) + (x104 * ((-1 * x378 * x138) + x382)); const FLT x384 = (x383 * x104) + (x378 * x141); const FLT x385 = x151 * ((x381 * x124) + x379 + (x384 * x149) + (x378 * x148) + (-1 * x145 * ((-1 * x118 * ((x378 * x144) + (x104 * (x384 + (x378 * x131) + (x104 * ((x378 * x132) + x383 + (x104 * ((x378 * x137) + x382 + (-1 * x378 * x134))))))) + (x378 * x143) + (x384 * x104))) + (-1 * x381 * x126)))); const FLT x386 = (x241 * (*error_model).Pose.AxisAngleRot[0]) + (2 * x10) + (-1 * x240 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x317 * (*error_model).Pose.AxisAngleRot[2]); const FLT x387 = x243 * x386; const FLT x388 = x248 * x387; const FLT x389 = x236 * x387; const FLT x390 = x280 + x269 + x271 + (x288 * x389) + (-1 * x286 * x387) + (-1 * x284 * x386) + (-1 * x11 * x388); const FLT x391 = x291 + (x276 * x389) + (-1 * x275 * x386) + (x273 * x387) + x354 + (x277 * x389) + x331; const FLT x392 = x11 * x389; const FLT x393 = (-1 * x264 * x386) + x325 + (x260 * x387) + (-1 * x15 * x392) + (-1 * x262 * x389) + x361 + x235; const FLT x394 = (-1 * x390 * sensor_pt[1]) + (x393 * sensor_pt[2]) + (x391 * sensor_pt[0]); const FLT x395 = x359 + (x13 * x392) + (-1 * x21 * x388) + (-1 * x252 * x386) + x327 + (-1 * x245 * x389) + x43; const FLT x396 = (x390 * sensor_pt[0]) + (x391 * sensor_pt[1]) + (-1 * x395 * sensor_pt[2]); const FLT x397 = (x283 * x396) + (x299 * x395) + (-1 * x297 * x394) + (-1 * x293 * x393); const FLT x398 = (x395 * sensor_pt[1]) + (-1 * x393 * sensor_pt[0]) + (x391 * sensor_pt[2]); const FLT x399 = (x293 * x390) + (x295 * x394) + (-1 * x283 * x398) + (-1 * x257 * x395); const FLT x400 = (x257 * x393) + (x297 * x398) + (-1 * x295 * x396) + (-1 * x299 * x390); const FLT x401 = (x400 * (*lh_p).Rot[3]) + (-1 * x397 * (*lh_p).Rot[1]) + (x399 * (*lh_p).Rot[0]); const FLT x402 = (-1 * x400 * (*lh_p).Rot[2]) + (x399 * (*lh_p).Rot[1]) + (x397 * (*lh_p).Rot[0]); const FLT x403 = (-1 * x401 * x190) + x400 + (x402 * x188); const FLT x404 = (x397 * (*lh_p).Rot[2]) + (-1 * x399 * (*lh_p).Rot[3]) + (x400 * (*lh_p).Rot[0]); const FLT x405 = x397 + (-1 * x404 * x188) + (x401 * x210); const FLT x406 = (x81 * x405) + (x80 * x403); const FLT x407 = x399 + (x404 * x190) + (-1 * x402 * x210); const FLT x408 = (x91 * x407) + (-1 * x87 * x406); const FLT x409 = ((-1 * x75 * x405) + (x69 * x403)) * x78; const FLT x410 = x409 + (-1 * x94 * x408); const FLT x411 = (x407 * x103) + (-1 * x128 * (x406 + (x407 * x127))); const FLT x412 = x411 * x135; const FLT x413 = (x411 * x139) + (x104 * ((-1 * x411 * x138) + x412)); const FLT x414 = (x413 * x104) + (x411 * x141); const FLT x415 = x151 * ((x414 * x149) + (x411 * x148) + x408 + (-1 * x145 * ((-1 * x118 * ((x411 * x144) + (x104 * (x414 + (x411 * x131) + (x104 * (x413 + (x411 * x132) + (x104 * ((x411 * x137) + x412 + (-1 * x411 * x134))))))) + (x411 * x143) + (x414 * x104))) + (-1 * x410 * x126))) + (x410 * x124)); const FLT x416 = dt * dt * dt; const FLT x417 = 0.5 * x32 * x416; const FLT x418 = x55 * x417; const FLT x419 = x21 * x21 * x21; const FLT x420 = dt * dt * dt * dt; const FLT x421 = x30 * (1. / (x27 * sqrt(x27))); const FLT x422 = 1.0 * x34; const FLT x423 = x422 * x421; const FLT x424 = x423 * x420; const FLT x425 = 2 * x33; const FLT x426 = x18 * x425; const FLT x427 = 2 * x31 * (1. / (x27 * x27)); const FLT x428 = x420 * x427; const FLT x429 = x19 * x428; const FLT x430 = x21 * x424; const FLT x431 = x39 * x422; const FLT x432 = x18 * x431; const FLT x433 = x25 * x428; const FLT x434 = (1. / (x35 * sqrt(x35))) * x17; const FLT x435 = ((-1 * x21 * x433) + (-1 * x21 * x432) + (x25 * x430) + (x21 * x426) + (x419 * x424) + (-1 * x21 * x429) + (-1 * x419 * x428) + (x19 * x430)) * x434; const FLT x436 = x435 * x236; const FLT x437 = x13 * x435; const FLT x438 = x435 * x248; const FLT x439 = x21 * x18; const FLT x440 = 0.5 * x39; const FLT x441 = x37 * x440; const FLT x442 = x10 * x441; const FLT x443 = x416 * x421; const FLT x444 = x37 * x13; const FLT x445 = x444 * x443; const FLT x446 = x435 * x250; const FLT x447 = x45 * x417; const FLT x448 = x21 * x11; const FLT x449 = x38 * x443; const FLT x450 = x21 * x449; const FLT x451 = (-1 * x14 * x450) + (x448 * x447); const FLT x452 = x37 * x443; const FLT x453 = x21 * x452; const FLT x454 = x24 * x21; const FLT x455 = x51 * x417; const FLT x456 = (-1 * x454 * x455) + (x453 * x288); const FLT x457 = x456 + x451 + x49 + (-1 * x22 * x445) + (x436 * x288) + (x22 * x418) + (-1 * x10 * x446) + (-1 * x437 * x259) + (-1 * x439 * x442) + (-1 * x11 * x438); const FLT x458 = x15 * x435; const FLT x459 = x454 * x445; const FLT x460 = x454 * x418; const FLT x461 = x22 * x452; const FLT x462 = x14 * x441; const FLT x463 = -1 * x46; const FLT x464 = x48 * x417; const FLT x465 = (-1 * x464 * x448) + (x10 * x450); const FLT x466 = (-1 * x462 * x439) + (-1 * x435 * x274) + x463 + (-1 * x460) + (x435 * x278) + (-1 * x22 * x455) + x465 + (x15 * x461) + x459 + (x458 * x259) + (x436 * x276); const FLT x467 = -1 * x56; const FLT x468 = x444 * x440; const FLT x469 = x455 * x448; const FLT x470 = x236 * x262; const FLT x471 = x15 * x450; const FLT x472 = (-1 * x453 * x262) + (x454 * x447); const FLT x473 = x472 + (x10 * x461) + (-1 * x458 * x237) + (-1 * x470 * x435) + (-1 * x471) + (-1 * x13 * x446) + (-1 * x22 * x464) + (-1 * x468 * x439) + (x435 * x260) + x467 + x469; const FLT x474 = (-1 * x457 * sensor_pt[1]) + (x473 * sensor_pt[2]) + (x466 * sensor_pt[0]); const FLT x475 = x15 * x441; const FLT x476 = x454 * x464; const FLT x477 = x13 * x450; const FLT x478 = x453 * x245; const FLT x479 = x418 * x448; const FLT x480 = (-1 * x479) + x476 + (x437 * x237) + (-1 * x475 * x439) + (-1 * x15 * x446) + (-1 * x14 * x461) + (-1 * x436 * x245) + x477 + x52 + (-1 * x21 * x438) + (-1 * x478) + (x22 * x447); const FLT x481 = (x457 * sensor_pt[0]) + (-1 * x480 * sensor_pt[2]) + (x466 * sensor_pt[1]); const FLT x482 = (-1 * x474 * x297) + (-1 * x473 * x293) + (x481 * x283) + (x480 * x299); const FLT x483 = (x480 * sensor_pt[1]) + (-1 * x473 * sensor_pt[0]) + (x466 * sensor_pt[2]); const FLT x484 = (x474 * x295) + (x457 * x293) + (-1 * x480 * x257) + (-1 * x483 * x283); const FLT x485 = (-1 * x457 * x299) + (-1 * x481 * x295) + (x483 * x297) + (x473 * x257); const FLT x486 = (x485 * (*lh_p).Rot[3]) + (-1 * x482 * (*lh_p).Rot[1]) + (x484 * (*lh_p).Rot[0]); const FLT x487 = (x484 * (*lh_p).Rot[1]) + (-1 * x485 * (*lh_p).Rot[2]) + (x482 * (*lh_p).Rot[0]); const FLT x488 = x485 + (-1 * x486 * x190) + (x487 * x188); const FLT x489 = (x482 * (*lh_p).Rot[2]) + (-1 * x484 * (*lh_p).Rot[3]) + (x485 * (*lh_p).Rot[0]); const FLT x490 = x482 + (-1 * x489 * x188) + (x486 * x210); const FLT x491 = (x81 * x490) + (x80 * x488); const FLT x492 = x484 + (-1 * x487 * x210) + (x489 * x190); const FLT x493 = (x91 * x492) + (-1 * x87 * x491); const FLT x494 = ((-1 * x75 * x490) + (x69 * x488)) * x78; const FLT x495 = x494 + (-1 * x94 * x493); const FLT x496 = (x492 * x103) + (-1 * x128 * (x491 + (x492 * x127))); const FLT x497 = x496 * x135; const FLT x498 = (x496 * x139) + (x104 * ((-1 * x496 * x138) + x497)); const FLT x499 = (x498 * x104) + (x496 * x141); const FLT x500 = x151 * (x493 + (x499 * x149) + (-1 * x145 * ((-1 * x118 * ((x496 * x144) + (x496 * x143) + (x104 * (x499 + (x496 * x131) + (x104 * (x498 + (x496 * x132) + (x104 * ((x496 * x137) + x497 + (-1 * x496 * x134))))))) + (x499 * x104))) + (-1 * x495 * x126))) + (x495 * x124) + (x496 * x148)); const FLT x501 = x24 * x18; const FLT x502 = x25 * x417; const FLT x503 = x22 * x424; const FLT x504 = x22 * x428; const FLT x505 = (x24 * x24 * x24) * x420; const FLT x506 = (-1 * x427 * x505) + (x423 * x505) + (x425 * x501) + (-1 * x24 * x504) + (x24 * x503) + (-1 * x431 * x501) + (-1 * x24 * x429) + (x24 * x19 * x424); const FLT x507 = x434 * x506; const FLT x508 = x236 * x245; const FLT x509 = x25 * x452; const FLT x510 = x434 * x250; const FLT x511 = x510 * x506; const FLT x512 = x13 * x236; const FLT x513 = x11 * x434; const FLT x514 = x513 * x506; const FLT x515 = x24 * x11; const FLT x516 = (x449 * x277) + (-1 * x418 * x515); const FLT x517 = x472 + x516 + x56 + (-1 * x15 * x511) + (x48 * x502) + (-1 * x475 * x501) + (-1 * x507 * x508) + (-1 * x507 * x249) + (x514 * x512) + (-1 * x10 * x509); const FLT x518 = -1 * x49; const FLT x519 = x464 * x515; const FLT x520 = x10 * x236; const FLT x521 = x449 * x245; const FLT x522 = x434 * x274; const FLT x523 = x456 + (x507 * x273) + (-1 * x462 * x501) + (x25 * x445) + (x507 * x278) + (-1 * x55 * x502) + x518 + (-1 * x522 * x506) + (-1 * x519) + (x514 * x520) + x521; const FLT x524 = x236 * x288; const FLT x525 = (x447 * x515) + (-1 * x449 * x262); const FLT x526 = x525 + (-1 * x442 * x501) + (-1 * x51 * x502) + (-1 * x514 * x248) + (-1 * x10 * x511) + x460 + x463 + (-1 * x507 * x286) + (x524 * x507) + (-1 * x459) + (x15 * x509); const FLT x527 = (x526 * sensor_pt[0]) + (-1 * x517 * sensor_pt[2]) + (x523 * sensor_pt[1]); const FLT x528 = x455 * x515; const FLT x529 = x15 * x236; const FLT x530 = x449 * x288; const FLT x531 = (x45 * x502) + x52 + (x507 * x260) + (-1 * x530) + (-1 * x470 * x507) + (-1 * x514 * x529) + (-1 * x476) + (-1 * x468 * x501) + (-1 * x14 * x509) + x478 + x528 + (-1 * x13 * x511); const FLT x532 = (x517 * sensor_pt[1]) + (-1 * x531 * sensor_pt[0]) + (x523 * sensor_pt[2]); const FLT x533 = (x531 * x257) + (x532 * x297) + (-1 * x527 * x295) + (-1 * x526 * x299); const FLT x534 = (x531 * sensor_pt[2]) + (-1 * x526 * sensor_pt[1]) + (x523 * sensor_pt[0]); const FLT x535 = (-1 * x531 * x293) + (x527 * x283) + (-1 * x534 * x297) + (x517 * x299); const FLT x536 = (x534 * x295) + (-1 * x532 * x283) + (-1 * x517 * x257) + (x526 * x293); const FLT x537 = (x536 * (*lh_p).Rot[1]) + (-1 * x533 * (*lh_p).Rot[2]) + (x535 * (*lh_p).Rot[0]); const FLT x538 = 2 * ((x535 * (*lh_p).Rot[2]) + (-1 * x536 * (*lh_p).Rot[3]) + (x533 * (*lh_p).Rot[0])); const FLT x539 = x536 + (-1 * x537 * x210) + (x538 * (*lh_p).Rot[3]); const FLT x540 = (x536 * (*lh_p).Rot[0]) + (x533 * (*lh_p).Rot[3]) + (-1 * x535 * (*lh_p).Rot[1]); const FLT x541 = (-1 * x540 * x190) + x533 + (x537 * x188); const FLT x542 = x535 + (-1 * x538 * (*lh_p).Rot[2]) + (x540 * x210); const FLT x543 = (x81 * x542) + (x80 * x541); const FLT x544 = (x539 * x103) + (-1 * x128 * (x543 + (x539 * x127))); const FLT x545 = x544 * x130; const FLT x546 = (x91 * x539) + (-1 * x87 * x543); const FLT x547 = ((-1 * x75 * x542) + (x69 * x541)) * x78; const FLT x548 = x547 + (-1 * x94 * x546); const FLT x549 = x545 * x106; const FLT x550 = (x545 * x107) + (x104 * ((-1 * x545 * x105) + x549)); const FLT x551 = (x550 * x104) + (x545 * x108); const FLT x552 = x151 * (x546 + (x551 * x149) + (-1 * x145 * ((-1 * x118 * ((x551 * x104) + (x545 * x109) + (x104 * (x551 + (x544 * x131) + (x104 * (x550 + (x545 * x112) + (x104 * ((-1 * x545 * x133) + (x544 * x137) + x549)))))) + (x545 * x114))) + (-1 * x548 * x126))) + (x545 * x147) + (x548 * x124)); const FLT x553 = x11 * x11 * x11; const FLT x554 = (x25 * x11 * x424) + (x424 * x553) + (-1 * x11 * x432) + (-1 * x428 * x553) + (-1 * x11 * x504) + (x11 * x503) + (x11 * x426) + (-1 * x11 * x433); const FLT x555 = x434 * x554; const FLT x556 = x513 * x554; const FLT x557 = x38 * x18 * x440; const FLT x558 = x19 * x452; const FLT x559 = x510 * x554; const FLT x560 = (-1 * x555 * x286) + (-1 * x556 * x248) + (x524 * x555) + x52 + x530 + x479 + (x19 * x447) + (-1 * x10 * x557) + (-1 * x477) + (-1 * x528) + (-1 * x14 * x558) + (-1 * x10 * x559); const FLT x561 = x516 + (x555 * x273) + (-1 * x14 * x557) + x471 + (x10 * x558) + (x520 * x556) + (-1 * x469) + (-1 * x19 * x464) + (x555 * x278) + (-1 * x522 * x554) + x467; const FLT x562 = x465 + (-1 * x529 * x556) + (-1 * x15 * x558) + x525 + x46 + (-1 * x13 * x559) + (-1 * x470 * x555) + (x555 * x260) + (-1 * x13 * x557) + (x19 * x455); const FLT x563 = (-1 * x560 * sensor_pt[1]) + (x562 * sensor_pt[2]) + (x561 * sensor_pt[0]); const FLT x564 = (-1 * x555 * x249) + (x512 * x556) + (x19 * x445) + (-1 * x19 * x418) + x518 + (-1 * x521) + (-1 * x15 * x559) + (-1 * x508 * x555) + x451 + x519 + (-1 * x15 * x557); const FLT x565 = (x560 * sensor_pt[0]) + (-1 * x564 * sensor_pt[2]) + (x561 * sensor_pt[1]); const FLT x566 = (-1 * x562 * x293) + (x564 * x299) + (-1 * x563 * x297) + (x565 * x283); const FLT x567 = (x564 * sensor_pt[1]) + (-1 * x562 * sensor_pt[0]) + (x561 * sensor_pt[2]); const FLT x568 = (x563 * x295) + (x560 * x293) + (-1 * x564 * x257) + (-1 * x567 * x283); const FLT x569 = (x562 * x257) + (x567 * x297) + (-1 * x565 * x295) + (-1 * x560 * x299); const FLT x570 = (-1 * x566 * (*lh_p).Rot[1]) + (x569 * (*lh_p).Rot[3]) + (x568 * (*lh_p).Rot[0]); const FLT x571 = (x568 * (*lh_p).Rot[1]) + (-1 * x569 * (*lh_p).Rot[2]) + (x566 * (*lh_p).Rot[0]); const FLT x572 = x569 + (-1 * x570 * x190) + (x571 * x188); const FLT x573 = (x566 * (*lh_p).Rot[2]) + (-1 * x568 * (*lh_p).Rot[3]) + (x569 * (*lh_p).Rot[0]); const FLT x574 = x566 + (-1 * x573 * x188) + (x570 * x210); const FLT x575 = (x81 * x574) + (x80 * x572); const FLT x576 = x568 + (x573 * x190) + (-1 * x571 * x210); const FLT x577 = (x91 * x576) + (-1 * x87 * x575); const FLT x578 = ((-1 * x75 * x574) + (x69 * x572)) * x78; const FLT x579 = x578 + (-1 * x94 * x577); const FLT x580 = (x576 * x103) + (-1 * x128 * (x575 + (x576 * x127))); const FLT x581 = x580 * x135; const FLT x582 = (x580 * x139) + (x104 * ((-1 * x580 * x138) + x581)); const FLT x583 = (x582 * x104) + (x580 * x141); const FLT x584 = x151 * ((x583 * x149) + x577 + (x580 * x148) + (-1 * x145 * ((-1 * x118 * ((x580 * x144) + (x583 * x104) + (x104 * (x583 + (x580 * x131) + (x104 * (x582 + (x580 * x132) + (x104 * ((x580 * x137) + x581 + (-1 * x580 * x134))))))) + (x580 * x143))) + (-1 * x579 * x126))) + (x579 * x124)); const FLT x585 = dt * x194; const FLT x586 = dt * x195; const FLT x587 = x586 + x585; const FLT x588 = -1 * dt * x185; const FLT x589 = -1 * dt * x183; const FLT x590 = dt + x589 + x588; const FLT x591 = dt * x189; const FLT x592 = dt * x191; const FLT x593 = x592 + (-1 * x591); const FLT x594 = (x81 * x593) + (x80 * x590); const FLT x595 = (x587 * x103) + (-1 * x128 * (x594 + (x587 * x127))); const FLT x596 = (x91 * x587) + (-1 * x87 * x594); const FLT x597 = ((-1 * x75 * x593) + (x69 * x590)) * x78; const FLT x598 = x597 + (-1 * x94 * x596); const FLT x599 = x595 * x135; const FLT x600 = (x595 * x139) + (x104 * ((-1 * x595 * x138) + x599)); const FLT x601 = (x600 * x104) + (x595 * x141); const FLT x602 = x151 * (x596 + (x601 * x149) + (x598 * x124) + (x595 * x148) + (-1 * x145 * ((-1 * x118 * ((x595 * x144) + (x601 * x104) + (x104 * ((x595 * x131) + x601 + (x104 * ((x595 * x132) + x600 + (x104 * ((x595 * x137) + x599 + (-1 * x595 * x134))))))) + (x595 * x143))) + (-1 * x598 * x126)))); const FLT x603 = x585 + (-1 * x586); const FLT x604 = dt * x209; const FLT x605 = dt * x211; const FLT x606 = x605 + x604; const FLT x607 = (x81 * x606) + (x80 * x603); const FLT x608 = (-1 * dt * x205) + dt; const FLT x609 = x608 + x589; const FLT x610 = (x91 * x609) + (-1 * x87 * x607); const FLT x611 = ((-1 * x75 * x606) + (x69 * x603)) * x78; const FLT x612 = x611 + (-1 * x94 * x610); const FLT x613 = (x609 * x103) + (-1 * x128 * (x607 + (x609 * x127))); const FLT x614 = x613 * x135; const FLT x615 = (x613 * x139) + (x104 * ((-1 * x613 * x138) + x614)); const FLT x616 = (x615 * x104) + (x613 * x141); const FLT x617 = x151 * (x610 + (x616 * x149) + (x613 * x148) + (-1 * x145 * ((-1 * x118 * ((x613 * x144) + (x104 * (x616 + (x613 * x131) + (x104 * (x615 + (x613 * x132) + (x104 * (x614 + (x613 * x137) + (-1 * x613 * x134))))))) + (x616 * x104) + (x613 * x143))) + (-1 * x612 * x126))) + (x612 * x124)); const FLT x618 = x591 + x592; const FLT x619 = x608 + x588; const FLT x620 = (x81 * x619) + (x80 * x618); const FLT x621 = x604 + (-1 * x605); const FLT x622 = (x91 * x621) + (-1 * x87 * x620); const FLT x623 = ((-1 * x75 * x619) + (x69 * x618)) * x78; const FLT x624 = x623 + (-1 * x94 * x622); const FLT x625 = (x621 * x103) + (-1 * x128 * (x620 + (x621 * x127))); const FLT x626 = x625 * x135; const FLT x627 = (x625 * x139) + (x104 * ((-1 * x625 * x138) + x626)); const FLT x628 = (x627 * x104) + (x625 * x141); const FLT x629 = x151 * (x622 + (x624 * x124) + (x628 * x149) + (-1 * x145 * ((-1 * x118 * ((x625 * x144) + (x628 * x104) + (x104 * (x628 + (x625 * x131) + (x104 * (x627 + (x625 * x132) + (x104 * ((x625 * x137) + x626 + (-1 * x625 * x134))))))) + (x625 * x143))) + (-1 * x624 * x126))) + (x625 * x148)); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), (-1 * x153 * ((-1 * x79) + x152)) + x79 + (-1 * x152)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), (-1 * x169) + (-1 * ((-1 * x163) + x169) * x153) + x163); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), (-1 * ((-1 * x177) + x182) * x153) + (-1 * x182) + x177); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), (-1 * ((-1 * x198) + x204) * x153) + (-1 * x204) + x198); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), (-1 * ((-1 * x216) + x221) * x153) + (-1 * x221) + x216); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), (-1 * ((-1 * x224) + x233) * x153) + x224 + (-1 * x233)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * ((-1 * x310) + x316) * x153) + (-1 * x316) + x310); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * ((-1 * x348) + x353) * x153) + (-1 * x353) + x348); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * ((-1 * x380) + x385) * x153) + (-1 * x385) + x380); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * ((-1 * x409) + x415) * x153) + (-1 * x415) + x409); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (-1 * x500) + (-1 * ((-1 * x494) + x500) * x153) + x494); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (-1 * ((-1 * x547) + x552) * x153) + (-1 * x552) + x547); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (-1 * x584) + (-1 * ((-1 * x578) + x584) * x153) + x578); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), (-1 * ((-1 * x597) + x602) * x153) + (-1 * x602) + x597); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), (-1 * ((-1 * x611) + x617) * x153) + (-1 * x617) + x611); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), (-1 * ((-1 * x623) + x629) * x153) + (-1 * x629) + x623); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x1 = dt * fabs(dt); const FLT x2 = -1 * x0 * x1; const FLT x3 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x4 = 1.0/2.0 * x1; const FLT x5 = x4 + (-1 * x1 * x3); const FLT x6 = x5 + x2; const FLT x7 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x8 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x9 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x10 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x11 = (x10 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x8 * (*_x0).Pose.Rot[2]) + (x9 * (*_x0).Pose.Rot[0]); const FLT x12 = (-1 * x9 * (*_x0).Pose.Rot[1]) + (x8 * (*_x0).Pose.Rot[3]) + (x10 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[2]; const FLT x13 = (-1 * x8 * (*_x0).Pose.Rot[1]) + (-1 * x10 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x9 * (*_x0).Pose.Rot[3]); const FLT x14 = (x8 * (*_x0).Pose.Rot[0]) + (-1 * x10 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[1] + (x9 * (*_x0).Pose.Rot[2]); const FLT x15 = (x14 * x14) + (x13 * x13) + (x11 * x11) + (x12 * x12); const FLT x16 = 1. / sqrt(x15); const FLT x17 = dt * dt; const FLT x18 = x7 * x7; const FLT x19 = x18 * x17; const FLT x20 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x21 = x20 * x20; const FLT x22 = x21 * x17; const FLT x23 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x24 = x23 * x23; const FLT x25 = x24 * x17; const FLT x26 = 1e-10 + x25 + x19 + x22; const FLT x27 = sqrt(x26); const FLT x28 = 0.5 * x27; const FLT x29 = sin(x28); const FLT x30 = x29 * x29; const FLT x31 = 1. / x26; const FLT x32 = x30 * x31; const FLT x33 = cos(x28); const FLT x34 = (x32 * x19) + (x32 * x22) + (x33 * x33) + (x32 * x25); const FLT x35 = 1. / sqrt(x34); const FLT x36 = x35 * x16; const FLT x37 = x29 * (1. / x27); const FLT x38 = dt * x37; const FLT x39 = x36 * x38; const FLT x40 = x39 * x11; const FLT x41 = x35 * x12; const FLT x42 = x41 * x16; const FLT x43 = x42 * x38; const FLT x44 = x33 * x16; const FLT x45 = x44 * x35; const FLT x46 = x45 * x13; const FLT x47 = x36 * x14; const FLT x48 = x47 * x38; const FLT x49 = x46 + (-1 * x7 * x40) + (-1 * x48 * x20) + (-1 * x43 * x23); const FLT x50 = x39 * x13; const FLT x51 = x45 * x11; const FLT x52 = (x43 * x20) + x51 + (x7 * x50) + (-1 * x48 * x23); const FLT x53 = x45 * x14; const FLT x54 = (x50 * x20) + (-1 * x7 * x43) + (x40 * x23) + x53; const FLT x55 = (-1 * x54 * sensor_pt[2]) + (x49 * sensor_pt[1]) + (x52 * sensor_pt[0]); const FLT x56 = x41 * x44; const FLT x57 = (-1 * x40 * x20) + (x7 * x48) + (x50 * x23) + x56; const FLT x58 = (-1 * x57 * sensor_pt[0]) + (x49 * sensor_pt[2]) + (x54 * sensor_pt[1]); const FLT x59 = (x4 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x58 * x57) + (-1 * x52 * x55))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x60 = (-1 * x52 * sensor_pt[1]) + (x49 * sensor_pt[0]) + (x57 * sensor_pt[2]); const FLT x61 = (*_x0).Pose.Pos[2] + (x4 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (2 * ((x54 * x55) + (-1 * x60 * x57))) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + sensor_pt[2]; const FLT x62 = (*_x0).Pose.Pos[1] + (2 * ((x60 * x52) + (-1 * x54 * x58))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x4 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x63 = (-1 * x62 * (*lh_p).Rot[3]) + (x59 * (*lh_p).Rot[0]) + (x61 * (*lh_p).Rot[2]); const FLT x64 = (-1 * x61 * (*lh_p).Rot[1]) + (x62 * (*lh_p).Rot[0]) + (x59 * (*lh_p).Rot[3]); const FLT x65 = x61 + (2 * ((x64 * (*lh_p).Rot[1]) + (-1 * x63 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x66 = (x61 * (*lh_p).Rot[0]) + (-1 * x59 * (*lh_p).Rot[2]) + (x62 * (*lh_p).Rot[1]); const FLT x67 = x59 + (*lh_p).Pos[0] + (2 * ((x66 * (*lh_p).Rot[2]) + (-1 * x64 * (*lh_p).Rot[3]))); const FLT x68 = x67 * x67; const FLT x69 = (1. / x68) * x65; const FLT x70 = x1 * (*lh_p).Rot[2] * (*lh_p).Rot[0]; const FLT x71 = x1 * (*lh_p).Rot[3]; const FLT x72 = x71 * (*lh_p).Rot[1]; const FLT x73 = x72 + (-1 * x70); const FLT x74 = 1. / x67; const FLT x75 = x68 + (x65 * x65); const FLT x76 = 1. / x75; const FLT x77 = x76 * x68; const FLT x78 = x77 * ((-1 * x73 * x74) + (x6 * x69)); const FLT x79 = 2 * x67; const FLT x80 = 2 * x65; const FLT x81 = (x80 * x73) + (x6 * x79); const FLT x82 = 0.523598775598299 + (*bsc0).tilt; const FLT x83 = tan(x82); const FLT x84 = x62 + (*lh_p).Pos[1] + (2 * ((x63 * (*lh_p).Rot[3]) + (-1 * x66 * (*lh_p).Rot[1]))); const FLT x85 = 1.0/2.0 * x84; const FLT x86 = x83 * x85 * (1. / (x75 * sqrt(x75))); const FLT x87 = x1 * (*lh_p).Rot[1]; const FLT x88 = x87 * (*lh_p).Rot[2]; const FLT x89 = x71 * (*lh_p).Rot[0]; const FLT x90 = x89 + x88; const FLT x91 = x83 * (1. / sqrt(x75)); const FLT x92 = (x91 * x90) + (-1 * x81 * x86); const FLT x93 = x84 * x84; const FLT x94 = 1. / sqrt(1 + (-1 * (x83 * x83) * x76 * x93)); const FLT x95 = x78 + (-1 * x92 * x94); const FLT x96 = atan2(-1 * x65, x67); const FLT x97 = x84 * x91; const FLT x98 = (*bsc0).ogeephase + (-1 * asin(x97)) + x96; const FLT x99 = cos(x98) * (*bsc0).ogeemag; const FLT x100 = x75 + x93; const FLT x101 = cos(x82); const FLT x102 = 1. / x101; const FLT x103 = (1. / sqrt(x100)) * x102; const FLT x104 = asin(x84 * x103); const FLT x105 = 8.0108022e-06 * x104; const FLT x106 = -8.0108022e-06 + (-1 * x105); const FLT x107 = 0.0028679863 + (x104 * x106); const FLT x108 = 5.3685255e-06 + (x104 * x107); const FLT x109 = 0.0076069798 + (x108 * x104); const FLT x110 = x109 * x104; const FLT x111 = -8.0108022e-06 + (-1.60216044e-05 * x104); const FLT x112 = x107 + (x104 * x111); const FLT x113 = x108 + (x104 * x112); const FLT x114 = x109 + (x104 * x113); const FLT x115 = (x104 * x114) + x110; const FLT x116 = sin(x82); const FLT x117 = (sin(x98) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x118 = x116 * x117; const FLT x119 = x101 + (-1 * x118 * x115); const FLT x120 = 1. / x119; const FLT x121 = x104 * x104; const FLT x122 = x109 * x121; const FLT x123 = x120 * x122; const FLT x124 = x99 * x123; const FLT x125 = x115 * x116; const FLT x126 = x99 * x125; const FLT x127 = 2 * x84; const FLT x128 = x85 * (1. / (x100 * sqrt(x100))) * x102; const FLT x129 = (x90 * x103) + (-1 * x128 * (x81 + (x90 * x127))); const FLT x130 = 1. / sqrt(1 + (-1 * x93 * (1. / (x101 * x101)) * (1. / x100))); const FLT x131 = x113 * x130; const FLT x132 = x112 * x130; const FLT x133 = 2.40324066e-05 * x104; const FLT x134 = x130 * x133; const FLT x135 = x106 * x130; const FLT x136 = x129 * x135; const FLT x137 = x111 * x130; const FLT x138 = x105 * x130; const FLT x139 = x107 * x130; const FLT x140 = (x129 * x139) + (x104 * ((-1 * x129 * x138) + x136)); const FLT x141 = x108 * x130; const FLT x142 = (x104 * x140) + (x129 * x141); const FLT x143 = x114 * x130; const FLT x144 = x109 * x130; const FLT x145 = (1. / (x119 * x119)) * x117 * x122; const FLT x146 = x117 * x120; const FLT x147 = 2 * x110 * x146; const FLT x148 = x130 * x147; const FLT x149 = x121 * x146; const FLT x150 = x97 + (x109 * x149); const FLT x151 = 1. / sqrt(1 + (-1 * (x150 * x150))); const FLT x152 = x151 * ((x142 * x149) + (x129 * x148) + (x95 * x124) + x92 + (-1 * x145 * ((-1 * x118 * ((x129 * x144) + (x104 * x142) + (x104 * ((x129 * x131) + x142 + (x104 * (x140 + (x129 * x132) + (x104 * ((x129 * x137) + (-1 * x129 * x134) + x136)))))) + (x129 * x143))) + (-1 * x95 * x126)))); const FLT x153 = cos(x96 + (-1 * asin(x150)) + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x154 = x88 + (-1 * x89); const FLT x155 = x71 * (*lh_p).Rot[2]; const FLT x156 = x87 * (*lh_p).Rot[0]; const FLT x157 = x156 + x155; const FLT x158 = (x80 * x157) + (x79 * x154); const FLT x159 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x160 = -1 * x1 * x159; const FLT x161 = x5 + x160; const FLT x162 = (x91 * x161) + (-1 * x86 * x158); const FLT x163 = ((-1 * x74 * x157) + (x69 * x154)) * x77; const FLT x164 = x163 + (-1 * x94 * x162); const FLT x165 = (x103 * x161) + (-1 * x128 * (x158 + (x127 * x161))); const FLT x166 = x165 * x135; const FLT x167 = (x165 * x139) + (x104 * ((-1 * x165 * x138) + x166)); const FLT x168 = (x104 * x167) + (x165 * x141); const FLT x169 = x151 * (x162 + (x165 * x148) + (-1 * x145 * ((-1 * x118 * ((x165 * x144) + (x165 * x143) + (x104 * ((x165 * x131) + x168 + (x104 * (x167 + (x165 * x132) + (x104 * ((x165 * x137) + (-1 * x165 * x134) + x166)))))) + (x104 * x168))) + (-1 * x126 * x164))) + (x168 * x149) + (x124 * x164)); const FLT x170 = x155 + (-1 * x156); const FLT x171 = x70 + x72; const FLT x172 = x2 + x4 + x160; const FLT x173 = (x80 * x172) + (x79 * x171); const FLT x174 = (x103 * x170) + (-1 * x128 * (x173 + (x127 * x170))); const FLT x175 = (x91 * x170) + (-1 * x86 * x173); const FLT x176 = ((-1 * x74 * x172) + (x69 * x171)) * x77; const FLT x177 = x176 + (-1 * x94 * x175); const FLT x178 = x174 * x135; const FLT x179 = (x174 * x139) + (x104 * ((-1 * x174 * x138) + x178)); const FLT x180 = (x104 * x179) + (x174 * x141); const FLT x181 = x151 * (x175 + (x124 * x177) + (x180 * x149) + (x174 * x148) + (-1 * x145 * ((-1 * x118 * ((x104 * x180) + (x104 * (x180 + (x174 * x131) + (x104 * (x179 + (x174 * x132) + (x104 * ((x174 * x137) + (-1 * x174 * x134) + x178)))))) + (x174 * x144) + (x174 * x143))) + (-1 * x126 * x177)))); const FLT x182 = x38 * x20; const FLT x183 = 1.0/2.0 * x13; const FLT x184 = x35 * x183; const FLT x185 = 1. / (x15 * sqrt(x15)); const FLT x186 = 1.0 * x13; const FLT x187 = 1.0 * x11; const FLT x188 = 1.0 * x14; const FLT x189 = 1.0 * x12; const FLT x190 = (x189 * (*_x0).Pose.Rot[3]) + (x188 * (*_x0).Pose.Rot[0]) + (-1 * x186 * (*_x0).Pose.Rot[1]) + (-1 * x187 * (*_x0).Pose.Rot[2]); const FLT x191 = x185 * x190; const FLT x192 = x184 * x191; const FLT x193 = x7 * x38; const FLT x194 = 1.0/2.0 * x41; const FLT x195 = x191 * x194; const FLT x196 = 1.0/2.0 * x11; const FLT x197 = x35 * x196; const FLT x198 = x38 * x23; const FLT x199 = x197 * x198; const FLT x200 = x33 * x185; const FLT x201 = x200 * x190; const FLT x202 = 1.0/2.0 * x14; const FLT x203 = x35 * x202; const FLT x204 = 0.5 * x39; const FLT x205 = x20 * x204; const FLT x206 = x23 * x204; const FLT x207 = 0.5 * x45; const FLT x208 = x204 * (*_x0).Pose.Rot[3]; const FLT x209 = (-1 * x7 * x208) + (x207 * (*_x0).Pose.Rot[0]) + (-1 * x205 * (*_x0).Pose.Rot[1]) + (-1 * x206 * (*_x0).Pose.Rot[2]); const FLT x210 = (-1 * x201 * x203) + x209 + (-1 * x191 * x199) + (-1 * x182 * x192) + (x193 * x195); const FLT x211 = 2 * x58; const FLT x212 = x203 * x191; const FLT x213 = x205 * (*_x0).Pose.Rot[2]; const FLT x214 = x206 * (*_x0).Pose.Rot[1]; const FLT x215 = x207 * (*_x0).Pose.Rot[3]; const FLT x216 = x191 * x197; const FLT x217 = x7 * x204; const FLT x218 = x217 * (*_x0).Pose.Rot[0]; const FLT x219 = x218 + x213 + (-1 * x198 * x192) + (-1 * x212 * x193) + (-1 * x214) + x215 + (-1 * x201 * x194) + (x216 * x182); const FLT x220 = x217 * (*_x0).Pose.Rot[2]; const FLT x221 = x23 * x208; const FLT x222 = x205 * (*_x0).Pose.Rot[0]; const FLT x223 = x207 * (*_x0).Pose.Rot[1]; const FLT x224 = (-1 * x223) + (-1 * x222) + x220 + (-1 * x221); const FLT x225 = x224 + (x212 * x182) + (-1 * x201 * x184) + (x198 * x195) + (x216 * x193); const FLT x226 = (x210 * sensor_pt[1]) + (-1 * x219 * sensor_pt[0]) + (x225 * sensor_pt[2]); const FLT x227 = 2 * x54; const FLT x228 = x184 * x193; const FLT x229 = x203 * x198; const FLT x230 = x20 * x208; const FLT x231 = x206 * (*_x0).Pose.Rot[0]; const FLT x232 = x217 * (*_x0).Pose.Rot[1]; const FLT x233 = x207 * (*_x0).Pose.Rot[2]; const FLT x234 = (-1 * x232) + (-1 * x233) + x230 + (-1 * x231); const FLT x235 = (-1 * x201 * x197) + (-1 * x182 * x195) + x234 + (x229 * x191) + (-1 * x228 * x191); const FLT x236 = 2 * x60; const FLT x237 = (x219 * sensor_pt[2]) + (-1 * x235 * sensor_pt[1]) + (x225 * sensor_pt[0]); const FLT x238 = 2 * x52; const FLT x239 = (x238 * x237) + (x236 * x235) + (-1 * x211 * x210) + (-1 * x227 * x226); const FLT x240 = 2 * x57; const FLT x241 = (x235 * sensor_pt[0]) + (-1 * x210 * sensor_pt[2]) + (x225 * sensor_pt[1]); const FLT x242 = 2 * x55; const FLT x243 = (x210 * x242) + (x227 * x241) + (-1 * x237 * x240) + (-1 * x219 * x236); const FLT x244 = (x226 * x240) + (-1 * x238 * x241) + (-1 * x235 * x242) + (x211 * x219); const FLT x245 = 2 * ((x239 * (*lh_p).Rot[0]) + (x244 * (*lh_p).Rot[3]) + (-1 * x243 * (*lh_p).Rot[1])); const FLT x246 = 2 * ((x239 * (*lh_p).Rot[1]) + (-1 * x244 * (*lh_p).Rot[2]) + (x243 * (*lh_p).Rot[0])); const FLT x247 = x244 + (-1 * x245 * (*lh_p).Rot[3]) + (x246 * (*lh_p).Rot[2]); const FLT x248 = 2 * ((-1 * x239 * (*lh_p).Rot[3]) + (x243 * (*lh_p).Rot[2]) + (x244 * (*lh_p).Rot[0])); const FLT x249 = x243 + (-1 * x248 * (*lh_p).Rot[2]) + (x245 * (*lh_p).Rot[1]); const FLT x250 = (x80 * x249) + (x79 * x247); const FLT x251 = x239 + (-1 * x246 * (*lh_p).Rot[1]) + (x248 * (*lh_p).Rot[3]); const FLT x252 = (x91 * x251) + (-1 * x86 * x250); const FLT x253 = ((-1 * x74 * x249) + (x69 * x247)) * x77; const FLT x254 = x253 + (-1 * x94 * x252); const FLT x255 = (x251 * x103) + (-1 * x128 * (x250 + (x251 * x127))); const FLT x256 = x255 * x135; const FLT x257 = (x255 * x139) + (x104 * ((-1 * x255 * x138) + x256)); const FLT x258 = (x257 * x104) + (x255 * x141); const FLT x259 = x151 * (x252 + (x258 * x149) + (x255 * x148) + (-1 * x145 * ((-1 * x118 * ((x255 * x144) + (x104 * (x258 + (x255 * x131) + (x104 * (x257 + (x255 * x132) + (x104 * ((x255 * x137) + x256 + (-1 * x255 * x134))))))) + (x255 * x143) + (x258 * x104))) + (-1 * x254 * x126))) + (x254 * x124)); const FLT x260 = (x189 * (*_x0).Pose.Rot[0]) + (x187 * (*_x0).Pose.Rot[1]) + (-1 * x188 * (*_x0).Pose.Rot[3]) + (-1 * x186 * (*_x0).Pose.Rot[2]); const FLT x261 = x260 * x185; const FLT x262 = x261 * x184; const FLT x263 = x261 * x194; const FLT x264 = x38 * x263; const FLT x265 = x261 * x197; const FLT x266 = x200 * x260; const FLT x267 = (-1 * x215) + x214 + (-1 * x218) + (-1 * x213); const FLT x268 = (-1 * x203 * x266) + (-1 * x265 * x198) + x267 + (-1 * x262 * x182) + (x7 * x264); const FLT x269 = x203 * x182; const FLT x270 = x234 + (x23 * x264) + (x269 * x261) + (x265 * x193) + (-1 * x266 * x184); const FLT x271 = x223 + (-1 * x266 * x197) + x222 + (-1 * x220) + (-1 * x261 * x228) + x221 + (-1 * x263 * x182) + (x261 * x229); const FLT x272 = (-1 * x268 * sensor_pt[2]) + (x271 * sensor_pt[0]) + (x270 * sensor_pt[1]); const FLT x273 = (x265 * x182) + (-1 * x266 * x194) + x209 + (-1 * x203 * x261 * x193) + (-1 * x262 * x198); const FLT x274 = (x268 * sensor_pt[1]) + (-1 * x273 * sensor_pt[0]) + (x270 * sensor_pt[2]); const FLT x275 = (x211 * x273) + (x274 * x240) + (-1 * x238 * x272) + (-1 * x271 * x242); const FLT x276 = (x273 * sensor_pt[2]) + (x270 * sensor_pt[0]) + (-1 * x271 * sensor_pt[1]); const FLT x277 = (x272 * x227) + (-1 * x276 * x240) + (x268 * x242) + (-1 * x236 * x273); const FLT x278 = (x236 * x271) + (x238 * x276) + (-1 * x211 * x268) + (-1 * x274 * x227); const FLT x279 = (-1 * x275 * (*lh_p).Rot[2]) + (x278 * (*lh_p).Rot[1]) + (x277 * (*lh_p).Rot[0]); const FLT x280 = 2 * (*lh_p).Rot[2]; const FLT x281 = (x275 * (*lh_p).Rot[3]) + (-1 * x277 * (*lh_p).Rot[1]) + (x278 * (*lh_p).Rot[0]); const FLT x282 = 2 * (*lh_p).Rot[3]; const FLT x283 = x275 + (x279 * x280) + (-1 * x282 * x281); const FLT x284 = (-1 * x278 * (*lh_p).Rot[3]) + (x277 * (*lh_p).Rot[2]) + (x275 * (*lh_p).Rot[0]); const FLT x285 = 2 * (*lh_p).Rot[1]; const FLT x286 = (-1 * x284 * x280) + x277 + (x285 * x281); const FLT x287 = (x80 * x286) + (x79 * x283); const FLT x288 = x278 + (-1 * x279 * x285) + (x282 * x284); const FLT x289 = (x91 * x288) + (-1 * x86 * x287); const FLT x290 = ((-1 * x74 * x286) + (x69 * x283)) * x77; const FLT x291 = x290 + (-1 * x94 * x289); const FLT x292 = (x288 * x103) + (-1 * x128 * (x287 + (x288 * x127))); const FLT x293 = x292 * x135; const FLT x294 = (x292 * x139) + (x104 * ((-1 * x292 * x138) + x293)); const FLT x295 = (x294 * x104) + (x292 * x141); const FLT x296 = x151 * ((x295 * x149) + (x292 * x148) + x289 + (x291 * x124) + (-1 * x145 * ((-1 * x118 * ((x295 * x104) + (x104 * (x295 + (x292 * x131) + (x104 * (x294 + (x292 * x132) + (x104 * ((x292 * x137) + x293 + (-1 * x292 * x134))))))) + (x292 * x144) + (x292 * x143))) + (-1 * x291 * x126)))); const FLT x297 = (-1 * x189 * (*_x0).Pose.Rot[1]) + (x187 * (*_x0).Pose.Rot[0]) + (x188 * (*_x0).Pose.Rot[2]) + (-1 * x186 * (*_x0).Pose.Rot[3]); const FLT x298 = x297 * x185; const FLT x299 = x298 * x184; const FLT x300 = x298 * x193; const FLT x301 = x200 * x297; const FLT x302 = x35 * x301; const FLT x303 = (-1 * x202 * x302) + (x300 * x194) + x233 + (-1 * x298 * x199) + x231 + x232 + (-1 * x299 * x182) + (-1 * x230); const FLT x304 = x298 * x197; const FLT x305 = x298 * x194; const FLT x306 = x267 + (-1 * x302 * x183) + (x298 * x269) + (x305 * x198) + (x304 * x193); const FLT x307 = x209 + (x298 * x229) + (-1 * x298 * x228) + (-1 * x305 * x182) + (-1 * x302 * x196); const FLT x308 = (x307 * sensor_pt[0]) + (-1 * x303 * sensor_pt[2]) + (x306 * sensor_pt[1]); const FLT x309 = (-1 * x301 * x194) + x224 + (-1 * x299 * x198) + (x304 * x182) + (-1 * x203 * x300); const FLT x310 = (-1 * x309 * sensor_pt[0]) + (x303 * sensor_pt[1]) + (x306 * sensor_pt[2]); const FLT x311 = (-1 * x242 * x307) + (-1 * x238 * x308) + (x211 * x309) + (x240 * x310); const FLT x312 = (x309 * sensor_pt[2]) + (-1 * x307 * sensor_pt[1]) + (x306 * sensor_pt[0]); const FLT x313 = (x242 * x303) + (x227 * x308) + (-1 * x240 * x312) + (-1 * x236 * x309); const FLT x314 = (x236 * x307) + (-1 * x211 * x303) + (x238 * x312) + (-1 * x227 * x310); const FLT x315 = (x314 * (*lh_p).Rot[1]) + (-1 * x311 * (*lh_p).Rot[2]) + (x313 * (*lh_p).Rot[0]); const FLT x316 = (x311 * (*lh_p).Rot[3]) + (-1 * x313 * (*lh_p).Rot[1]) + (x314 * (*lh_p).Rot[0]); const FLT x317 = (x280 * x315) + x311 + (-1 * x282 * x316); const FLT x318 = (x313 * (*lh_p).Rot[2]) + (-1 * x314 * (*lh_p).Rot[3]) + (x311 * (*lh_p).Rot[0]); const FLT x319 = x313 + (-1 * x280 * x318) + (x285 * x316); const FLT x320 = (x80 * x319) + (x79 * x317); const FLT x321 = x314 + (x282 * x318) + (-1 * x285 * x315); const FLT x322 = (x91 * x321) + (-1 * x86 * x320); const FLT x323 = ((-1 * x74 * x319) + (x69 * x317)) * x77; const FLT x324 = x323 + (-1 * x94 * x322); const FLT x325 = (x321 * x103) + (-1 * x128 * (x320 + (x321 * x127))); const FLT x326 = x325 * x135; const FLT x327 = (x325 * x139) + (x104 * ((-1 * x325 * x138) + x326)); const FLT x328 = (x327 * x104) + (x325 * x141); const FLT x329 = x151 * (x322 + (x328 * x149) + (x325 * x148) + (-1 * x145 * ((-1 * x118 * ((x325 * x144) + (x328 * x104) + (x104 * ((x325 * x131) + x328 + (x104 * ((x325 * x132) + x327 + (x104 * ((x325 * x137) + x326 + (-1 * x325 * x134))))))) + (x325 * x143))) + (-1 * x324 * x126))) + (x324 * x124)); const FLT x330 = 2 * x0; const FLT x331 = -1 * x330; const FLT x332 = 2 * x3; const FLT x333 = 1 + (-1 * x332); const FLT x334 = x333 + x331; const FLT x335 = x280 * (*lh_p).Rot[0]; const FLT x336 = x285 * (*lh_p).Rot[3]; const FLT x337 = x336 + (-1 * x335); const FLT x338 = (x80 * x337) + (x79 * x334); const FLT x339 = x285 * (*lh_p).Rot[2]; const FLT x340 = x282 * (*lh_p).Rot[0]; const FLT x341 = x340 + x339; const FLT x342 = (x91 * x341) + (-1 * x86 * x338); const FLT x343 = ((-1 * x74 * x337) + (x69 * x334)) * x77; const FLT x344 = x343 + (-1 * x94 * x342); const FLT x345 = (x341 * x103) + (-1 * x128 * (x338 + (x341 * x127))); const FLT x346 = x345 * x135; const FLT x347 = (x345 * x139) + (x104 * ((-1 * x345 * x138) + x346)); const FLT x348 = (x347 * x104) + (x345 * x141); const FLT x349 = x151 * (x342 + (-1 * x145 * ((-1 * x118 * ((x348 * x104) + (x104 * (x348 + (x345 * x131) + (x104 * (x347 + (x345 * x132) + (x104 * (x346 + (x345 * x137) + (-1 * x345 * x134))))))) + (x345 * x144) + (x345 * x143))) + (-1 * x344 * x126))) + (x348 * x149) + (x344 * x124) + (x345 * x148)); const FLT x350 = 2 * x159; const FLT x351 = -1 * x350; const FLT x352 = x333 + x351; const FLT x353 = x339 + (-1 * x340); const FLT x354 = x282 * (*lh_p).Rot[2]; const FLT x355 = x285 * (*lh_p).Rot[0]; const FLT x356 = x355 + x354; const FLT x357 = (x80 * x356) + (x79 * x353); const FLT x358 = (x352 * x103) + (-1 * x128 * (x357 + (x352 * x127))); const FLT x359 = (x91 * x352) + (-1 * x86 * x357); const FLT x360 = ((-1 * x74 * x356) + (x69 * x353)) * x77; const FLT x361 = x360 + (-1 * x94 * x359); const FLT x362 = x358 * x135; const FLT x363 = (x358 * x139) + (x104 * ((-1 * x358 * x138) + x362)); const FLT x364 = (x363 * x104) + (x358 * x141); const FLT x365 = x151 * ((x364 * x149) + (-1 * x145 * ((-1 * x118 * ((x364 * x104) + (x104 * (x364 + (x358 * x131) + (x104 * (x363 + (x358 * x132) + (x104 * ((x358 * x137) + x362 + (-1 * x358 * x134))))))) + (x358 * x144) + (x358 * x143))) + (-1 * x361 * x126))) + x359 + (x358 * x148) + (x361 * x124)); const FLT x366 = x335 + x336; const FLT x367 = 1 + x351 + x331; const FLT x368 = ((-1 * x74 * x367) + (x69 * x366)) * x77; const FLT x369 = (x80 * x367) + (x79 * x366); const FLT x370 = x354 + (-1 * x355); const FLT x371 = (x91 * x370) + (-1 * x86 * x369); const FLT x372 = x368 + (-1 * x94 * x371); const FLT x373 = (x370 * x103) + (-1 * x128 * (x369 + (x370 * x127))); const FLT x374 = x373 * x130; const FLT x375 = x374 * x106; const FLT x376 = (x374 * x107) + (x104 * ((-1 * x374 * x105) + x375)); const FLT x377 = (x376 * x104) + (x374 * x108); const FLT x378 = x151 * (x371 + (x377 * x149) + (x372 * x124) + (x374 * x147) + (-1 * x145 * ((-1 * x118 * ((x373 * x144) + (x374 * x114) + (x377 * x104) + (x104 * (x377 + (x374 * x113) + (x104 * ((x374 * x112) + x376 + (x104 * ((x373 * x137) + x375 + (-1 * x374 * x133))))))))) + (-1 * x372 * x126)))); const FLT x379 = dt * dt * dt; const FLT x380 = 0.5 * x31 * x379; const FLT x381 = x56 * x380; const FLT x382 = x20 * x20 * x20; const FLT x383 = dt * dt * dt * dt; const FLT x384 = 1.0 * x33; const FLT x385 = x29 * (1. / (x26 * sqrt(x26))); const FLT x386 = x384 * x385; const FLT x387 = x383 * x386; const FLT x388 = 2 * x20; const FLT x389 = x32 * x17; const FLT x390 = x30 * (1. / (x26 * x26)); const FLT x391 = 2 * x390; const FLT x392 = x391 * x383; const FLT x393 = x390 * x388; const FLT x394 = x24 * x383; const FLT x395 = x394 * x386; const FLT x396 = x37 * x17; const FLT x397 = x396 * x384; const FLT x398 = (-1 * x392 * x382) + (-1 * x394 * x393) + (-1 * x20 * x397) + (x389 * x388) + (x20 * x18 * x387) + (x382 * x387) + (x20 * x395) + (-1 * x18 * x393 * x383); const FLT x399 = 1. / (x34 * sqrt(x34)); const FLT x400 = x16 * x399; const FLT x401 = x400 * x398; const FLT x402 = x202 * x198; const FLT x403 = 1.0/2.0 * x12; const FLT x404 = x403 * x182; const FLT x405 = x400 * x183; const FLT x406 = x405 * x398; const FLT x407 = 0.5 * x396; const FLT x408 = x36 * x407; const FLT x409 = x11 * x408; const FLT x410 = x379 * x385; const FLT x411 = x42 * x410; const FLT x412 = x44 * x399; const FLT x413 = 1.0/2.0 * x412; const FLT x414 = x413 * x398; const FLT x415 = x47 * x410; const FLT x416 = x20 * x415; const FLT x417 = x53 * x380; const FLT x418 = x20 * x417; const FLT x419 = (-1 * x23 * x418) + (x23 * x416); const FLT x420 = x36 * x410; const FLT x421 = x13 * x420; const FLT x422 = x20 * x421; const FLT x423 = x46 * x380; const FLT x424 = x20 * x423; const FLT x425 = (x7 * x424) + (-1 * x7 * x422); const FLT x426 = x419 + x43 + (-1 * x11 * x414) + (-1 * x21 * x411) + x425 + (-1 * x20 * x409) + (x21 * x381) + (x402 * x401) + (-1 * x401 * x404) + (-1 * x406 * x193); const FLT x427 = x401 * x198; const FLT x428 = x202 * x182; const FLT x429 = x20 * x411; const FLT x430 = x23 * x429; const FLT x431 = x401 * x193; const FLT x432 = x20 * x381; const FLT x433 = x23 * x432; const FLT x434 = x13 * x408; const FLT x435 = -1 * x48; const FLT x436 = x11 * x420; const FLT x437 = x20 * x436; const FLT x438 = x51 * x380; const FLT x439 = x20 * x438; const FLT x440 = (-1 * x7 * x439) + (x7 * x437); const FLT x441 = x435 + (-1 * x20 * x434) + (x21 * x415) + (x403 * x427) + (-1 * x13 * x414) + (-1 * x21 * x417) + (x401 * x428) + x430 + x440 + (-1 * x433) + (x431 * x196); const FLT x442 = -1 * x40; const FLT x443 = x42 * x407; const FLT x444 = x7 * x418; const FLT x445 = x182 * x196; const FLT x446 = x7 * x416; const FLT x447 = (-1 * x23 * x422) + (x23 * x424); const FLT x448 = (x21 * x436) + (-1 * x431 * x202) + (-1 * x446) + (-1 * x406 * x198) + (-1 * x12 * x414) + (-1 * x20 * x443) + (x401 * x445) + x447 + (-1 * x21 * x438) + x442 + x444; const FLT x449 = (x448 * sensor_pt[2]) + (-1 * x426 * sensor_pt[1]) + (x441 * sensor_pt[0]); const FLT x450 = x412 * x202; const FLT x451 = x47 * x407; const FLT x452 = x23 * x439; const FLT x453 = x7 * x429; const FLT x454 = x23 * x437; const FLT x455 = x7 * x432; const FLT x456 = (-1 * x427 * x196) + (-1 * x450 * x398) + (-1 * x455) + (x403 * x431) + (-1 * x20 * x451) + x452 + x50 + x453 + (x21 * x423) + (-1 * x21 * x421) + (-1 * x406 * x182) + (-1 * x454); const FLT x457 = (x426 * sensor_pt[0]) + (-1 * x456 * sensor_pt[2]) + (x441 * sensor_pt[1]); const FLT x458 = (-1 * x448 * x236) + (x457 * x227) + (-1 * x449 * x240) + (x456 * x242); const FLT x459 = (x456 * sensor_pt[1]) + (-1 * x448 * sensor_pt[0]) + (x441 * sensor_pt[2]); const FLT x460 = (x449 * x238) + (x426 * x236) + (-1 * x456 * x211) + (-1 * x459 * x227); const FLT x461 = (x459 * x240) + (-1 * x426 * x242) + (-1 * x457 * x238) + (x448 * x211); const FLT x462 = (x461 * (*lh_p).Rot[3]) + (-1 * x458 * (*lh_p).Rot[1]) + (x460 * (*lh_p).Rot[0]); const FLT x463 = (x460 * (*lh_p).Rot[1]) + (-1 * x461 * (*lh_p).Rot[2]) + (x458 * (*lh_p).Rot[0]); const FLT x464 = x461 + (-1 * x462 * x282) + (x463 * x280); const FLT x465 = (-1 * x460 * (*lh_p).Rot[3]) + (x458 * (*lh_p).Rot[2]) + (x461 * (*lh_p).Rot[0]); const FLT x466 = x458 + (-1 * x465 * x280) + (x462 * x285); const FLT x467 = (x80 * x466) + (x79 * x464); const FLT x468 = x460 + (-1 * x463 * x285) + (x465 * x282); const FLT x469 = (x91 * x468) + (-1 * x86 * x467); const FLT x470 = ((-1 * x74 * x466) + (x69 * x464)) * x77; const FLT x471 = x470 + (-1 * x94 * x469); const FLT x472 = (x468 * x103) + (-1 * x128 * (x467 + (x468 * x127))); const FLT x473 = x472 * x135; const FLT x474 = (x472 * x139) + (x104 * ((-1 * x472 * x138) + x473)); const FLT x475 = (x474 * x104) + (x472 * x141); const FLT x476 = x151 * ((-1 * x145 * ((-1 * x118 * ((x472 * x144) + (x472 * x143) + (x104 * (x475 + (x472 * x131) + (x104 * ((x472 * x132) + x474 + (x104 * ((x472 * x137) + x473 + (-1 * x472 * x134))))))) + (x475 * x104))) + (-1 * x471 * x126))) + (x475 * x149) + (x471 * x124) + x469 + (x472 * x148)); const FLT x477 = x21 * x387; const FLT x478 = x21 * x392; const FLT x479 = x23 * x18; const FLT x480 = (x23 * x23 * x23) * x383; const FLT x481 = 2 * x389; const FLT x482 = (-1 * x480 * x391) + (x23 * x481) + (x480 * x386) + (-1 * x23 * x478) + (-1 * x479 * x392) + (x23 * x477) + (-1 * x23 * x397) + (x479 * x387); const FLT x483 = x482 * x400; const FLT x484 = x483 * x196; const FLT x485 = x482 * x405; const FLT x486 = x24 * x410; const FLT x487 = x36 * x486; const FLT x488 = x403 * x193; const FLT x489 = x7 * x23; const FLT x490 = (-1 * x489 * x381) + (x489 * x411); const FLT x491 = x447 + (x483 * x488) + (-1 * x450 * x482) + x40 + (x24 * x438) + (-1 * x485 * x182) + (-1 * x23 * x451) + x490 + (-1 * x484 * x198) + (-1 * x11 * x487); const FLT x492 = -1 * x43; const FLT x493 = x489 * x438; const FLT x494 = x489 * x436; const FLT x495 = x482 * x413; const FLT x496 = x403 * x198; const FLT x497 = (x483 * x428) + (-1 * x23 * x434) + (x42 * x486) + x419 + (-1 * x24 * x381) + (x496 * x483) + (-1 * x13 * x495) + x492 + (x484 * x193) + (-1 * x493) + x494; const FLT x498 = (x489 * x423) + (-1 * x489 * x421); const FLT x499 = (-1 * x23 * x409) + (-1 * x24 * x417) + (-1 * x430) + (-1 * x485 * x193) + (x47 * x486) + x435 + (-1 * x11 * x495) + (x483 * x402) + x498 + x433 + (-1 * x483 * x404); const FLT x500 = (x499 * sensor_pt[0]) + (-1 * x491 * sensor_pt[2]) + (x497 * sensor_pt[1]); const FLT x501 = x489 * x417; const FLT x502 = x202 * x193; const FLT x503 = x489 * x415; const FLT x504 = (x483 * x445) + x50 + (x24 * x423) + x501 + (-1 * x485 * x198) + (-1 * x452) + (-1 * x503) + (-1 * x483 * x502) + (-1 * x13 * x487) + (-1 * x23 * x443) + x454 + (-1 * x12 * x495); const FLT x505 = (x491 * sensor_pt[1]) + (-1 * x504 * sensor_pt[0]) + (x497 * sensor_pt[2]); const FLT x506 = (x504 * x211) + (x505 * x240) + (-1 * x500 * x238) + (-1 * x499 * x242); const FLT x507 = (x504 * sensor_pt[2]) + (-1 * x499 * sensor_pt[1]) + (x497 * sensor_pt[0]); const FLT x508 = (-1 * x504 * x236) + (x500 * x227) + (-1 * x507 * x240) + (x491 * x242); const FLT x509 = (x507 * x238) + (-1 * x491 * x211) + (-1 * x505 * x227) + (x499 * x236); const FLT x510 = (x509 * (*lh_p).Rot[1]) + (-1 * x506 * (*lh_p).Rot[2]) + (x508 * (*lh_p).Rot[0]); const FLT x511 = 2 * ((x508 * (*lh_p).Rot[2]) + (-1 * x509 * (*lh_p).Rot[3]) + (x506 * (*lh_p).Rot[0])); const FLT x512 = (-1 * x510 * x285) + x509 + (x511 * (*lh_p).Rot[3]); const FLT x513 = 2 * ((x506 * (*lh_p).Rot[3]) + (x509 * (*lh_p).Rot[0]) + (-1 * x508 * (*lh_p).Rot[1])); const FLT x514 = x506 + (-1 * x513 * (*lh_p).Rot[3]) + (x510 * x280); const FLT x515 = (-1 * x511 * (*lh_p).Rot[2]) + x508 + (x513 * (*lh_p).Rot[1]); const FLT x516 = (x80 * x515) + (x79 * x514); const FLT x517 = (x512 * x103) + (-1 * x128 * (x516 + (x512 * x127))); const FLT x518 = (x91 * x512) + (-1 * x86 * x516); const FLT x519 = ((-1 * x74 * x515) + (x69 * x514)) * x77; const FLT x520 = x519 + (-1 * x94 * x518); const FLT x521 = x517 * x135; const FLT x522 = (x517 * x139) + (x104 * ((-1 * x517 * x138) + x521)); const FLT x523 = (x522 * x104) + (x517 * x141); const FLT x524 = x151 * (x518 + (x523 * x149) + (-1 * x145 * ((-1 * x118 * ((x517 * x144) + (x523 * x104) + (x104 * (x523 + (x517 * x131) + (x104 * ((x517 * x132) + x522 + (x104 * ((x517 * x137) + (-1 * x517 * x134) + x521)))))) + (x517 * x143))) + (-1 * x520 * x126))) + (x517 * x148) + (x520 * x124)); const FLT x525 = x7 * x7 * x7; const FLT x526 = (x7 * x395) + (-1 * x7 * x397) + (x7 * x477) + (-1 * x525 * x392) + (x7 * x481) + (-1 * x7 * x24 * x392) + (x525 * x387) + (-1 * x7 * x478); const FLT x527 = x400 * x526; const FLT x528 = x405 * x526; const FLT x529 = x412 * x526; const FLT x530 = 1.0/2.0 * x529; const FLT x531 = (-1 * x404 * x527) + (-1 * x528 * x193) + (x402 * x527) + x455 + x50 + (-1 * x453) + (-1 * x11 * x530) + x503 + (x18 * x423) + (-1 * x18 * x421) + (-1 * x7 * x409) + (-1 * x501); const FLT x532 = x527 * x196; const FLT x533 = x490 + x446 + (x496 * x527) + (x532 * x193) + (x428 * x527) + (x18 * x436) + (-1 * x13 * x530) + (-1 * x7 * x434) + (-1 * x444) + (-1 * x18 * x438) + x442; const FLT x534 = x498 + x440 + (-1 * x527 * x502) + (-1 * x18 * x415) + x48 + (-1 * x12 * x530) + (-1 * x528 * x198) + (-1 * x7 * x443) + (x18 * x417) + (x445 * x527); const FLT x535 = (x534 * sensor_pt[2]) + (-1 * x531 * sensor_pt[1]) + (x533 * sensor_pt[0]); const FLT x536 = (-1 * x18 * x381) + x425 + (x18 * x411) + (-1 * x528 * x182) + (-1 * x494) + (x488 * x527) + (-1 * x7 * x451) + x493 + (-1 * x532 * x198) + x492 + (-1 * x529 * x202); const FLT x537 = (x531 * sensor_pt[0]) + (-1 * x536 * sensor_pt[2]) + (x533 * sensor_pt[1]); const FLT x538 = (-1 * x534 * x236) + (x536 * x242) + (-1 * x535 * x240) + (x537 * x227); const FLT x539 = (x536 * sensor_pt[1]) + (-1 * x534 * sensor_pt[0]) + (x533 * sensor_pt[2]); const FLT x540 = (x535 * x238) + (x531 * x236) + (-1 * x536 * x211) + (-1 * x539 * x227); const FLT x541 = (x534 * x211) + (x539 * x240) + (-1 * x537 * x238) + (-1 * x531 * x242); const FLT x542 = (-1 * x538 * (*lh_p).Rot[1]) + (x541 * (*lh_p).Rot[3]) + (x540 * (*lh_p).Rot[0]); const FLT x543 = (x540 * (*lh_p).Rot[1]) + (-1 * x541 * (*lh_p).Rot[2]) + (x538 * (*lh_p).Rot[0]); const FLT x544 = x541 + (-1 * x542 * x282) + (x543 * x280); const FLT x545 = (x538 * (*lh_p).Rot[2]) + (-1 * x540 * (*lh_p).Rot[3]) + (x541 * (*lh_p).Rot[0]); const FLT x546 = x538 + (-1 * x545 * x280) + (x542 * x285); const FLT x547 = (x80 * x546) + (x79 * x544); const FLT x548 = (x545 * x282) + x540 + (-1 * x543 * x285); const FLT x549 = (x91 * x548) + (-1 * x86 * x547); const FLT x550 = ((-1 * x74 * x546) + (x69 * x544)) * x77; const FLT x551 = x550 + (-1 * x94 * x549); const FLT x552 = (x548 * x103) + (-1 * x128 * (x547 + (x548 * x127))); const FLT x553 = x552 * x135; const FLT x554 = (x552 * x139) + (x104 * ((-1 * x552 * x138) + x553)); const FLT x555 = (x554 * x104) + (x552 * x141); const FLT x556 = x151 * (x549 + (x555 * x149) + (x552 * x148) + (-1 * x145 * ((-1 * x118 * ((x555 * x104) + (x552 * x144) + (x104 * (x555 + (x552 * x131) + (x104 * (x554 + (x552 * x132) + (x104 * (x553 + (x552 * x137) + (-1 * x552 * x134))))))) + (x552 * x143))) + (-1 * x551 * x126))) + (x551 * x124)); const FLT x557 = dt * x339; const FLT x558 = dt * x340; const FLT x559 = x558 + x557; const FLT x560 = -1 * dt * x330; const FLT x561 = (-1 * dt * x332) + dt; const FLT x562 = x561 + x560; const FLT x563 = dt * x335; const FLT x564 = dt * x336; const FLT x565 = x564 + (-1 * x563); const FLT x566 = (x80 * x565) + (x79 * x562); const FLT x567 = (x559 * x103) + (-1 * x128 * (x566 + (x559 * x127))); const FLT x568 = x567 * x130; const FLT x569 = (x91 * x559) + (-1 * x86 * x566); const FLT x570 = ((-1 * x74 * x565) + (x69 * x562)) * x77; const FLT x571 = x570 + (-1 * x94 * x569); const FLT x572 = x568 * x106; const FLT x573 = (x567 * x139) + (x104 * ((-1 * x568 * x105) + x572)); const FLT x574 = (x573 * x104) + (x568 * x108); const FLT x575 = x151 * (x569 + (x574 * x149) + (x571 * x124) + (x568 * x147) + (-1 * x145 * ((-1 * x118 * ((x574 * x104) + (x104 * (x574 + (x568 * x113) + (x104 * ((x568 * x112) + x573 + (x104 * (x572 + (x567 * x137) + (-1 * x568 * x133))))))) + (x567 * x144) + (x568 * x114))) + (-1 * x571 * x126)))); const FLT x576 = x557 + (-1 * x558); const FLT x577 = dt * x354; const FLT x578 = dt * x355; const FLT x579 = x578 + x577; const FLT x580 = (x80 * x579) + (x79 * x576); const FLT x581 = -1 * dt * x350; const FLT x582 = x561 + x581; const FLT x583 = (x91 * x582) + (-1 * x86 * x580); const FLT x584 = ((-1 * x74 * x579) + (x69 * x576)) * x77; const FLT x585 = x99 * (x584 + (-1 * x94 * x583)); const FLT x586 = (x582 * x103) + (-1 * x128 * (x580 + (x582 * x127))); const FLT x587 = x586 * x135; const FLT x588 = x586 * x130; const FLT x589 = (x586 * x139) + (x104 * ((-1 * x588 * x105) + x587)); const FLT x590 = (x589 * x104) + (x586 * x141); const FLT x591 = x151 * (x583 + (x590 * x149) + (x588 * x147) + (-1 * x145 * ((-1 * x118 * ((x586 * x144) + (x590 * x104) + (x104 * (x590 + (x586 * x131) + (x104 * (x589 + (x586 * x132) + (x104 * ((x586 * x137) + x587 + (-1 * x588 * x133))))))) + (x586 * x143))) + (-1 * x585 * x125))) + (x585 * x123)); const FLT x592 = x563 + x564; const FLT x593 = x560 + dt + x581; const FLT x594 = (x80 * x593) + (x79 * x592); const FLT x595 = x577 + (-1 * x578); const FLT x596 = (x91 * x595) + (-1 * x86 * x594); const FLT x597 = ((-1 * x74 * x593) + (x69 * x592)) * x77; const FLT x598 = x597 + (-1 * x94 * x596); const FLT x599 = (x595 * x103) + (-1 * x128 * (x594 + (x595 * x127))); const FLT x600 = x599 * x135; const FLT x601 = (x599 * x139) + (x104 * ((-1 * x599 * x138) + x600)); const FLT x602 = (x601 * x104) + (x599 * x141); const FLT x603 = x151 * ((x602 * x149) + x596 + (x598 * x124) + (-1 * x145 * ((-1 * x118 * ((x602 * x104) + (x599 * x144) + (x104 * (x602 + (x599 * x131) + (x104 * (x601 + (x599 * x132) + (x104 * ((x599 * x137) + x600 + (-1 * x599 * x134))))))) + (x599 * x143))) + (-1 * x598 * x126))) + (x599 * x148)); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), (-1 * x153 * ((-1 * x78) + x152)) + x78 + (-1 * x152)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), (-1 * x169) + (-1 * ((-1 * x163) + x169) * x153) + x163); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), (-1 * x181) + (-1 * ((-1 * x176) + x181) * x153) + x176); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * ((-1 * x253) + x259) * x153) + (-1 * x259) + x253); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * ((-1 * x290) + x296) * x153) + (-1 * x296) + x290); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * ((-1 * x323) + x329) * x153) + (-1 * x329) + x323); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), (-1 * x349) + (-1 * ((-1 * x343) + x349) * x153) + x343); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), (-1 * ((-1 * x360) + x365) * x153) + (-1 * x365) + x360); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), (-1 * ((-1 * x368) + x378) * x153) + x368 + (-1 * x378)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (-1 * ((-1 * x470) + x476) * x153) + (-1 * x476) + x470); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (-1 * ((-1 * x519) + x524) * x153) + (-1 * x524) + x519); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (-1 * ((-1 * x550) + x556) * x153) + (-1 * x556) + x550); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), (-1 * ((-1 * x570) + x575) * x153) + (-1 * x575) + x570); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), (-1 * x591) + (-1 * ((-1 * x584) + x591) * x153) + x584); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), (-1 * x603) + (-1 * ((-1 * x597) + x603) * x153) + x597); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x6 = dt * dt; const FLT x7 = x6 * (x5 * x5); const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x6 * (x8 * x8); const FLT x10 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x11 = x6 * (x10 * x10); const FLT x12 = 1e-10 + x11 + x7 + x9; const FLT x13 = sqrt(x12); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x18 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x19 = (x15 * x15) * (1. / x12); const FLT x20 = cos(x14); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x4 * x4) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x11 * x19))); const FLT x22 = dt * x21 * x15 * (1. / x13); const FLT x23 = x5 * x22; const FLT x24 = x22 * x10; const FLT x25 = x20 * x21; const FLT x26 = x8 * x22; const FLT x27 = (x25 * x17) + (-1 * x26 * x18) + (-1 * x4 * x23) + (-1 * x24 * x16); const FLT x28 = x24 * x17; const FLT x29 = x25 * x16; const FLT x30 = x23 * x18; const FLT x31 = x4 * x26; const FLT x32 = x30 + x28 + (-1 * x31) + x29; const FLT x33 = x23 * x17; const FLT x34 = x24 * x18; const FLT x35 = x4 * x25; const FLT x36 = x26 * x16; const FLT x37 = x36 + x35 + x33 + (-1 * x34); const FLT x38 = (-1 * x37 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x32 * sensor_pt[2]); const FLT x39 = x4 * x24; const FLT x40 = x25 * x18; const FLT x41 = x23 * x16; const FLT x42 = x26 * x17; const FLT x43 = x42 + (-1 * x41) + x39 + x40; const FLT x44 = (-1 * x43 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x37 * sensor_pt[0]); const FLT x45 = (x0 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*_x0).Pose.Pos[2] + (2 * ((x43 * x44) + (-1 * x32 * x38))) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + sensor_pt[2]; const FLT x46 = (-1 * x32 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x43 * sensor_pt[1]); const FLT x47 = (*_x0).Pose.Pos[1] + (2 * ((x38 * x37) + (-1 * x43 * x46))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x0 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x48 = (x0 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x46 * x32) + (-1 * x44 * x37))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x49 = (-1 * x48 * (*lh_p).Rot[2]) + (x45 * (*lh_p).Rot[0]) + (x47 * (*lh_p).Rot[1]); const FLT x50 = (-1 * x47 * (*lh_p).Rot[3]) + (x48 * (*lh_p).Rot[0]) + (x45 * (*lh_p).Rot[2]); const FLT x51 = x47 + (*lh_p).Pos[1] + (2 * ((x50 * (*lh_p).Rot[3]) + (-1 * x49 * (*lh_p).Rot[1]))); const FLT x52 = x51 * x51; const FLT x53 = (-1 * x45 * (*lh_p).Rot[1]) + (x47 * (*lh_p).Rot[0]) + (x48 * (*lh_p).Rot[3]); const FLT x54 = x45 + (2 * ((x53 * (*lh_p).Rot[1]) + (-1 * x50 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x55 = x48 + (*lh_p).Pos[0] + (2 * ((x49 * (*lh_p).Rot[2]) + (-1 * x53 * (*lh_p).Rot[3]))); const FLT x56 = x55 * x55; const FLT x57 = x56 + (x54 * x54); const FLT x58 = x57 + x52; const FLT x59 = 0.523598775598299 + (*bsc0).tilt; const FLT x60 = cos(x59); const FLT x61 = 1. / x60; const FLT x62 = x61 * (1. / sqrt(x58)); const FLT x63 = asin(x62 * x51); const FLT x64 = 8.0108022e-06 * x63; const FLT x65 = -8.0108022e-06 + (-1 * x64); const FLT x66 = 0.0028679863 + (x63 * x65); const FLT x67 = 5.3685255e-06 + (x63 * x66); const FLT x68 = 0.0076069798 + (x63 * x67); const FLT x69 = x63 * x63; const FLT x70 = atan2(-1 * x54, x55); const FLT x71 = tan(x59); const FLT x72 = x71 * (1. / sqrt(x57)); const FLT x73 = x72 * x51; const FLT x74 = (-1 * asin(x73)) + (*bsc0).ogeephase + x70; const FLT x75 = (sin(x74) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x76 = x63 * x68; const FLT x77 = -8.0108022e-06 + (-1.60216044e-05 * x63); const FLT x78 = x66 + (x77 * x63); const FLT x79 = x67 + (x78 * x63); const FLT x80 = x68 + (x79 * x63); const FLT x81 = (x80 * x63) + x76; const FLT x82 = sin(x59); const FLT x83 = x82 * x75; const FLT x84 = x60 + (-1 * x81 * x83); const FLT x85 = 1. / x84; const FLT x86 = x85 * x75; const FLT x87 = x86 * x69; const FLT x88 = x73 + (x87 * x68); const FLT x89 = 1. / sqrt(1 + (-1 * (x88 * x88))); const FLT x90 = 1. / x57; const FLT x91 = 1. / sqrt(1 + (-1 * (x71 * x71) * x52 * x90)); const FLT x92 = (-1 * x30) + x31 + (-1 * x29) + (-1 * x28); const FLT x93 = 2 * x32; const FLT x94 = 1 + (x93 * x92) + (-2 * (x37 * x37)); const FLT x95 = x93 * x27; const FLT x96 = 2 * x43; const FLT x97 = (x96 * x37) + (-1 * x95); const FLT x98 = 2 * x37; const FLT x99 = x98 * x27; const FLT x100 = x99 + (-1 * x92 * x96); const FLT x101 = (-1 * x94 * (*lh_p).Rot[2]) + (x100 * (*lh_p).Rot[1]) + (x97 * (*lh_p).Rot[0]); const FLT x102 = 2 * (*lh_p).Rot[2]; const FLT x103 = 2 * ((x94 * (*lh_p).Rot[3]) + (-1 * x97 * (*lh_p).Rot[1]) + (x100 * (*lh_p).Rot[0])); const FLT x104 = x94 + (x101 * x102) + (-1 * x103 * (*lh_p).Rot[3]); const FLT x105 = 2 * x55; const FLT x106 = (x97 * (*lh_p).Rot[2]) + (-1 * x100 * (*lh_p).Rot[3]) + (x94 * (*lh_p).Rot[0]); const FLT x107 = (x103 * (*lh_p).Rot[1]) + x97 + (-1 * x102 * x106); const FLT x108 = 2 * x54; const FLT x109 = (x108 * x107) + (x105 * x104); const FLT x110 = 1.0/2.0 * x51; const FLT x111 = x71 * (1. / (x57 * sqrt(x57))) * x110; const FLT x112 = 2 * (*lh_p).Rot[3]; const FLT x113 = 2 * (*lh_p).Rot[1]; const FLT x114 = x100 + (x106 * x112) + (-1 * x101 * x113); const FLT x115 = (x72 * x114) + (-1 * x109 * x111); const FLT x116 = x54 * (1. / x56); const FLT x117 = 1. / x55; const FLT x118 = x56 * x90; const FLT x119 = ((-1 * x107 * x117) + (x104 * x116)) * x118; const FLT x120 = x119 + (-1 * x91 * x115); const FLT x121 = cos(x74) * (*bsc0).ogeemag; const FLT x122 = x81 * x82; const FLT x123 = x122 * x121; const FLT x124 = 1. / sqrt(1 + (-1 * (1. / (x60 * x60)) * x52 * (1. / x58))); const FLT x125 = 2 * x51; const FLT x126 = x61 * (1. / (x58 * sqrt(x58))) * x110; const FLT x127 = x124 * ((x62 * x114) + (-1 * x126 * (x109 + (x114 * x125)))); const FLT x128 = x65 * x127; const FLT x129 = (x66 * x127) + (x63 * ((-1 * x64 * x127) + x128)); const FLT x130 = (x63 * x129) + (x67 * x127); const FLT x131 = 2.40324066e-05 * x63; const FLT x132 = x68 * x69; const FLT x133 = (1. / (x84 * x84)) * x75 * x132; const FLT x134 = x85 * x132; const FLT x135 = x121 * x134; const FLT x136 = 2 * x86 * x76; const FLT x137 = x89 * (x115 + (x127 * x136) + (-1 * x133 * ((-1 * x83 * ((x63 * (x130 + (x79 * x127) + (x63 * ((x78 * x127) + x129 + (x63 * ((x77 * x127) + x128 + (-1 * x127 * x131))))))) + (x68 * x127) + (x80 * x127) + (x63 * x130))) + (-1 * x120 * x123))) + (x87 * x130) + (x120 * x135)); const FLT x138 = cos((-1 * asin(x88)) + x70 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x139 = (-1 * x33) + (-1 * x36) + x34 + (-1 * x35); const FLT x140 = x96 * x27; const FLT x141 = x140 + (-1 * x93 * x139); const FLT x142 = (x96 * x32) + (-1 * x99); const FLT x143 = 1 + (x98 * x139) + (-2 * (x43 * x43)); const FLT x144 = 2 * ((x143 * (*lh_p).Rot[1]) + (x141 * (*lh_p).Rot[0]) + (-1 * x142 * (*lh_p).Rot[2])); const FLT x145 = (-1 * x141 * (*lh_p).Rot[1]) + (x142 * (*lh_p).Rot[3]) + (x143 * (*lh_p).Rot[0]); const FLT x146 = x142 + (x144 * (*lh_p).Rot[2]) + (-1 * x112 * x145); const FLT x147 = (x141 * (*lh_p).Rot[2]) + (-1 * x143 * (*lh_p).Rot[3]) + (x142 * (*lh_p).Rot[0]); const FLT x148 = x141 + (x113 * x145) + (-1 * x102 * x147); const FLT x149 = (x108 * x148) + (x105 * x146); const FLT x150 = x143 + (x112 * x147) + (-1 * x144 * (*lh_p).Rot[1]); const FLT x151 = (x72 * x150) + (-1 * x111 * x149); const FLT x152 = ((-1 * x117 * x148) + (x116 * x146)) * x118; const FLT x153 = x121 * (x152 + (-1 * x91 * x151)); const FLT x154 = x124 * ((x62 * x150) + (-1 * x126 * (x149 + (x125 * x150)))); const FLT x155 = x65 * x154; const FLT x156 = (x66 * x154) + (x63 * ((-1 * x64 * x154) + x155)); const FLT x157 = (x63 * x156) + (x67 * x154); const FLT x158 = x89 * ((x87 * x157) + (x136 * x154) + x151 + (-1 * x133 * ((-1 * x83 * ((x68 * x154) + (x63 * (x157 + (x79 * x154) + (x63 * (x156 + (x78 * x154) + (x63 * ((x77 * x154) + (-1 * x131 * x154) + x155)))))) + (x80 * x154) + (x63 * x157))) + (-1 * x122 * x153))) + (x134 * x153)); const FLT x159 = x41 + (-1 * x42) + (-1 * x40) + (-1 * x39); const FLT x160 = x95 + (-1 * x98 * x159); const FLT x161 = 1 + (x96 * x159) + (-2 * (x32 * x32)); const FLT x162 = (x93 * x37) + (-1 * x140); const FLT x163 = 2 * ((x162 * (*lh_p).Rot[1]) + (-1 * x160 * (*lh_p).Rot[2]) + (x161 * (*lh_p).Rot[0])); const FLT x164 = (x160 * (*lh_p).Rot[3]) + (-1 * x161 * (*lh_p).Rot[1]) + (x162 * (*lh_p).Rot[0]); const FLT x165 = x160 + (x163 * (*lh_p).Rot[2]) + (-1 * x112 * x164); const FLT x166 = (x161 * (*lh_p).Rot[2]) + (-1 * x162 * (*lh_p).Rot[3]) + (x160 * (*lh_p).Rot[0]); const FLT x167 = x161 + (x113 * x164) + (-1 * x102 * x166); const FLT x168 = (x108 * x167) + (x105 * x165); const FLT x169 = x162 + (x112 * x166) + (-1 * x163 * (*lh_p).Rot[1]); const FLT x170 = (x72 * x169) + (-1 * x111 * x168); const FLT x171 = ((-1 * x117 * x167) + (x116 * x165)) * x118; const FLT x172 = x171 + (-1 * x91 * x170); const FLT x173 = x124 * ((x62 * x169) + (-1 * x126 * (x168 + (x125 * x169)))); const FLT x174 = x65 * x173; const FLT x175 = (x66 * x173) + (x63 * ((-1 * x64 * x173) + x174)); const FLT x176 = (x63 * x175) + (x67 * x173); const FLT x177 = x89 * ((x87 * x176) + x170 + (x173 * x136) + (-1 * x133 * ((-1 * x83 * ((x68 * x173) + (x63 * x176) + (x63 * (x176 + (x79 * x173) + (x63 * (x175 + (x78 * x173) + (x63 * ((x77 * x173) + x174 + (-1 * x173 * x131))))))) + (x80 * x173))) + (-1 * x123 * x172))) + (x172 * x135)); cnMatrixOptionalSet(Hx, 0, 0, (-1 * ((-1 * x119) + x137) * x138) + (-1 * x137) + x119); cnMatrixOptionalSet(Hx, 0, 1, (-1 * ((-1 * x152) + x158) * x138) + (-1 * x158) + x152); cnMatrixOptionalSet(Hx, 0, 2, (-1 * ((-1 * x171) + x177) * x138) + (-1 * x177) + x171); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x7 = (x6 * x6) * x5; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x5 * (x8 * x8); const FLT x10 = (x0 * x0) * 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 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x7 * x18) + (x9 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x20 * x19; const FLT x24 = x21 * x15; const FLT x25 = x21 * x16; const FLT x26 = (x8 * x25) + (-1 * x6 * x24) + (x0 * x22) + (x23 * x17); const FLT x27 = x21 * x17; const FLT x28 = (-1 * x8 * x27) + (x23 * x16) + (-1 * x6 * x22) + (-1 * x0 * x24); const FLT x29 = (x6 * x27) + (x0 * x25) + (-1 * x8 * x22) + (x23 * x15); const FLT x30 = (-1 * x29 * sensor_pt[0]) + (x28 * sensor_pt[2]) + (x26 * sensor_pt[1]); const FLT x31 = (x4 * x23) + (x6 * x25) + (x8 * x24) + (-1 * x0 * x27); const FLT x32 = (-1 * x31 * sensor_pt[1]) + (x28 * sensor_pt[0]) + (x29 * sensor_pt[2]); const FLT x33 = 2 * ((x32 * x31) + (-1 * x30 * x26)); const FLT x34 = dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1]); const FLT x35 = 1.0/2.0 * dt * fabs(dt); const FLT x36 = x35 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x37 = (*_x0).Pose.Pos[1] + x33 + (*error_model).Pose.Pos[1] + sensor_pt[1] + x34 + x36; const FLT x38 = x37 * (*lh_p).Rot[0]; const FLT x39 = (-1 * x26 * sensor_pt[2]) + (x28 * sensor_pt[1]) + (x31 * sensor_pt[0]); const FLT x40 = 2 * ((x30 * x29) + (-1 * x31 * x39)); const FLT x41 = dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0]); const FLT x42 = x35 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x43 = x42 + x40 + (*error_model).Pose.Pos[0] + x41 + (*_x0).Pose.Pos[0] + sensor_pt[0]; const FLT x44 = x43 * (*lh_p).Rot[3]; const FLT x45 = x35 * ((*_x0).Acc[2] + (*error_model).Acc[2]); const FLT x46 = 2 * ((x39 * x26) + (-1 * x32 * x29)); const FLT x47 = dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2]); const FLT x48 = (*error_model).Pose.Pos[2] + x46 + x45 + sensor_pt[2] + (*_x0).Pose.Pos[2] + x47; const FLT x49 = x48 * (*lh_p).Rot[1]; const FLT x50 = (-1 * x49) + x38 + x44; const FLT x51 = x48 * (*lh_p).Rot[0]; const FLT x52 = x37 * (*lh_p).Rot[1]; const FLT x53 = x43 * (*lh_p).Rot[2]; const FLT x54 = (-1 * x53) + x51 + x52; const FLT x55 = x43 + (*lh_p).Pos[0] + (2 * ((x54 * (*lh_p).Rot[2]) + (-1 * x50 * (*lh_p).Rot[3]))); const FLT x56 = x43 * (*lh_p).Rot[0]; const FLT x57 = x48 * (*lh_p).Rot[2]; const FLT x58 = x37 * (*lh_p).Rot[3]; const FLT x59 = (-1 * x58) + x56 + x57; const FLT x60 = x37 + (*lh_p).Pos[1] + (2 * ((x59 * (*lh_p).Rot[3]) + (-1 * x54 * (*lh_p).Rot[1]))); const FLT x61 = 0.523598775598299 + (*bsc0).tilt; const FLT x62 = tan(x61); const FLT x63 = x48 + (2 * ((x50 * (*lh_p).Rot[1]) + (-1 * x59 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x64 = x55 * x55; const FLT x65 = x64 + (x63 * x63); const FLT x66 = x62 * (1. / (x65 * sqrt(x65))); const FLT x67 = x60 * x66; const FLT x68 = x67 * x55; const FLT x69 = x60 * x60; const FLT x70 = x65 + x69; const FLT x71 = cos(x61); const FLT x72 = 1. / x71; const FLT x73 = (1. / sqrt(x70)) * x72; const FLT x74 = asin(x73 * x60); const FLT x75 = 8.0108022e-06 * x74; const FLT x76 = -8.0108022e-06 + (-1 * x75); const FLT x77 = 0.0028679863 + (x74 * x76); const FLT x78 = 5.3685255e-06 + (x74 * x77); const FLT x79 = 1. / sqrt(1 + (-1 * (1. / (x71 * x71)) * (1. / x70) * x69)); const FLT x80 = (1. / (x70 * sqrt(x70))) * x72; const FLT x81 = x80 * x60; const FLT x82 = x81 * x55; const FLT x83 = x82 * x79; const FLT x84 = x79 * x76; const FLT x85 = -1 * x82 * x84; const FLT x86 = x79 * x77; const FLT x87 = (-1 * x82 * x86) + (x74 * ((x83 * x75) + x85)); const FLT x88 = (x87 * x74) + (-1 * x83 * x78); const FLT x89 = x74 * x74; const FLT x90 = atan2(-1 * x63, x55); const FLT x91 = x62 * (1. / sqrt(x65)); const FLT x92 = x60 * x91; const FLT x93 = (-1 * asin(x92)) + (*bsc0).ogeephase + x90; const FLT x94 = (sin(x93) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x95 = 0.0076069798 + (x78 * x74); const FLT x96 = x74 * x95; const FLT x97 = -8.0108022e-06 + (-1.60216044e-05 * x74); const FLT x98 = x77 + (x74 * x97); const FLT x99 = x78 + (x74 * x98); const FLT x100 = x95 + (x74 * x99); const FLT x101 = (x74 * x100) + x96; const FLT x102 = sin(x61); const FLT x103 = x94 * x102; const FLT x104 = x71 + (-1 * x101 * x103); const FLT x105 = 1. / x104; const FLT x106 = x94 * x105; const FLT x107 = x89 * x106; const FLT x108 = 2 * x60; const FLT x109 = x96 * x106; const FLT x110 = x80 * x79 * x109 * x108; const FLT x111 = 1. / x65; const FLT x112 = 1. / sqrt(1 + (-1 * (x62 * x62) * x69 * x111)); const FLT x113 = x63 * x111; const FLT x114 = x113 + (x68 * x112); const FLT x115 = cos(x93) * (*bsc0).ogeemag; const FLT x116 = x89 * x95; const FLT x117 = x105 * x116; const FLT x118 = x115 * x117; const FLT x119 = x101 * x102; const FLT x120 = x119 * x115; const FLT x121 = x79 * x98; const FLT x122 = 2.40324066e-05 * x74; const FLT x123 = x79 * x100; const FLT x124 = x94 * (1. / (x104 * x104)) * x116; const FLT x125 = x92 + (x95 * x107); const FLT x126 = 1. / sqrt(1 + (-1 * (x125 * x125))); const FLT x127 = x126 * ((-1 * x124 * ((-1 * x103 * ((-1 * x82 * x123) + (x74 * (x88 + (-1 * x83 * x99) + (x74 * (x87 + (-1 * x82 * x121) + (x74 * ((-1 * x83 * x97) + x85 + (x83 * x122))))))) + (-1 * x83 * x95) + (x88 * x74))) + (-1 * x114 * x120))) + (-1 * x55 * x110) + (-1 * x68) + (x118 * x114) + (x88 * x107)); const FLT x128 = cos((-1 * asin(x125)) + x90 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x129 = x73 + (-1 * x80 * x69); const FLT x130 = x79 * x129; const FLT x131 = x76 * x130; const FLT x132 = (x77 * x130) + (x74 * ((-1 * x75 * x130) + x131)); const FLT x133 = (x74 * x132) + (x78 * x130); const FLT x134 = 2 * x109; const FLT x135 = x91 * x112; const FLT x136 = x126 * ((-1 * x118 * x135) + x91 + (x107 * x133) + (-1 * x124 * ((-1 * x103 * ((x95 * x130) + (x74 * x133) + (x74 * (x133 + (x99 * x130) + (x74 * (x132 + (x98 * x130) + (x74 * ((x97 * x130) + (-1 * x122 * x130) + x131)))))) + (x123 * x129))) + (x120 * x135))) + (x130 * x134)); const FLT x137 = x60 * x63; const FLT x138 = x66 * x137; const FLT x139 = x55 * x111; const FLT x140 = -1 * x139; const FLT x141 = x140 + (x112 * x138); const FLT x142 = x80 * x137; const FLT x143 = x79 * x142; const FLT x144 = -1 * x84 * x142; const FLT x145 = (-1 * x86 * x142) + (x74 * ((x75 * x143) + x144)); const FLT x146 = (x74 * x145) + (-1 * x78 * x143); const FLT x147 = x126 * ((x118 * x141) + (-1 * x63 * x110) + (x107 * x146) + (-1 * x124 * ((-1 * x103 * ((-1 * x95 * x143) + (x74 * (x146 + (-1 * x99 * x143) + (x74 * (x145 + (-1 * x121 * x142) + (x74 * ((-1 * x97 * x143) + x144 + (x122 * x143))))))) + (x74 * x146) + (-1 * x123 * x142))) + (-1 * x120 * x141))) + (-1 * x138)); const FLT x148 = 2 * x58; const FLT x149 = (2 * x57) + (-1 * x148); const FLT x150 = x63 * (1. / x64); const FLT x151 = 2 * x53; const FLT x152 = (2 * x52) + (-1 * x151); const FLT x153 = 1. / x55; const FLT x154 = x64 * x111; const FLT x155 = ((-1 * x152 * x153) + (x149 * x150)) * x154; const FLT x156 = 2 * x49; const FLT x157 = (2 * x44) + (-1 * x156); const FLT x158 = 2 * x55; const FLT x159 = 2 * x63; const FLT x160 = (x152 * x159) + (x149 * x158); const FLT x161 = 1.0/2.0 * x81; const FLT x162 = (x73 * x157) + (-1 * x161 * (x160 + (x108 * x157))); const FLT x163 = x79 * x162; const FLT x164 = 1.0/2.0 * x67; const FLT x165 = (x91 * x157) + (-1 * x160 * x164); const FLT x166 = x155 + (-1 * x112 * x165); const FLT x167 = x76 * x163; const FLT x168 = (x77 * x163) + (x74 * ((-1 * x75 * x163) + x167)); const FLT x169 = (x74 * x168) + (x78 * x163); const FLT x170 = x126 * (x165 + (x107 * x169) + (x118 * x166) + (x163 * x134) + (-1 * x124 * ((-1 * x103 * ((x95 * x163) + (x74 * x169) + (x74 * ((x99 * x163) + x169 + (x74 * (x168 + (x121 * x162) + (x74 * ((x97 * x163) + x167 + (-1 * x122 * x163))))))) + (x123 * x162))) + (-1 * x120 * x166)))); const FLT x171 = (-1 * x46) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x47) + (-1 * sensor_pt[2]) + (-1 * x45) + (-1 * (*error_model).Pose.Pos[2]); const FLT x172 = 2 * (*lh_p).Rot[3]; const FLT x173 = 2 * (*lh_p).Rot[2]; const FLT x174 = (x37 * x173) + (-1 * x171 * x172); const FLT x175 = 2 * (*lh_p).Rot[1]; const FLT x176 = 2 * x38; const FLT x177 = x157 + (x171 * x175) + x176; const FLT x178 = (x177 * x159) + (x174 * x158); const FLT x179 = 2 * x51; const FLT x180 = (-1 * x179) + x151 + (-4 * x52); const FLT x181 = (x91 * x180) + (-1 * x164 * x178); const FLT x182 = ((-1 * x177 * x153) + (x174 * x150)) * x154; const FLT x183 = x115 * (x182 + (-1 * x112 * x181)); const FLT x184 = x79 * ((x73 * x180) + (-1 * x161 * (x178 + (x108 * x180)))); const FLT x185 = x76 * x184; const FLT x186 = (x77 * x184) + (x74 * ((-1 * x75 * x184) + x185)); const FLT x187 = (x74 * x186) + (x78 * x184); const FLT x188 = x126 * (x181 + (x107 * x187) + (-1 * x124 * ((-1 * x103 * ((x95 * x184) + (x74 * x187) + (x100 * x184) + (x74 * (x187 + (x99 * x184) + (x74 * (x186 + (x98 * x184) + (x74 * ((x97 * x184) + (-1 * x122 * x184) + x185)))))))) + (-1 * x119 * x183))) + (x117 * x183) + (x184 * x134)); const FLT x189 = (-1 * sensor_pt[0]) + (-1 * (*_x0).Pose.Pos[0]) + (-1 * x42) + (-1 * x41) + (-1 * (*error_model).Pose.Pos[0]) + (-1 * x40); const FLT x190 = x152 + (x173 * x189) + x179; const FLT x191 = 2 * x56; const FLT x192 = x148 + (-1 * x191) + (-4 * x57); const FLT x193 = ((-1 * x192 * x153) + (x190 * x150)) * x154; const FLT x194 = (x192 * x159) + (x190 * x158); const FLT x195 = (x48 * x172) + (-1 * x175 * x189); const FLT x196 = (x91 * x195) + (-1 * x164 * x194); const FLT x197 = x193 + (-1 * x112 * x196); const FLT x198 = (x73 * x195) + (-1 * x161 * (x194 + (x108 * x195))); const FLT x199 = x79 * x198; const FLT x200 = x76 * x199; const FLT x201 = (x77 * x199) + (x74 * ((-1 * x75 * x199) + x200)); const FLT x202 = (x74 * x201) + (x78 * x199); const FLT x203 = x126 * ((x202 * x107) + (x118 * x197) + (-1 * x124 * ((-1 * x103 * ((x74 * x202) + (x74 * (x202 + (x99 * x199) + (x74 * (x201 + (x121 * x198) + (x74 * (x200 + (x97 * x199) + (-1 * x122 * x199))))))) + (x95 * x199) + (x123 * x198))) + (-1 * x120 * x197))) + x196 + (x199 * x134)); const FLT x204 = (-1 * x176) + x156 + (-4 * x44); const FLT x205 = (-1 * x34) + (-1 * x33) + (-1 * x36) + (-1 * (*_x0).Pose.Pos[1]) + (-1 * (*error_model).Pose.Pos[1]) + (-1 * sensor_pt[1]); const FLT x206 = (x43 * x175) + (-1 * x205 * x173); const FLT x207 = (x206 * x159) + (x204 * x158); const FLT x208 = x149 + x191 + (x205 * x172); const FLT x209 = (x91 * x208) + (-1 * x207 * x164); const FLT x210 = ((-1 * x206 * x153) + (x204 * x150)) * x154; const FLT x211 = x210 + (-1 * x209 * x112); const FLT x212 = (x73 * x208) + (-1 * x161 * (x207 + (x208 * x108))); const FLT x213 = x79 * x212; const FLT x214 = x76 * x213; const FLT x215 = (x77 * x213) + (x74 * ((-1 * x75 * x213) + x214)); const FLT x216 = (x74 * x215) + (x78 * x213); const FLT x217 = x126 * ((x216 * x107) + (-1 * x124 * ((-1 * x103 * ((x95 * x213) + (x212 * x123) + (x74 * (x216 + (x99 * x213) + (x74 * (x215 + (x212 * x121) + (x74 * ((x97 * x213) + x214 + (-1 * x213 * x122))))))) + (x74 * x216))) + (-1 * x211 * x120))) + (x211 * x118) + x209 + (x213 * x134)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), (-1 * ((-1 * x113) + x127) * x128) + (-1 * x127) + x113); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x128 * x136) + (-1 * x136)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), (-1 * (x139 + x147) * x128) + (-1 * x147) + x140); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x155 + (-1 * ((-1 * x155) + x170) * x128) + (-1 * x170)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (-1 * ((-1 * x182) + x188) * x128) + (-1 * x188) + x182); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x193 + (-1 * ((-1 * x193) + x203) * x128) + (-1 * x203)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (-1 * ((-1 * x210) + x217) * x128) + (-1 * x217) + x210); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen2_jac_lh_p(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x3 = 0.5 * (*_x0).Pose.Rot[1]; const FLT x4 = (x3 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*error_model).Pose.AxisAngleRot[0]) + (x2 * (*error_model).Pose.AxisAngleRot[2]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*_x0).Pose.Rot[3]; const FLT x16 = (x2 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x3 * (*error_model).Pose.AxisAngleRot[2]) + (x15 * (*error_model).Pose.AxisAngleRot[0]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x3 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[0] + (-1 * x15 * (*error_model).Pose.AxisAngleRot[2]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x15 * (*error_model).Pose.AxisAngleRot[1]) + (x1 * (*error_model).Pose.AxisAngleRot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x4 * x4) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x6 * x19) + (x8 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x7 * x26) + (x25 * x17) + (-1 * x0 * x23) + (-1 * x9 * x24); const FLT x28 = x22 * x17; const FLT x29 = (x7 * x24) + (x4 * x25) + (x0 * x28) + (-1 * x9 * x26); const FLT x30 = (x7 * x28) + (-1 * x0 * x24) + (x9 * x23) + (x25 * x18); const FLT x31 = (-1 * x30 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x29 * sensor_pt[0]); const FLT x32 = (-1 * x7 * x23) + (x0 * x26) + (x9 * x28) + (x25 * x16); const FLT x33 = (-1 * x32 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x30 * sensor_pt[1]); const FLT x34 = 1.0/2.0 * dt * fabs(dt); const FLT x35 = (x34 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x32 * x33) + (-1 * x31 * x29))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x36 = (-1 * x29 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x32 * sensor_pt[2]); const FLT x37 = (*_x0).Pose.Pos[2] + (x34 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x30 * x31) + (-1 * x32 * x36))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x38 = (*_x0).Pose.Pos[1] + (2 * ((x36 * x29) + (-1 * x30 * x33))) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (*error_model).Pose.Pos[1] + (x34 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x39 = (-1 * x38 * (*lh_p).Rot[3]) + (x35 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[2]); const FLT x40 = (-1 * x37 * (*lh_p).Rot[1]) + (x38 * (*lh_p).Rot[0]) + (x35 * (*lh_p).Rot[3]); const FLT x41 = (2 * ((x40 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + x37 + (*lh_p).Pos[2]; const FLT x42 = (-1 * x35 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[0]) + (x38 * (*lh_p).Rot[1]); const FLT x43 = (*lh_p).Pos[0] + x35 + (2 * ((x42 * (*lh_p).Rot[2]) + (-1 * x40 * (*lh_p).Rot[3]))); const FLT x44 = atan2(-1 * x41, x43); const FLT x45 = 0.523598775598299 + (*bsc0).tilt; const FLT x46 = tan(x45); const FLT x47 = (*lh_p).Pos[1] + x38 + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x42 * (*lh_p).Rot[1]))); const FLT x48 = (x43 * x43) + (x41 * x41); const FLT x49 = x47 * (1. / sqrt(x48)); const FLT x50 = x46 * x49; const FLT x51 = (-1 * asin(x50)) + (*bsc0).ogeephase + x44; const FLT x52 = sin(x51); const FLT x53 = (x52 * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x54 = cos(x45); const FLT x55 = x47 * x47; const FLT x56 = x48 + x55; const FLT x57 = (1. / sqrt(x56)) * x47; const FLT x58 = asin((1. / x54) * x57); const FLT x59 = 8.0108022e-06 * x58; const FLT x60 = -8.0108022e-06 + (-1 * x59); const FLT x61 = 0.0028679863 + (x60 * x58); const FLT x62 = 5.3685255e-06 + (x61 * x58); const FLT x63 = 0.0076069798 + (x62 * x58); const FLT x64 = x58 * x58; const FLT x65 = x63 * x58; const FLT x66 = -8.0108022e-06 + (-1.60216044e-05 * x58); const FLT x67 = x61 + (x66 * x58); const FLT x68 = x62 + (x67 * x58); const FLT x69 = x63 + (x68 * x58); const FLT x70 = (x69 * x58) + x65; const FLT x71 = sin(x45); const FLT x72 = x71 * x53; const FLT x73 = x70 * x72; const FLT x74 = x54 + (-1 * x73); const FLT x75 = 1. / x74; const FLT x76 = x75 * x64; const FLT x77 = x76 * x63; const FLT x78 = x50 + (x77 * x53); const FLT x79 = 1. / sqrt(1 + (-1 * (x78 * x78))); const FLT x80 = (1. / (x74 * x74)) * x63 * x64; const FLT x81 = x80 * x73; const FLT x82 = (x77 + x81) * x79; const FLT x83 = asin(x78) + (-1 * x44) + (-1 * (*bsc0).gibpha); const FLT x84 = cos(x83) * (*bsc0).gibmag; const FLT x85 = ((x77 * x52) + (x81 * x52)) * x79; const FLT x86 = cos(x51) * (*bsc0).ogeemag; const FLT x87 = x86 * x77; const FLT x88 = x79 * (x87 + (x81 * x86)); const FLT x89 = 1. / (x54 * x54); const FLT x90 = x89 * x57 * (1. / sqrt(1 + (-1 * x89 * (1. / x56) * x55))); const FLT x91 = x71 * x90; const FLT x92 = x60 * x91; const FLT x93 = (x61 * x91) + (x58 * ((-1 * x59 * x91) + x92)); const FLT x94 = (x58 * x93) + (x62 * x91); const FLT x95 = x46 * x46; const FLT x96 = x49 * (1 + x95); const FLT x97 = x96 * (1. / sqrt(1 + (-1 * x55 * x95 * (1. / x48)))); const FLT x98 = x79 * ((-1 * x87 * x97) + (-1 * x80 * x53 * ((-1 * x70 * x54 * x53) + (-1 * x72 * ((x58 * x94) + (x58 * (x94 + (x68 * x91) + (x58 * ((x67 * x91) + x93 + (x58 * ((x66 * x91) + x92 + (-2.40324066e-05 * x58 * x91))))))) + (x63 * x91) + (x69 * x91))) + (-1 * x71) + (x86 * x71 * x70 * x97))) + (2 * x72 * x75 * x65 * x90) + (x76 * x53 * x94) + x96); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), (-1 * x82 * x84) + (-1 * x82)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), -1 * sin(x83)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x84); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeemag)/sizeof(FLT), (-1 * x84 * x85) + (-1 * x85)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeephase)/sizeof(FLT), (-1 * x88 * x84) + (-1 * x88)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x84 * x98) + (-1 * x98)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_x_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_x_gen2_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_x_gen2_jac_bsc0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } static inline FLT SurviveKalmanErrorModel_LightMeas_y_gen2(const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x2 = 0.5 * (*_x0).Pose.Rot[1]; const FLT x3 = (x2 * (*error_model).Pose.AxisAngleRot[1]) + (*_x0).Pose.Rot[3] + (-1 * x0 * (*_x0).Pose.Rot[2]) + (x1 * (*_x0).Pose.Rot[0]); const FLT x4 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x5 = dt * dt; const FLT x6 = (x4 * x4) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x16 = (x15 * (*_x0).Pose.Rot[0]) + (x0 * (*_x0).Pose.Rot[3]) + (-1 * x2 * (*error_model).Pose.AxisAngleRot[2]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x0 * (*_x0).Pose.Rot[1]) + (-1 * x15 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x1 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x0 * (*_x0).Pose.Rot[0]) + (-1 * x15 * (*_x0).Pose.Rot[3]) + (x1 * (*_x0).Pose.Rot[2]); const FLT x19 = (x14 * x14) * (1. / x11); const FLT x20 = cos(x13); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x3 * x3) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x6 * x19) + (x8 * x19) + (x10 * x19))); const FLT x22 = dt * x21 * x14 * (1. / x12); const FLT x23 = x4 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x7 * x26) + (x25 * x17) + (-1 * x3 * x23) + (-1 * x9 * x24); const FLT x28 = (x7 * x24) + (x3 * x25) + (x23 * x17) + (-1 * x9 * x26); const FLT x29 = x3 * x22; const FLT x30 = x22 * x17; const FLT x31 = (x7 * x30) + (-1 * x23 * x16) + (x9 * x29) + (x25 * x18); const FLT x32 = (-1 * x31 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x28 * sensor_pt[0]); const FLT x33 = (-1 * x7 * x29) + (x23 * x18) + (x9 * x30) + (x25 * x16); const FLT x34 = (-1 * x33 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x31 * sensor_pt[1]); const FLT x35 = 1.0/2.0 * dt * fabs(dt); const FLT x36 = (x35 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (2 * ((x34 * x33) + (-1 * x32 * x28))) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x37 = (-1 * x28 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x33 * sensor_pt[2]); const FLT x38 = (*_x0).Pose.Pos[2] + (x35 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + sensor_pt[2] + (*error_model).Pose.Pos[2] + (2 * ((x32 * x31) + (-1 * x33 * x37))) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x39 = (*_x0).Pose.Pos[1] + (2 * ((x37 * x28) + (-1 * x31 * x34))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x35 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x40 = (x36 * (*lh_p).Rot[0]) + (-1 * x39 * (*lh_p).Rot[3]) + (x38 * (*lh_p).Rot[2]); const FLT x41 = (x39 * (*lh_p).Rot[0]) + (-1 * x38 * (*lh_p).Rot[1]) + (x36 * (*lh_p).Rot[3]); const FLT x42 = x38 + (2 * ((x41 * (*lh_p).Rot[1]) + (-1 * x40 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x43 = (-1 * x36 * (*lh_p).Rot[2]) + (x38 * (*lh_p).Rot[0]) + (x39 * (*lh_p).Rot[1]); const FLT x44 = x36 + (*lh_p).Pos[0] + (2 * ((x43 * (*lh_p).Rot[2]) + (-1 * x41 * (*lh_p).Rot[3]))); const FLT x45 = atan2(-1 * x42, x44); const FLT x46 = x39 + (*lh_p).Pos[1] + (2 * ((x40 * (*lh_p).Rot[3]) + (-1 * x43 * (*lh_p).Rot[1]))); const FLT x47 = (x44 * x44) + (x42 * x42); const FLT x48 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x49 = cos(x48); const FLT x50 = asin(x46 * (1. / x49) * (1. / sqrt(x47 + (x46 * x46)))); const FLT x51 = 0.0028679863 + (x50 * (-8.0108022e-06 + (-8.0108022e-06 * x50))); const FLT x52 = 5.3685255e-06 + (x50 * x51); const FLT x53 = 0.0076069798 + (x50 * x52); const FLT x54 = -1 * x46 * (1. / sqrt(x47)) * tan(x48); const FLT x55 = ((*bsc0).ogeemag * sin((-1 * asin(x54)) + (*bsc0).ogeephase + x45)) + (*bsc0).curve; const FLT x56 = (-1 * asin(x54 + ((x50 * x50) * x53 * x55 * (1. / (x49 + (x55 * sin(x48) * ((x50 * (x53 + (x50 * (x52 + (x50 * (x51 + (x50 * (-8.0108022e-06 + (-1.60216044e-05 * x50))))))))) + (x50 * x53)))))))) + x45; return -1.5707963267949 + x56 + (sin(x56 + (*bsc0).gibpha) * (*bsc0).gibmag) + (-1 * (*bsc0).phase); } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = 1.0/2.0 * x0; const FLT x2 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x3 = dt * dt; const FLT x4 = x2 * x2; const FLT x5 = x4 * x3; const FLT x6 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x7 = x6 * x6; const FLT x8 = x3 * x7; const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x9 * x9; const FLT x11 = x3 * x10; const FLT x12 = 1e-10 + x11 + x5 + x8; const FLT x13 = sqrt(x12); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = x15 * (1. / x13); const FLT x17 = dt * x16; const FLT x18 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x19 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x20 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x21 = (x20 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x18 * (*error_model).Pose.AxisAngleRot[0]) + (x19 * (*_x0).Pose.Rot[0]); const FLT x22 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x23 = (-1 * x19 * (*_x0).Pose.Rot[1]) + (x20 * (*_x0).Pose.Rot[0]) + (x22 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x24 = (-1 * x18 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x22 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[0] + (-1 * x19 * (*_x0).Pose.Rot[3]); const FLT x25 = (*_x0).Pose.Rot[1] + (x22 * (*_x0).Pose.Rot[0]) + (-1 * x20 * (*_x0).Pose.Rot[3]) + (x18 * (*error_model).Pose.AxisAngleRot[2]); const FLT x26 = (x25 * x25) + (x24 * x24) + (x21 * x21) + (x23 * x23); const FLT x27 = 1. / sqrt(x26); const FLT x28 = x15 * x15; const FLT x29 = 1. / x12; const FLT x30 = x28 * x29; const FLT x31 = cos(x14); const FLT x32 = (x31 * x31) + (x8 * x30) + (x5 * x30) + (x30 * x11); const FLT x33 = 1. / sqrt(x32); const FLT x34 = x33 * x21; const FLT x35 = x34 * x27; const FLT x36 = x35 * x17; const FLT x37 = x33 * x27; const FLT x38 = x37 * x17; const FLT x39 = x38 * x23; const FLT x40 = x31 * x27; const FLT x41 = x40 * x33; const FLT x42 = x41 * x24; const FLT x43 = x37 * x25; const FLT x44 = x43 * x17; const FLT x45 = x42 + (-1 * x2 * x36) + (-1 * x6 * x44) + (-1 * x9 * x39); const FLT x46 = x38 * x24; const FLT x47 = x41 * x23; const FLT x48 = (-1 * x6 * x36) + (x2 * x44) + (x9 * x46) + x47; const FLT x49 = x41 * x21; const FLT x50 = (x6 * x39) + (x2 * x46) + x49 + (-1 * x9 * x44); const FLT x51 = (-1 * x50 * sensor_pt[1]) + (x45 * sensor_pt[0]) + (x48 * sensor_pt[2]); const FLT x52 = x41 * x25; const FLT x53 = (x6 * x46) + (-1 * x2 * x39) + (x9 * x36) + x52; const FLT x54 = (x45 * sensor_pt[1]) + (-1 * x53 * sensor_pt[2]) + (x50 * sensor_pt[0]); const FLT x55 = (*_x0).Pose.Pos[2] + (x1 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x54 * x53) + (-1 * x51 * x48))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x56 = (-1 * x48 * sensor_pt[0]) + (x45 * sensor_pt[2]) + (x53 * sensor_pt[1]); const FLT x57 = (*_x0).Pose.Pos[1] + (2 * ((x50 * x51) + (-1 * x53 * x56))) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (x1 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x58 = (2 * ((x56 * x48) + (-1 * x50 * x54))) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + (x1 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x59 = (-1 * x58 * (*lh_p).Rot[2]) + (x55 * (*lh_p).Rot[0]) + (x57 * (*lh_p).Rot[1]); const FLT x60 = (-1 * x57 * (*lh_p).Rot[3]) + (x58 * (*lh_p).Rot[0]) + (x55 * (*lh_p).Rot[2]); const FLT x61 = x57 + (*lh_p).Pos[1] + (2 * ((x60 * (*lh_p).Rot[3]) + (-1 * x59 * (*lh_p).Rot[1]))); const FLT x62 = x61 * x61; const FLT x63 = (-1 * x55 * (*lh_p).Rot[1]) + (x57 * (*lh_p).Rot[0]) + (x58 * (*lh_p).Rot[3]); const FLT x64 = (2 * ((x63 * (*lh_p).Rot[1]) + (-1 * x60 * (*lh_p).Rot[2]))) + x55 + (*lh_p).Pos[2]; const FLT x65 = (*lh_p).Pos[0] + x58 + (2 * ((x59 * (*lh_p).Rot[2]) + (-1 * x63 * (*lh_p).Rot[3]))); const FLT x66 = x65 * x65; const FLT x67 = x66 + (x64 * x64); const FLT x68 = x67 + x62; const FLT x69 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x70 = cos(x69); const FLT x71 = 1. / x70; const FLT x72 = x71 * (1. / sqrt(x68)); const FLT x73 = asin(x72 * x61); const FLT x74 = 8.0108022e-06 * x73; const FLT x75 = -8.0108022e-06 + (-1 * x74); const FLT x76 = 0.0028679863 + (x73 * x75); const FLT x77 = 5.3685255e-06 + (x73 * x76); const FLT x78 = 0.0076069798 + (x73 * x77); const FLT x79 = x73 * x78; const FLT x80 = -8.0108022e-06 + (-1.60216044e-05 * x73); const FLT x81 = x76 + (x80 * x73); const FLT x82 = x77 + (x81 * x73); const FLT x83 = x78 + (x82 * x73); const FLT x84 = (x83 * x73) + x79; const FLT x85 = atan2(-1 * x64, x65); const FLT x86 = tan(x69); const FLT x87 = x86 * (1. / sqrt(x67)); const FLT x88 = -1 * x87 * x61; const FLT x89 = (-1 * asin(x88)) + (*bsc0).ogeephase + x85; const FLT x90 = (sin(x89) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x91 = sin(x69); const FLT x92 = x91 * x90; const FLT x93 = x70 + (x84 * x92); const FLT x94 = 1. / x93; const FLT x95 = x73 * x73; const FLT x96 = x90 * x95; const FLT x97 = x96 * x94; const FLT x98 = x88 + (x78 * x97); const FLT x99 = 1. / sqrt(1 + (-1 * (x98 * x98))); const FLT x100 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x101 = -1 * x0 * x100; const FLT x102 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x103 = x1 + (-1 * x0 * x102); const FLT x104 = x103 + x101; const FLT x105 = 2 * x65; const FLT x106 = x0 * (*lh_p).Rot[0]; const FLT x107 = x106 * (*lh_p).Rot[2]; const FLT x108 = x0 * (*lh_p).Rot[3]; const FLT x109 = x108 * (*lh_p).Rot[1]; const FLT x110 = x109 + (-1 * x107); const FLT x111 = 2 * x64; const FLT x112 = (x110 * x111) + (x105 * x104); const FLT x113 = 1.0/2.0 * x61; const FLT x114 = x86 * (1. / (x67 * sqrt(x67))) * x113; const FLT x115 = x0 * (*lh_p).Rot[1] * (*lh_p).Rot[2]; const FLT x116 = x108 * (*lh_p).Rot[0]; const FLT x117 = x116 + x115; const FLT x118 = (-1 * x87 * x117) + (x112 * x114); const FLT x119 = 1. / x67; const FLT x120 = 1. / sqrt(1 + (-1 * (x86 * x86) * x62 * x119)); const FLT x121 = x64 * (1. / x66); const FLT x122 = 1. / x65; const FLT x123 = x66 * x119; const FLT x124 = ((-1 * x110 * x122) + (x104 * x121)) * x123; const FLT x125 = cos(x89) * (*bsc0).ogeemag; const FLT x126 = x125 * (x124 + (-1 * x118 * x120)); const FLT x127 = x84 * x91; const FLT x128 = 1. / sqrt(1 + (-1 * (1. / (x70 * x70)) * x62 * (1. / x68))); const FLT x129 = 2 * x61; const FLT x130 = x71 * (1. / (x68 * sqrt(x68))) * x113; const FLT x131 = (x72 * x117) + (-1 * x130 * (x112 + (x117 * x129))); const FLT x132 = x128 * x131; const FLT x133 = x81 * x128; const FLT x134 = 2.40324066e-05 * x73; const FLT x135 = x75 * x132; const FLT x136 = (x76 * x132) + (x73 * ((-1 * x74 * x132) + x135)); const FLT x137 = x82 * x128; const FLT x138 = (x73 * x136) + (x77 * x132); const FLT x139 = x78 * x128; const FLT x140 = x78 * (1. / (x93 * x93)) * x96; const FLT x141 = 2 * x79 * x90 * x94; const FLT x142 = x78 * x95 * x94; const FLT x143 = x124 + (-1 * x99 * (x118 + (x97 * x138) + (x126 * x142) + (-1 * x140 * ((x92 * ((x73 * x138) + (x131 * x139) + (x83 * x132) + (x73 * (x138 + (x73 * (x136 + (x133 * x131) + (x73 * ((x80 * x132) + (-1 * x134 * x132) + x135)))) + (x131 * x137))))) + (x127 * x126))) + (x132 * x141))); const FLT x144 = cos((-1 * asin(x98)) + x85 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x145 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x146 = -1 * x0 * x145; const FLT x147 = x103 + x146; const FLT x148 = x115 + (-1 * x116); const FLT x149 = x108 * (*lh_p).Rot[2]; const FLT x150 = x106 * (*lh_p).Rot[1]; const FLT x151 = x150 + x149; const FLT x152 = (x111 * x151) + (x105 * x148); const FLT x153 = (x72 * x147) + (-1 * x130 * (x152 + (x129 * x147))); const FLT x154 = x77 * x128; const FLT x155 = x75 * x128; const FLT x156 = x153 * x155; const FLT x157 = x74 * x128; const FLT x158 = x76 * x128; const FLT x159 = (x153 * x158) + (x73 * ((-1 * x153 * x157) + x156)); const FLT x160 = (x73 * x159) + (x153 * x154); const FLT x161 = (-1 * x87 * x147) + (x114 * x152); const FLT x162 = ((-1 * x122 * x151) + (x121 * x148)) * x123; const FLT x163 = x162 + (-1 * x120 * x161); const FLT x164 = x127 * x125; const FLT x165 = x83 * x128; const FLT x166 = x128 * x134; const FLT x167 = x80 * x128; const FLT x168 = x128 * x141; const FLT x169 = x125 * x142; const FLT x170 = x162 + (-1 * x99 * (x161 + (x169 * x163) + (x168 * x153) + (x97 * x160) + (-1 * x140 * ((x92 * ((x73 * x160) + (x139 * x153) + (x165 * x153) + (x73 * (x160 + (x73 * (x159 + (x133 * x153) + (x73 * ((x167 * x153) + x156 + (-1 * x166 * x153))))) + (x137 * x153))))) + (x164 * x163))))); const FLT x171 = x149 + (-1 * x150); const FLT x172 = x107 + x109; const FLT x173 = x101 + x1 + x146; const FLT x174 = (x111 * x173) + (x105 * x172); const FLT x175 = (x72 * x171) + (-1 * x130 * (x174 + (x129 * x171))); const FLT x176 = (-1 * x87 * x171) + (x114 * x174); const FLT x177 = ((-1 * x122 * x173) + (x121 * x172)) * x123; const FLT x178 = x177 + (-1 * x120 * x176); const FLT x179 = x175 * x155; const FLT x180 = (x175 * x158) + (x73 * ((-1 * x175 * x157) + x179)); const FLT x181 = (x73 * x180) + (x175 * x154); const FLT x182 = x177 + (-1 * x99 * (x176 + (x169 * x178) + (x168 * x175) + (x97 * x181) + (-1 * x140 * ((x92 * ((x73 * x181) + (x175 * x139) + (x165 * x175) + (x73 * (x181 + (x73 * (x180 + (x175 * x133) + (x73 * (x179 + (x167 * x175) + (-1 * x166 * x175))))) + (x175 * x137))))) + (x164 * x178))))); const FLT x183 = 2 * (*lh_p).Rot[1]; const FLT x184 = x183 * (*lh_p).Rot[2]; const FLT x185 = 2 * (*lh_p).Rot[0]; const FLT x186 = x185 * (*lh_p).Rot[3]; const FLT x187 = x186 + x184; const FLT x188 = 2 * x102; const FLT x189 = -1 * x188; const FLT x190 = 2 * x100; const FLT x191 = -1 * x190; const FLT x192 = 1 + x191 + x189; const FLT x193 = x185 * (*lh_p).Rot[2]; const FLT x194 = 2 * (*lh_p).Rot[3]; const FLT x195 = x194 * (*lh_p).Rot[1]; const FLT x196 = x195 + (-1 * x193); const FLT x197 = (x111 * x196) + (x105 * x192); const FLT x198 = (x72 * x187) + (-1 * x130 * (x197 + (x129 * x187))); const FLT x199 = x198 * x155; const FLT x200 = (x198 * x158) + (x73 * ((-1 * x198 * x157) + x199)); const FLT x201 = (x73 * x200) + (x198 * x154); const FLT x202 = (-1 * x87 * x187) + (x114 * x197); const FLT x203 = ((-1 * x122 * x196) + (x121 * x192)) * x123; const FLT x204 = x203 + (-1 * x202 * x120); const FLT x205 = x203 + (-1 * x99 * (x202 + (x204 * x169) + (x168 * x198) + (-1 * x140 * ((x92 * ((x198 * x139) + (x165 * x198) + (x73 * x201) + (x73 * (x201 + (x73 * (x200 + (x198 * x133) + (x73 * ((x167 * x198) + x199 + (-1 * x166 * x198))))) + (x198 * x137))))) + (x204 * x164))) + (x97 * x201))); const FLT x206 = 2 * x145; const FLT x207 = 1 + (-1 * x206); const FLT x208 = x207 + x189; const FLT x209 = x184 + (-1 * x186); const FLT x210 = x194 * (*lh_p).Rot[2]; const FLT x211 = x185 * (*lh_p).Rot[1]; const FLT x212 = x211 + x210; const FLT x213 = (x212 * x111) + (x209 * x105); const FLT x214 = (x72 * x208) + (-1 * x130 * (x213 + (x208 * x129))); const FLT x215 = x214 * x155; const FLT x216 = (x214 * x158) + (x73 * ((-1 * x214 * x157) + x215)); const FLT x217 = (x73 * x216) + (x214 * x154); const FLT x218 = (-1 * x87 * x208) + (x213 * x114); const FLT x219 = ((-1 * x212 * x122) + (x209 * x121)) * x123; const FLT x220 = x219 + (-1 * x218 * x120); const FLT x221 = x219 + (-1 * x99 * ((x220 * x169) + (x214 * x168) + (x97 * x217) + x218 + (-1 * x140 * ((x92 * ((x73 * (x217 + (x73 * (x216 + (x214 * x133) + (x73 * ((x214 * x167) + x215 + (-1 * x214 * x166))))) + (x214 * x137))) + (x73 * x217) + (x214 * x165) + (x214 * x139))) + (x220 * x164))))); const FLT x222 = x210 + (-1 * x211); const FLT x223 = x193 + x195; const FLT x224 = x207 + x191; const FLT x225 = (x224 * x111) + (x223 * x105); const FLT x226 = (x72 * x222) + (-1 * x130 * (x225 + (x222 * x129))); const FLT x227 = x226 * x155; const FLT x228 = (x226 * x158) + (x73 * ((-1 * x226 * x157) + x227)); const FLT x229 = (x73 * x228) + (x226 * x154); const FLT x230 = (-1 * x87 * x222) + (x225 * x114); const FLT x231 = ((-1 * x224 * x122) + (x223 * x121)) * x123; const FLT x232 = x231 + (-1 * x230 * x120); const FLT x233 = x231 + (-1 * x99 * (x230 + (x97 * x229) + (x232 * x169) + (x226 * x168) + (-1 * x140 * ((x92 * ((x73 * x229) + (x226 * x139) + (x73 * (x229 + (x73 * (x228 + (x226 * x133) + (x73 * ((x226 * x167) + x227 + (-1 * x226 * x166))))) + (x226 * x137))) + (x226 * x165))) + (x232 * x164))))); const FLT x234 = x41 * x22; const FLT x235 = x6 * x38; const FLT x236 = 1.0/2.0 * x17; const FLT x237 = x2 * x236; const FLT x238 = x23 * x237; const FLT x239 = 1.0 * (*error_model).Pose.AxisAngleRot[2]; const FLT x240 = 1.0 * (*error_model).Pose.AxisAngleRot[0]; const FLT x241 = 1.0 * (*error_model).Pose.AxisAngleRot[1]; const FLT x242 = (x23 * x241) + (2 * x24) + (x25 * x240) + (x21 * x239); const FLT x243 = 1. / (x26 * sqrt(x26)); const FLT x244 = x33 * x243; const FLT x245 = x242 * x244; const FLT x246 = x9 * x236; const FLT x247 = x34 * x243; const FLT x248 = x242 * x247; const FLT x249 = x24 * x236; const FLT x250 = x6 * x249; const FLT x251 = 1.0/2.0 * x25; const FLT x252 = x31 * x243; const FLT x253 = x252 * x251; const FLT x254 = x33 * x253; const FLT x255 = x2 * x38; const FLT x256 = -1 * x20 * x255; const FLT x257 = x9 * x38; const FLT x258 = x19 * x257; const FLT x259 = x258 + x256; const FLT x260 = (-1 * x254 * x242) + x235 + x259 + x234 + (-1 * x250 * x245) + (x238 * x245) + (-1 * x246 * x248); const FLT x261 = x22 * x235; const FLT x262 = -1 * x261; const FLT x263 = x19 * x255; const FLT x264 = -1 * x263; const FLT x265 = x17 * x251; const FLT x266 = x265 * x245; const FLT x267 = 1.0/2.0 * x252; const FLT x268 = x33 * x267; const FLT x269 = x24 * x268; const FLT x270 = x23 * x246; const FLT x271 = x20 * x257; const FLT x272 = (-1 * x271) + x41; const FLT x273 = x272 + (x270 * x245) + x264 + (x6 * x266) + x262 + (-1 * x269 * x242) + (x237 * x248); const FLT x274 = x34 * x267; const FLT x275 = x24 * x237; const FLT x276 = x6 * x236; const FLT x277 = x23 * x276; const FLT x278 = x41 * x19; const FLT x279 = x20 * x235; const FLT x280 = -1 * x22 * x257; const FLT x281 = x280 + x279; const FLT x282 = x281 + (-1 * x275 * x245) + (-1 * x274 * x242) + (x9 * x266) + (-1 * x277 * x245) + x255 + x278; const FLT x283 = (x282 * sensor_pt[0]) + (-1 * x260 * sensor_pt[2]) + (x273 * sensor_pt[1]); const FLT x284 = 2 * x50; const FLT x285 = 2 * x54; const FLT x286 = x2 * x265; const FLT x287 = x22 * x255; const FLT x288 = x9 * x249; const FLT x289 = x23 * x268; const FLT x290 = -1 * x19 * x235; const FLT x291 = x41 * x20; const FLT x292 = x291 + x290; const FLT x293 = x292 + (-1 * x289 * x242) + (-1 * x286 * x245) + (x276 * x248) + x287 + (-1 * x288 * x245) + x257; const FLT x294 = (x260 * sensor_pt[1]) + (-1 * x293 * sensor_pt[0]) + (x273 * sensor_pt[2]); const FLT x295 = 2 * x48; const FLT x296 = 2 * x56; const FLT x297 = (x296 * x293) + (-1 * x283 * x284) + (x295 * x294) + (-1 * x282 * x285); const FLT x298 = (x293 * sensor_pt[2]) + (x273 * sensor_pt[0]) + (-1 * x282 * sensor_pt[1]); const FLT x299 = 2 * x51; const FLT x300 = 2 * x53; const FLT x301 = (x260 * x285) + (x283 * x300) + (-1 * x298 * x295) + (-1 * x299 * x293); const FLT x302 = (x299 * x282) + (x298 * x284) + (-1 * x296 * x260) + (-1 * x294 * x300); const FLT x303 = 2 * ((x302 * (*lh_p).Rot[1]) + (-1 * x297 * (*lh_p).Rot[2]) + (x301 * (*lh_p).Rot[0])); const FLT x304 = 2 * ((x301 * (*lh_p).Rot[2]) + (-1 * x302 * (*lh_p).Rot[3]) + (x297 * (*lh_p).Rot[0])); const FLT x305 = x302 + (-1 * x303 * (*lh_p).Rot[1]) + (x304 * (*lh_p).Rot[3]); const FLT x306 = 2 * ((x297 * (*lh_p).Rot[3]) + (x302 * (*lh_p).Rot[0]) + (-1 * x301 * (*lh_p).Rot[1])); const FLT x307 = x297 + (-1 * x306 * (*lh_p).Rot[3]) + (x303 * (*lh_p).Rot[2]); const FLT x308 = x301 + (-1 * x304 * (*lh_p).Rot[2]) + (x306 * (*lh_p).Rot[1]); const FLT x309 = (x308 * x111) + (x307 * x105); const FLT x310 = (x72 * x305) + (-1 * x130 * (x309 + (x305 * x129))); const FLT x311 = (-1 * x87 * x305) + (x309 * x114); const FLT x312 = ((-1 * x308 * x122) + (x307 * x121)) * x123; const FLT x313 = x312 + (-1 * x311 * x120); const FLT x314 = x310 * x155; const FLT x315 = (x310 * x158) + (x73 * ((-1 * x310 * x157) + x314)); const FLT x316 = (x73 * x315) + (x310 * x154); const FLT x317 = x312 + (-1 * x99 * (x311 + (x313 * x169) + (x310 * x168) + (x97 * x316) + (-1 * x140 * ((x92 * ((x73 * x316) + (x310 * x139) + (x310 * x165) + (x73 * (x316 + (x73 * (x315 + (x310 * x133) + (x73 * ((x310 * x167) + x314 + (-1 * x310 * x166))))) + (x310 * x137))))) + (x313 * x164))))); const FLT x318 = (-1 * x23 * x239) + (x21 * x241) + (2 * x25) + (-1 * x24 * x240); const FLT x319 = x244 * x318; const FLT x320 = x247 * x318; const FLT x321 = x41 + x271; const FLT x322 = x321 + (-1 * x254 * x318) + x262 + (-1 * x250 * x319) + (x238 * x319) + (-1 * x246 * x320) + x263; const FLT x323 = -1 * x287; const FLT x324 = x265 * x319; const FLT x325 = -1 * x257; const FLT x326 = (-1 * x274 * x318) + x325 + (-1 * x277 * x319) + (-1 * x275 * x319) + (x9 * x324) + x292 + x323; const FLT x327 = -1 * x235; const FLT x328 = -1 * x234; const FLT x329 = (x270 * x319) + x328 + x327 + x259 + (x6 * x324) + (-1 * x269 * x318) + (x237 * x320); const FLT x330 = -1 * x278; const FLT x331 = (-1 * x279) + x280; const FLT x332 = x331 + x330 + x255 + (-1 * x2 * x324) + (-1 * x289 * x318) + (x276 * x320) + (-1 * x288 * x319); const FLT x333 = (x332 * sensor_pt[2]) + (-1 * x326 * sensor_pt[1]) + (x329 * sensor_pt[0]); const FLT x334 = (x322 * sensor_pt[1]) + (x329 * sensor_pt[2]) + (-1 * x332 * sensor_pt[0]); const FLT x335 = (x299 * x326) + (-1 * x300 * x334) + (-1 * x296 * x322) + (x284 * x333); const FLT x336 = (x326 * sensor_pt[0]) + (-1 * x322 * sensor_pt[2]) + (x329 * sensor_pt[1]); const FLT x337 = (x300 * x336) + (-1 * x299 * x332) + (-1 * x295 * x333) + (x285 * x322); const FLT x338 = (x295 * x334) + (-1 * x284 * x336) + (x296 * x332) + (-1 * x285 * x326); const FLT x339 = (x335 * (*lh_p).Rot[0]) + (x338 * (*lh_p).Rot[3]) + (-1 * x337 * (*lh_p).Rot[1]); const FLT x340 = (x335 * (*lh_p).Rot[1]) + (-1 * x338 * (*lh_p).Rot[2]) + (x337 * (*lh_p).Rot[0]); const FLT x341 = 2 * (*lh_p).Rot[2]; const FLT x342 = x338 + (-1 * x339 * x194) + (x340 * x341); const FLT x343 = (x338 * (*lh_p).Rot[0]) + (x337 * (*lh_p).Rot[2]) + (-1 * x335 * (*lh_p).Rot[3]); const FLT x344 = x337 + (x339 * x183) + (-1 * x343 * x341); const FLT x345 = (x344 * x111) + (x342 * x105); const FLT x346 = (x343 * x194) + x335 + (-1 * x340 * x183); const FLT x347 = (-1 * x87 * x346) + (x345 * x114); const FLT x348 = ((-1 * x344 * x122) + (x342 * x121)) * x123; const FLT x349 = x348 + (-1 * x347 * x120); const FLT x350 = (x72 * x346) + (-1 * x130 * (x345 + (x346 * x129))); const FLT x351 = x350 * x155; const FLT x352 = (x350 * x158) + (x73 * ((-1 * x350 * x157) + x351)); const FLT x353 = (x73 * x352) + (x350 * x154); const FLT x354 = x348 + (-1 * x99 * ((x349 * x169) + (x350 * x168) + (-1 * x140 * ((x92 * ((x350 * x165) + (x73 * x353) + (x350 * x139) + (x73 * (x353 + (x73 * ((x350 * x133) + x352 + (x73 * ((x350 * x167) + x351 + (-1 * x350 * x166))))) + (x350 * x137))))) + (x349 * x164))) + x347 + (x97 * x353))); const FLT x355 = -1 * x255; const FLT x356 = (2 * x23) + (-1 * x21 * x240) + (x25 * x239) + (-1 * x24 * x241); const FLT x357 = x244 * x356; const FLT x358 = x247 * x356; const FLT x359 = x355 + x331 + (-1 * x246 * x358) + (-1 * x254 * x356) + x278 + (-1 * x250 * x357) + (x238 * x357); const FLT x360 = x265 * x357; const FLT x361 = (-1 * x291) + x290; const FLT x362 = x361 + x325 + (x270 * x357) + x287 + (-1 * x269 * x356) + (x237 * x358) + (x6 * x360); const FLT x363 = x256 + (-1 * x258); const FLT x364 = x363 + x328 + (-1 * x275 * x357) + x235 + (-1 * x277 * x357) + (x9 * x360) + (-1 * x274 * x356); const FLT x365 = (x364 * sensor_pt[0]) + (-1 * x359 * sensor_pt[2]) + (x362 * sensor_pt[1]); const FLT x366 = x272 + (x276 * x358) + x261 + x263 + (-1 * x2 * x360) + (-1 * x288 * x357) + (-1 * x289 * x356); const FLT x367 = (x359 * sensor_pt[1]) + (x362 * sensor_pt[2]) + (-1 * x366 * sensor_pt[0]); const FLT x368 = (-1 * x284 * x365) + (x296 * x366) + (x295 * x367) + (-1 * x285 * x364); const FLT x369 = (-1 * x364 * sensor_pt[1]) + (x366 * sensor_pt[2]) + (x362 * sensor_pt[0]); const FLT x370 = (x365 * x300) + (x285 * x359) + (-1 * x299 * x366) + (-1 * x295 * x369); const FLT x371 = (x284 * x369) + (x299 * x364) + (-1 * x296 * x359) + (-1 * x367 * x300); const FLT x372 = 2 * ((x371 * (*lh_p).Rot[1]) + (-1 * x368 * (*lh_p).Rot[2]) + (x370 * (*lh_p).Rot[0])); const FLT x373 = 2 * ((x370 * (*lh_p).Rot[2]) + (-1 * x371 * (*lh_p).Rot[3]) + (x368 * (*lh_p).Rot[0])); const FLT x374 = x371 + (-1 * x372 * (*lh_p).Rot[1]) + (x373 * (*lh_p).Rot[3]); const FLT x375 = 2 * ((x368 * (*lh_p).Rot[3]) + (x371 * (*lh_p).Rot[0]) + (-1 * x370 * (*lh_p).Rot[1])); const FLT x376 = x368 + (-1 * x375 * (*lh_p).Rot[3]) + (x372 * (*lh_p).Rot[2]); const FLT x377 = x370 + (x375 * (*lh_p).Rot[1]) + (-1 * x373 * (*lh_p).Rot[2]); const FLT x378 = (x377 * x111) + (x376 * x105); const FLT x379 = (x72 * x374) + (-1 * x130 * (x378 + (x374 * x129))); const FLT x380 = (-1 * x87 * x374) + (x378 * x114); const FLT x381 = ((-1 * x377 * x122) + (x376 * x121)) * x123; const FLT x382 = x381 + (-1 * x380 * x120); const FLT x383 = x379 * x155; const FLT x384 = (x379 * x158) + (x73 * ((-1 * x379 * x157) + x383)); const FLT x385 = (x73 * x384) + (x379 * x154); const FLT x386 = x381 + (-1 * x99 * ((x97 * x385) + x380 + (x379 * x168) + (x382 * x169) + (-1 * x140 * ((x92 * ((x379 * x139) + (x379 * x165) + (x73 * x385) + (x73 * (x385 + (x73 * ((x379 * x133) + x384 + (x73 * ((x379 * x167) + (-1 * x379 * x166) + x383)))) + (x379 * x137))))) + (x382 * x164))))); const FLT x387 = (x23 * x240) + (2 * x21) + (-1 * x25 * x241) + (-1 * x24 * x239); const FLT x388 = x247 * x387; const FLT x389 = x33 * x387; const FLT x390 = x243 * x389; const FLT x391 = x327 + x363 + x234 + (-1 * x289 * x387) + (x276 * x388) + (-1 * x286 * x390) + (-1 * x288 * x390); const FLT x392 = x6 * x390; const FLT x393 = x281 + (x270 * x390) + (x237 * x388) + (-1 * x269 * x387) + (x265 * x392) + x355 + x330; const FLT x394 = (-1 * x253 * x389) + x323 + x361 + x257 + (x238 * x390) + (-1 * x249 * x392) + (-1 * x246 * x388); const FLT x395 = (x394 * sensor_pt[1]) + (-1 * x391 * sensor_pt[0]) + (x393 * sensor_pt[2]); const FLT x396 = x9 * x265; const FLT x397 = x321 + x261 + (-1 * x23 * x236 * x392) + (-1 * x275 * x390) + (-1 * x274 * x387) + x264 + (x396 * x390); const FLT x398 = (x391 * sensor_pt[2]) + (-1 * x397 * sensor_pt[1]) + (x393 * sensor_pt[0]); const FLT x399 = (x299 * x397) + (x284 * x398) + (-1 * x395 * x300) + (-1 * x296 * x394); const FLT x400 = (x397 * sensor_pt[0]) + (x393 * sensor_pt[1]) + (-1 * x394 * sensor_pt[2]); const FLT x401 = (x296 * x391) + (x295 * x395) + (-1 * x400 * x284) + (-1 * x285 * x397); const FLT x402 = (x285 * x394) + (x400 * x300) + (-1 * x295 * x398) + (-1 * x299 * x391); const FLT x403 = (x402 * (*lh_p).Rot[2]) + (-1 * x399 * (*lh_p).Rot[3]) + (x401 * (*lh_p).Rot[0]); const FLT x404 = (x399 * (*lh_p).Rot[1]) + (-1 * x401 * (*lh_p).Rot[2]) + (x402 * (*lh_p).Rot[0]); const FLT x405 = x399 + (x403 * x194) + (-1 * x404 * x183); const FLT x406 = (x401 * (*lh_p).Rot[3]) + (-1 * x402 * (*lh_p).Rot[1]) + (x399 * (*lh_p).Rot[0]); const FLT x407 = x401 + (-1 * x406 * x194) + (x404 * x341); const FLT x408 = x402 + (-1 * x403 * x341) + (x406 * x183); const FLT x409 = (x408 * x111) + (x407 * x105); const FLT x410 = (x72 * x405) + (-1 * x130 * (x409 + (x405 * x129))); const FLT x411 = (-1 * x87 * x405) + (x409 * x114); const FLT x412 = ((-1 * x408 * x122) + (x407 * x121)) * x123; const FLT x413 = x412 + (-1 * x411 * x120); const FLT x414 = x410 * x155; const FLT x415 = (x410 * x158) + (x73 * ((-1 * x410 * x157) + x414)); const FLT x416 = (x73 * x415) + (x410 * x154); const FLT x417 = x412 + (-1 * x99 * (x411 + (x413 * x169) + (x97 * x416) + (x410 * x168) + (-1 * x140 * ((x92 * ((x410 * x139) + (x73 * x416) + (x410 * x165) + (x73 * (x416 + (x73 * (x415 + (x410 * x133) + (x73 * ((x410 * x167) + x414 + (-1 * x410 * x166))))) + (x410 * x137))))) + (x413 * x164))))); const FLT x418 = x6 * x6 * x6; const FLT x419 = dt * dt * dt * dt; const FLT x420 = x15 * (1. / (x12 * sqrt(x12))); const FLT x421 = 1.0 * x31; const FLT x422 = x421 * x420; const FLT x423 = x419 * x422; const FLT x424 = 2 * x30; const FLT x425 = x3 * x424; const FLT x426 = 2 * x28 * (1. / (x12 * x12)); const FLT x427 = x419 * x426; const FLT x428 = x6 * x427; const FLT x429 = x6 * x423; const FLT x430 = x16 * x421; const FLT x431 = x3 * x430; const FLT x432 = (-1 * x6 * x431) + (x10 * x429) + (-1 * x10 * x428) + (x6 * x425) + (-1 * x418 * x427) + (x418 * x423) + (-1 * x4 * x428) + (x4 * x429); const FLT x433 = 1. / (x32 * sqrt(x32)); const FLT x434 = x40 * x433; const FLT x435 = x434 * x251; const FLT x436 = 0.5 * x16; const FLT x437 = x43 * x436; const FLT x438 = x3 * x437; const FLT x439 = x6 * x9; const FLT x440 = dt * dt * dt; const FLT x441 = 0.5 * x29 * x440; const FLT x442 = x49 * x441; const FLT x443 = x439 * x442; const FLT x444 = x27 * x433; const FLT x445 = x432 * x444; const FLT x446 = x21 * x445; const FLT x447 = x2 * x6; const FLT x448 = x420 * x440; const FLT x449 = x37 * x23; const FLT x450 = x449 * x448; const FLT x451 = x450 * x447; const FLT x452 = x42 * x441; const FLT x453 = x35 * x448; const FLT x454 = x453 * x439; const FLT x455 = x7 * x448; const FLT x456 = x37 * x24; const FLT x457 = x47 * x441; const FLT x458 = x457 * x447; const FLT x459 = (-1 * x458) + (-1 * x446 * x246) + (-1 * x6 * x438) + x451 + (-1 * x435 * x432) + x46 + (-1 * x445 * x250) + (x7 * x452) + (-1 * x454) + x443 + (x445 * x238) + (-1 * x455 * x456); const FLT x460 = -1 * x44; const FLT x461 = x7 * x441; const FLT x462 = x445 * x265; const FLT x463 = x450 * x439; const FLT x464 = x457 * x439; const FLT x465 = 1.0/2.0 * x434; const FLT x466 = x465 * x432; const FLT x467 = x3 * x436; const FLT x468 = x6 * x467; const FLT x469 = (x453 * x447) + (-1 * x442 * x447); const FLT x470 = x469 + (x43 * x455) + (-1 * x464) + (-1 * x456 * x468) + (x446 * x237) + (-1 * x52 * x461) + x463 + x460 + (x445 * x270) + (-1 * x24 * x466) + (x6 * x462); const FLT x471 = x43 * x448; const FLT x472 = x52 * x441; const FLT x473 = (-1 * x472 * x439) + (x471 * x439); const FLT x474 = x456 * x448; const FLT x475 = x2 * x474; const FLT x476 = (x452 * x447) + (-1 * x6 * x475); const FLT x477 = x476 + x39 + (-1 * x21 * x466) + (-1 * x455 * x449) + (x9 * x462) + (x47 * x461) + x473 + (-1 * x445 * x277) + (-1 * x35 * x468) + (-1 * x445 * x275); const FLT x478 = (x477 * sensor_pt[0]) + (-1 * x459 * sensor_pt[2]) + (x470 * sensor_pt[1]); const FLT x479 = -1 * x36; const FLT x480 = x472 * x447; const FLT x481 = x21 * x276; const FLT x482 = x471 * x447; const FLT x483 = (-1 * x474 * x439) + (x452 * x439); const FLT x484 = (-1 * x445 * x286) + (x35 * x455) + (-1 * x445 * x288) + (-1 * x23 * x466) + (-1 * x468 * x449) + x479 + x483 + x480 + (-1 * x49 * x461) + (-1 * x482) + (x481 * x445); const FLT x485 = (-1 * x484 * sensor_pt[0]) + (x459 * sensor_pt[1]) + (x470 * sensor_pt[2]); const FLT x486 = (-1 * x477 * x285) + (-1 * x478 * x284) + (x485 * x295) + (x484 * x296); const FLT x487 = (x484 * sensor_pt[2]) + (-1 * x477 * sensor_pt[1]) + (x470 * sensor_pt[0]); const FLT x488 = (x478 * x300) + (-1 * x484 * x299) + (-1 * x487 * x295) + (x459 * x285); const FLT x489 = (x487 * x284) + (x477 * x299) + (-1 * x459 * x296) + (-1 * x485 * x300); const FLT x490 = (x489 * (*lh_p).Rot[1]) + (-1 * x486 * (*lh_p).Rot[2]) + (x488 * (*lh_p).Rot[0]); const FLT x491 = (-1 * x489 * (*lh_p).Rot[3]) + (x488 * (*lh_p).Rot[2]) + (x486 * (*lh_p).Rot[0]); const FLT x492 = x489 + (-1 * x490 * x183) + (x491 * x194); const FLT x493 = (x486 * (*lh_p).Rot[3]) + (-1 * x488 * (*lh_p).Rot[1]) + (x489 * (*lh_p).Rot[0]); const FLT x494 = x486 + (-1 * x493 * x194) + (x490 * x341); const FLT x495 = x488 + (-1 * x491 * x341) + (x493 * x183); const FLT x496 = (x495 * x111) + (x494 * x105); const FLT x497 = (x72 * x492) + (-1 * x130 * (x496 + (x492 * x129))); const FLT x498 = x497 * x155; const FLT x499 = (x497 * x158) + (x73 * ((-1 * x497 * x157) + x498)); const FLT x500 = (x73 * x499) + (x497 * x154); const FLT x501 = (-1 * x87 * x492) + (x496 * x114); const FLT x502 = ((-1 * x495 * x122) + (x494 * x121)) * x123; const FLT x503 = x502 + (-1 * x501 * x120); const FLT x504 = x502 + (-1 * x99 * ((x497 * x168) + x501 + (x97 * x500) + (x503 * x169) + (-1 * x140 * ((x92 * ((x73 * x500) + (x497 * x139) + (x497 * x165) + (x73 * (x500 + (x73 * ((x497 * x133) + x499 + (x73 * ((x497 * x167) + x498 + (-1 * x497 * x166))))) + (x497 * x137))))) + (x503 * x164))))); const FLT x505 = x3 * x9; const FLT x506 = x10 * x441; const FLT x507 = x9 * x423; const FLT x508 = x9 * x427; const FLT x509 = x9 * x9 * x9; const FLT x510 = (-1 * x427 * x509) + (x424 * x505) + (-1 * x430 * x505) + (-1 * x7 * x508) + (x7 * x507) + (-1 * x4 * x508) + (x423 * x509) + (x4 * x507); const FLT x511 = x444 * x510; const FLT x512 = x21 * x511; const FLT x513 = x2 * x9; const FLT x514 = (x450 * x513) + (-1 * x457 * x513); const FLT x515 = x483 + x36 + (x511 * x238) + (-1 * x435 * x510) + x514 + (x49 * x506) + (-1 * x512 * x246) + (-1 * x511 * x250) + (-1 * x437 * x505) + (-1 * x10 * x453); const FLT x516 = -1 * x39; const FLT x517 = x442 * x513; const FLT x518 = x453 * x513; const FLT x519 = x10 * x448; const FLT x520 = x436 * x505; const FLT x521 = x465 * x510; const FLT x522 = x473 + (-1 * x24 * x521) + (x449 * x519) + (x511 * x270) + x516 + (x6 * x511 * x265) + (-1 * x517) + (x512 * x237) + (-1 * x456 * x520) + (-1 * x47 * x506) + x518; const FLT x523 = (x452 * x513) + (-1 * x9 * x475); const FLT x524 = (-1 * x35 * x520) + (-1 * x52 * x506) + (-1 * x21 * x521) + (-1 * x463) + (-1 * x511 * x275) + x460 + x523 + x464 + (x43 * x519) + (x511 * x396) + (-1 * x511 * x277); const FLT x525 = (x524 * sensor_pt[0]) + (-1 * x515 * sensor_pt[2]) + (x522 * sensor_pt[1]); const FLT x526 = x472 * x513; const FLT x527 = x471 * x513; const FLT x528 = (x481 * x511) + (x10 * x452) + x454 + x526 + (-1 * x443) + x46 + (-1 * x23 * x521) + (-1 * x511 * x286) + (-1 * x10 * x474) + (-1 * x449 * x520) + (-1 * x511 * x288) + (-1 * x527); const FLT x529 = (x515 * sensor_pt[1]) + (-1 * x528 * sensor_pt[0]) + (x522 * sensor_pt[2]); const FLT x530 = (x528 * x296) + (x529 * x295) + (-1 * x525 * x284) + (-1 * x524 * x285); const FLT x531 = (-1 * x524 * sensor_pt[1]) + (x528 * sensor_pt[2]) + (x522 * sensor_pt[0]); const FLT x532 = (-1 * x528 * x299) + (x525 * x300) + (-1 * x531 * x295) + (x515 * x285); const FLT x533 = (-1 * x529 * x300) + (x531 * x284) + (-1 * x515 * x296) + (x524 * x299); const FLT x534 = (x533 * (*lh_p).Rot[1]) + (-1 * x530 * (*lh_p).Rot[2]) + (x532 * (*lh_p).Rot[0]); const FLT x535 = 2 * ((x532 * (*lh_p).Rot[2]) + (-1 * x533 * (*lh_p).Rot[3]) + (x530 * (*lh_p).Rot[0])); const FLT x536 = x533 + (-1 * x534 * x183) + (x535 * (*lh_p).Rot[3]); const FLT x537 = 2 * ((x530 * (*lh_p).Rot[3]) + (x533 * (*lh_p).Rot[0]) + (-1 * x532 * (*lh_p).Rot[1])); const FLT x538 = x530 + (-1 * x537 * (*lh_p).Rot[3]) + (x534 * x341); const FLT x539 = x532 + (-1 * x535 * (*lh_p).Rot[2]) + (x537 * (*lh_p).Rot[1]); const FLT x540 = (x539 * x111) + (x538 * x105); const FLT x541 = (x72 * x536) + (-1 * x130 * (x540 + (x536 * x129))); const FLT x542 = x541 * x155; const FLT x543 = (x541 * x158) + (x73 * ((-1 * x541 * x157) + x542)); const FLT x544 = (x73 * x543) + (x541 * x154); const FLT x545 = (-1 * x87 * x536) + (x540 * x114); const FLT x546 = ((-1 * x539 * x122) + (x538 * x121)) * x123; const FLT x547 = x546 + (-1 * x545 * x120); const FLT x548 = x546 + (-1 * x99 * (x545 + (x547 * x169) + (-1 * x140 * ((x92 * ((x73 * x544) + (x541 * x139) + (x541 * x165) + (x73 * (x544 + (x73 * (x543 + (x541 * x133) + (x73 * ((x541 * x167) + x542 + (-1 * x541 * x166))))) + (x541 * x137))))) + (x547 * x164))) + (x541 * x168) + (x97 * x544))); const FLT x549 = x2 * x423; const FLT x550 = (x2 * x2 * x2) * x419; const FLT x551 = x2 * x427; const FLT x552 = (-1 * x2 * x431) + (x10 * x549) + (-1 * x426 * x550) + (x422 * x550) + (-1 * x10 * x551) + (x7 * x549) + (-1 * x7 * x551) + (x2 * x425); const FLT x553 = x444 * x552; const FLT x554 = x21 * x553; const FLT x555 = x4 * x448; const FLT x556 = x4 * x441; const FLT x557 = (-1 * x47 * x556) + (x553 * x238) + (x449 * x555) + (-1 * x2 * x438) + (-1 * x553 * x250) + (-1 * x518) + (-1 * x554 * x246) + x517 + x476 + x516 + (-1 * x435 * x552); const FLT x558 = x2 * x467; const FLT x559 = x465 * x552; const FLT x560 = x523 + (-1 * x553 * x286) + x469 + (-1 * x43 * x555) + x44 + (-1 * x23 * x559) + (x481 * x553) + (-1 * x449 * x558) + (x52 * x556) + (-1 * x553 * x288); const FLT x561 = x553 * x265; const FLT x562 = (x6 * x561) + x482 + (-1 * x456 * x558) + (x553 * x270) + x514 + (x554 * x237) + x479 + (x4 * x453) + (-1 * x480) + (-1 * x24 * x559) + (-1 * x49 * x556); const FLT x563 = (x557 * sensor_pt[1]) + (-1 * x560 * sensor_pt[0]) + (x562 * sensor_pt[2]); const FLT x564 = (x9 * x561) + (-1 * x4 * x474) + (-1 * x553 * x275) + x46 + (-1 * x21 * x559) + (-1 * x553 * x277) + x458 + (-1 * x35 * x558) + (x42 * x556) + x527 + (-1 * x451) + (-1 * x526); const FLT x565 = (x560 * sensor_pt[2]) + (-1 * x564 * sensor_pt[1]) + (x562 * sensor_pt[0]); const FLT x566 = (x565 * x284) + (x564 * x299) + (-1 * x557 * x296) + (-1 * x563 * x300); const FLT x567 = (x564 * sensor_pt[0]) + (-1 * x557 * sensor_pt[2]) + (x562 * sensor_pt[1]); const FLT x568 = (x563 * x295) + (-1 * x567 * x284) + (x560 * x296) + (-1 * x564 * x285); const FLT x569 = (x557 * x285) + (-1 * x565 * x295) + (-1 * x560 * x299) + (x567 * x300); const FLT x570 = (x569 * (*lh_p).Rot[2]) + (-1 * x566 * (*lh_p).Rot[3]) + (x568 * (*lh_p).Rot[0]); const FLT x571 = (x566 * (*lh_p).Rot[1]) + (-1 * x568 * (*lh_p).Rot[2]) + (x569 * (*lh_p).Rot[0]); const FLT x572 = (x570 * x194) + x566 + (-1 * x571 * x183); const FLT x573 = (-1 * x569 * (*lh_p).Rot[1]) + (x568 * (*lh_p).Rot[3]) + (x566 * (*lh_p).Rot[0]); const FLT x574 = x568 + (-1 * x573 * x194) + (x571 * x341); const FLT x575 = x569 + (-1 * x570 * x341) + (x573 * x183); const FLT x576 = (x575 * x111) + (x574 * x105); const FLT x577 = (x72 * x572) + (-1 * x130 * (x576 + (x572 * x129))); const FLT x578 = x577 * x155; const FLT x579 = (x577 * x158) + (x73 * ((-1 * x577 * x157) + x578)); const FLT x580 = (x73 * x579) + (x577 * x154); const FLT x581 = (-1 * x87 * x572) + (x576 * x114); const FLT x582 = ((-1 * x575 * x122) + (x574 * x121)) * x123; const FLT x583 = x582 + (-1 * x581 * x120); const FLT x584 = x582 + (-1 * x99 * ((x583 * x169) + x581 + (-1 * x140 * ((x92 * ((x73 * x580) + (x577 * x139) + (x73 * ((x73 * (x579 + (x577 * x133) + (x73 * ((x577 * x167) + x578 + (-1 * x577 * x166))))) + x580 + (x577 * x137))) + (x577 * x165))) + (x583 * x164))) + (x97 * x580) + (x577 * x168))); const FLT x585 = dt * x184; const FLT x586 = dt * x186; const FLT x587 = x586 + x585; const FLT x588 = -1 * dt * x190; const FLT x589 = -1 * dt * x188; const FLT x590 = dt + x589 + x588; const FLT x591 = dt * x193; const FLT x592 = dt * x195; const FLT x593 = x592 + (-1 * x591); const FLT x594 = (x593 * x111) + (x590 * x105); const FLT x595 = (x72 * x587) + (-1 * x130 * (x594 + (x587 * x129))); const FLT x596 = x595 * x155; const FLT x597 = (x595 * x158) + (x73 * ((-1 * x595 * x157) + x596)); const FLT x598 = (x73 * x597) + (x595 * x154); const FLT x599 = (-1 * x87 * x587) + (x594 * x114); const FLT x600 = ((-1 * x593 * x122) + (x590 * x121)) * x123; const FLT x601 = x600 + (-1 * x599 * x120); const FLT x602 = x600 + (-1 * x99 * ((x601 * x169) + (x595 * x168) + (x97 * x598) + x599 + (-1 * x140 * ((x92 * ((x595 * x139) + (x595 * x165) + (x73 * x598) + (x73 * (x598 + (x73 * (x597 + (x595 * x133) + (x73 * (x596 + (x595 * x167) + (-1 * x595 * x166))))) + (x595 * x137))))) + (x601 * x164))))); const FLT x603 = (-1 * dt * x206) + dt; const FLT x604 = x603 + x589; const FLT x605 = x585 + (-1 * x586); const FLT x606 = dt * x210; const FLT x607 = dt * x211; const FLT x608 = x607 + x606; const FLT x609 = (x608 * x111) + (x605 * x105); const FLT x610 = x128 * ((x72 * x604) + (-1 * x130 * (x609 + (x604 * x129)))); const FLT x611 = x75 * x610; const FLT x612 = (x76 * x610) + (x73 * ((-1 * x74 * x610) + x611)); const FLT x613 = (x73 * x612) + (x77 * x610); const FLT x614 = (-1 * x87 * x604) + (x609 * x114); const FLT x615 = ((-1 * x608 * x122) + (x605 * x121)) * x123; const FLT x616 = x615 + (-1 * x614 * x120); const FLT x617 = x615 + (-1 * x99 * (x614 + (-1 * x140 * ((x92 * ((x73 * x613) + (x83 * x610) + (x73 * ((x73 * (x612 + (x81 * x610) + (x73 * ((x80 * x610) + x611 + (-1 * x610 * x134))))) + x613 + (x82 * x610))) + (x78 * x610))) + (x616 * x164))) + (x616 * x169) + (x610 * x141) + (x97 * x613))); const FLT x618 = x606 + (-1 * x607); const FLT x619 = x591 + x592; const FLT x620 = x603 + x588; const FLT x621 = (x620 * x111) + (x619 * x105); const FLT x622 = (x72 * x618) + (-1 * x130 * (x621 + (x618 * x129))); const FLT x623 = x622 * x155; const FLT x624 = x622 * x128; const FLT x625 = (x622 * x158) + (x73 * ((-1 * x74 * x624) + x623)); const FLT x626 = (x73 * x625) + (x622 * x154); const FLT x627 = (-1 * x87 * x618) + (x621 * x114); const FLT x628 = ((-1 * x620 * x122) + (x619 * x121)) * x123; const FLT x629 = x628 + (-1 * x627 * x120); const FLT x630 = x628 + (-1 * x99 * (x627 + (x629 * x169) + (x97 * x626) + (-1 * x140 * ((x92 * ((x622 * x139) + (x73 * x626) + (x622 * x165) + (x73 * (x626 + (x73 * (x625 + (x622 * x133) + (x73 * ((x622 * x167) + x623 + (-1 * x624 * x134))))) + (x622 * x137))))) + (x629 * x164))) + (x624 * x141))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x143 + (x144 * x143)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x170 + (x170 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x182 + (x182 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[0])/sizeof(FLT), x205 + (x205 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[1])/sizeof(FLT), x221 + (x221 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Pos[2])/sizeof(FLT), x233 + (x233 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x317 + (x317 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x354 + (x354 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x386 + (x386 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x417 + (x417 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x504 + (x504 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x548 + (x548 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x584 + (x584 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[0])/sizeof(FLT), x602 + (x602 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[1])/sizeof(FLT), x617 + (x617 * x144)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Velocity.Pos[2])/sizeof(FLT), x630 + (x630 * x144)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = dt * fabs(dt); const FLT x1 = 1.0/2.0 * x0; const FLT x2 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x3 = 0.5 * (*_x0).Pose.Rot[2]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x5 = 0.5 * (*_x0).Pose.Rot[1]; const FLT x6 = (*_x0).Pose.Rot[3] + (-1 * x3 * (*error_model).Pose.AxisAngleRot[0]) + (x5 * (*error_model).Pose.AxisAngleRot[1]) + (x4 * (*_x0).Pose.Rot[0]); const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x8 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x9 = (-1 * x5 * (*error_model).Pose.AxisAngleRot[2]) + (x8 * (*_x0).Pose.Rot[0]) + (x7 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x10 = (-1 * x3 * (*error_model).Pose.AxisAngleRot[1]) + (-1 * x5 * (*error_model).Pose.AxisAngleRot[0]) + (*_x0).Pose.Rot[0] + (-1 * x4 * (*_x0).Pose.Rot[3]); const FLT x11 = (-1 * x8 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[1] + (x7 * (*_x0).Pose.Rot[0]) + (x3 * (*error_model).Pose.AxisAngleRot[2]); const FLT x12 = (x11 * x11) + (x10 * x10) + (x6 * x6) + (x9 * x9); const FLT x13 = 1. / sqrt(x12); const FLT x14 = dt * dt; const FLT x15 = x2 * x2; const FLT x16 = x15 * x14; const FLT x17 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x18 = x17 * x17; const FLT x19 = x14 * x18; const FLT x20 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x21 = x20 * x20; const FLT x22 = x21 * x14; const FLT x23 = 1e-10 + x22 + x16 + x19; const FLT x24 = sqrt(x23); const FLT x25 = 0.5 * x24; const FLT x26 = sin(x25); const FLT x27 = (1. / x24) * x26; const FLT x28 = dt * x27; const FLT x29 = x28 * x13; const FLT x30 = x26 * x26; const FLT x31 = 1. / x23; const FLT x32 = x30 * x31; const FLT x33 = cos(x25); const FLT x34 = (x32 * x19) + (x32 * x16) + (x33 * x33) + (x32 * x22); const FLT x35 = 1. / sqrt(x34); const FLT x36 = x6 * x35; const FLT x37 = x36 * x29; const FLT x38 = x9 * x35; const FLT x39 = x38 * x29; const FLT x40 = x33 * x13; const FLT x41 = x40 * x35; const FLT x42 = x41 * x10; const FLT x43 = x35 * x11; const FLT x44 = x43 * x29; const FLT x45 = (-1 * x44 * x17) + x42 + (-1 * x2 * x37) + (-1 * x39 * x20); const FLT x46 = x35 * x10; const FLT x47 = x46 * x29; const FLT x48 = x9 * x41; const FLT x49 = (-1 * x37 * x17) + (x2 * x44) + (x47 * x20) + x48; const FLT x50 = x6 * x41; const FLT x51 = (x39 * x17) + x50 + (x2 * x47) + (-1 * x44 * x20); const FLT x52 = (-1 * x51 * sensor_pt[1]) + (x45 * sensor_pt[0]) + (x49 * sensor_pt[2]); const FLT x53 = x40 * x43; const FLT x54 = (-1 * x2 * x39) + (x47 * x17) + (x37 * x20) + x53; const FLT x55 = (-1 * x54 * sensor_pt[2]) + (x45 * sensor_pt[1]) + (x51 * sensor_pt[0]); const FLT x56 = (*_x0).Pose.Pos[2] + (x1 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x54 * x55) + (-1 * x52 * x49))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x57 = (-1 * x49 * sensor_pt[0]) + (x45 * sensor_pt[2]) + (x54 * sensor_pt[1]); const FLT x58 = (*_x0).Pose.Pos[1] + (2 * ((x52 * x51) + (-1 * x54 * x57))) + (x1 * ((*_x0).Acc[1] + (*error_model).Acc[1])) + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])); const FLT x59 = (2 * ((x57 * x49) + (-1 * x51 * x55))) + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + (x1 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x60 = (-1 * x59 * (*lh_p).Rot[2]) + (x56 * (*lh_p).Rot[0]) + (x58 * (*lh_p).Rot[1]); const FLT x61 = (x59 * (*lh_p).Rot[0]) + (-1 * x58 * (*lh_p).Rot[3]) + (x56 * (*lh_p).Rot[2]); const FLT x62 = x58 + (*lh_p).Pos[1] + (2 * ((x61 * (*lh_p).Rot[3]) + (-1 * x60 * (*lh_p).Rot[1]))); const FLT x63 = x62 * x62; const FLT x64 = (-1 * x56 * (*lh_p).Rot[1]) + (x58 * (*lh_p).Rot[0]) + (x59 * (*lh_p).Rot[3]); const FLT x65 = x56 + (2 * ((x64 * (*lh_p).Rot[1]) + (-1 * x61 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x66 = x59 + (*lh_p).Pos[0] + (2 * ((x60 * (*lh_p).Rot[2]) + (-1 * x64 * (*lh_p).Rot[3]))); const FLT x67 = x66 * x66; const FLT x68 = x67 + (x65 * x65); const FLT x69 = x68 + x63; const FLT x70 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x71 = cos(x70); const FLT x72 = 1. / x71; const FLT x73 = x72 * (1. / sqrt(x69)); const FLT x74 = asin(x73 * x62); const FLT x75 = 8.0108022e-06 * x74; const FLT x76 = -8.0108022e-06 + (-1 * x75); const FLT x77 = 0.0028679863 + (x74 * x76); const FLT x78 = 5.3685255e-06 + (x74 * x77); const FLT x79 = 0.0076069798 + (x78 * x74); const FLT x80 = x79 * x74; const FLT x81 = -8.0108022e-06 + (-1.60216044e-05 * x74); const FLT x82 = x77 + (x81 * x74); const FLT x83 = x78 + (x82 * x74); const FLT x84 = x79 + (x83 * x74); const FLT x85 = (x84 * x74) + x80; const FLT x86 = atan2(-1 * x65, x66); const FLT x87 = tan(x70); const FLT x88 = x87 * (1. / sqrt(x68)); const FLT x89 = -1 * x88 * x62; const FLT x90 = (-1 * asin(x89)) + (*bsc0).ogeephase + x86; const FLT x91 = (sin(x90) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x92 = sin(x70); const FLT x93 = x92 * x91; const FLT x94 = x71 + (x85 * x93); const FLT x95 = 1. / x94; const FLT x96 = x74 * x74; const FLT x97 = x91 * x96; const FLT x98 = x97 * x95; const FLT x99 = x89 + (x79 * x98); const FLT x100 = 1. / sqrt(1 + (-1 * (x99 * x99))); const FLT x101 = (*lh_p).Rot[2] * (*lh_p).Rot[2]; const FLT x102 = -1 * x0 * x101; const FLT x103 = (*lh_p).Rot[3] * (*lh_p).Rot[3]; const FLT x104 = x1 + (-1 * x0 * x103); const FLT x105 = x104 + x102; const FLT x106 = 2 * x66; const FLT x107 = x0 * (*lh_p).Rot[2]; const FLT x108 = x107 * (*lh_p).Rot[0]; const FLT x109 = x0 * (*lh_p).Rot[1]; const FLT x110 = x109 * (*lh_p).Rot[3]; const FLT x111 = x110 + (-1 * x108); const FLT x112 = 2 * x65; const FLT x113 = (x111 * x112) + (x105 * x106); const FLT x114 = 1.0/2.0 * x62; const FLT x115 = x87 * (1. / (x68 * sqrt(x68))) * x114; const FLT x116 = x109 * (*lh_p).Rot[2]; const FLT x117 = x0 * (*lh_p).Rot[3] * (*lh_p).Rot[0]; const FLT x118 = x117 + x116; const FLT x119 = (-1 * x88 * x118) + (x113 * x115); const FLT x120 = 1. / x68; const FLT x121 = 1. / sqrt(1 + (-1 * (x87 * x87) * x63 * x120)); const FLT x122 = (1. / x67) * x65; const FLT x123 = 1. / x66; const FLT x124 = x67 * x120; const FLT x125 = ((-1 * x111 * x123) + (x105 * x122)) * x124; const FLT x126 = x125 + (-1 * x119 * x121); const FLT x127 = cos(x90) * (*bsc0).ogeemag; const FLT x128 = x85 * x92; const FLT x129 = x128 * x127; const FLT x130 = 2 * x62; const FLT x131 = x72 * (1. / (x69 * sqrt(x69))) * x114; const FLT x132 = (x73 * x118) + (-1 * x131 * (x113 + (x118 * x130))); const FLT x133 = 1. / sqrt(1 + (-1 * (1. / (x71 * x71)) * x63 * (1. / x69))); const FLT x134 = x84 * x133; const FLT x135 = x82 * x133; const FLT x136 = x133 * x132; const FLT x137 = 2.40324066e-05 * x74; const FLT x138 = x76 * x136; const FLT x139 = x81 * x133; const FLT x140 = x77 * x133; const FLT x141 = (x132 * x140) + (x74 * ((-1 * x75 * x136) + x138)); const FLT x142 = x78 * x133; const FLT x143 = (x74 * x141) + (x132 * x142); const FLT x144 = x79 * x97 * (1. / (x94 * x94)); const FLT x145 = 2 * x80 * x91 * x95; const FLT x146 = x79 * x96 * x95; const FLT x147 = x127 * x146; const FLT x148 = x125 + (-1 * x100 * ((x98 * x143) + (x126 * x147) + x119 + (-1 * x144 * ((x93 * ((x74 * x143) + (x134 * x132) + (x79 * x136) + (x74 * (x143 + (x74 * (x141 + (x132 * x135) + (x74 * ((x132 * x139) + (-1 * x137 * x136) + x138)))) + (x83 * x136))))) + (x126 * x129))) + (x136 * x145))); const FLT x149 = cos((-1 * asin(x99)) + x86 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x150 = (*lh_p).Rot[1] * (*lh_p).Rot[1]; const FLT x151 = -1 * x0 * x150; const FLT x152 = x104 + x151; const FLT x153 = x116 + (-1 * x117); const FLT x154 = x107 * (*lh_p).Rot[3]; const FLT x155 = x109 * (*lh_p).Rot[0]; const FLT x156 = x155 + x154; const FLT x157 = (x112 * x156) + (x106 * x153); const FLT x158 = (x73 * x152) + (-1 * x131 * (x157 + (x130 * x152))); const FLT x159 = x76 * x133; const FLT x160 = x158 * x159; const FLT x161 = x75 * x133; const FLT x162 = (x140 * x158) + (x74 * ((-1 * x161 * x158) + x160)); const FLT x163 = (x74 * x162) + (x142 * x158); const FLT x164 = (-1 * x88 * x152) + (x115 * x157); const FLT x165 = ((-1 * x123 * x156) + (x122 * x153)) * x124; const FLT x166 = x127 * (x165 + (-1 * x121 * x164)); const FLT x167 = x133 * x137; const FLT x168 = x83 * x133; const FLT x169 = x79 * x133; const FLT x170 = x133 * x145; const FLT x171 = x165 + (-1 * x100 * ((x166 * x146) + x164 + (x170 * x158) + (x98 * x163) + (-1 * x144 * ((x93 * ((x74 * x163) + (x169 * x158) + (x134 * x158) + (x74 * (x163 + (x74 * ((x135 * x158) + x162 + (x74 * ((x139 * x158) + x160 + (-1 * x167 * x158))))) + (x168 * x158))))) + (x128 * x166))))); const FLT x172 = x154 + (-1 * x155); const FLT x173 = x108 + x110; const FLT x174 = x1 + x102 + x151; const FLT x175 = (x112 * x174) + (x106 * x173); const FLT x176 = (x73 * x172) + (-1 * x131 * (x175 + (x172 * x130))); const FLT x177 = (-1 * x88 * x172) + (x115 * x175); const FLT x178 = ((-1 * x123 * x174) + (x122 * x173)) * x124; const FLT x179 = x178 + (-1 * x121 * x177); const FLT x180 = x176 * x159; const FLT x181 = (x176 * x140) + (x74 * ((-1 * x161 * x176) + x180)); const FLT x182 = (x74 * x181) + (x176 * x142); const FLT x183 = x178 + (-1 * x100 * (x177 + (x98 * x182) + (x179 * x147) + (x170 * x176) + (-1 * x144 * ((x93 * ((x74 * x182) + (x176 * x134) + (x169 * x176) + (x74 * (x182 + (x74 * (x181 + (x176 * x135) + (x74 * ((x176 * x139) + x180 + (-1 * x167 * x176))))) + (x168 * x176))))) + (x129 * x179))))); const FLT x184 = 1.0 * x10; const FLT x185 = 1.0 * x6; const FLT x186 = 1.0 * x11; const FLT x187 = 1.0 * x9; const FLT x188 = (x186 * (*_x0).Pose.Rot[0]) + (x187 * (*_x0).Pose.Rot[3]) + (-1 * x184 * (*_x0).Pose.Rot[1]) + (-1 * x185 * (*_x0).Pose.Rot[2]); const FLT x189 = 1.0/2.0 * (1. / (x12 * sqrt(x12))); const FLT x190 = x28 * x189; const FLT x191 = x188 * x190; const FLT x192 = x46 * x191; const FLT x193 = x2 * x191; const FLT x194 = x36 * x20; const FLT x195 = x33 * x189; const FLT x196 = x188 * x195; const FLT x197 = x35 * x29; const FLT x198 = 0.5 * x2; const FLT x199 = x197 * x198; const FLT x200 = 0.5 * x41; const FLT x201 = x5 * x197; const FLT x202 = x3 * x197; const FLT x203 = (-1 * x20 * x202) + (-1 * x17 * x201) + (-1 * x199 * (*_x0).Pose.Rot[3]) + (x200 * (*_x0).Pose.Rot[0]); const FLT x204 = x203 + (-1 * x191 * x194) + (-1 * x43 * x196) + (-1 * x17 * x192) + (x38 * x193); const FLT x205 = x38 * x191; const FLT x206 = x43 * x191; const FLT x207 = 0.5 * x197; const FLT x208 = x207 * (*_x0).Pose.Rot[3]; const FLT x209 = x20 * x208; const FLT x210 = x207 * (*_x0).Pose.Rot[0]; const FLT x211 = x17 * x210; const FLT x212 = x5 * x41; const FLT x213 = x2 * x202; const FLT x214 = (-1 * x212) + x213 + (-1 * x209) + (-1 * x211); const FLT x215 = x214 + (x20 * x205) + (-1 * x46 * x196) + (x17 * x206) + (x36 * x193); const FLT x216 = x20 * x210; const FLT x217 = x2 * x201; const FLT x218 = x3 * x41; const FLT x219 = x17 * x208; const FLT x220 = x219 + (-1 * x216) + (-1 * x218) + (-1 * x217); const FLT x221 = x220 + (-1 * x36 * x196) + (-1 * x17 * x205) + (x20 * x206) + (-1 * x46 * x193); const FLT x222 = (-1 * x204 * sensor_pt[2]) + (x221 * sensor_pt[0]) + (x215 * sensor_pt[1]); const FLT x223 = 2 * x51; const FLT x224 = x17 * x202; const FLT x225 = x20 * x201; const FLT x226 = x200 * (*_x0).Pose.Rot[3]; const FLT x227 = x36 * x17; const FLT x228 = x199 * (*_x0).Pose.Rot[0]; const FLT x229 = (-1 * x20 * x192) + (-1 * x43 * x193) + (-1 * x225) + x224 + x226 + x228 + (-1 * x38 * x196) + (x227 * x191); const FLT x230 = 2 * x57; const FLT x231 = 2 * x55; const FLT x232 = (-1 * x229 * sensor_pt[0]) + (x204 * sensor_pt[1]) + (x215 * sensor_pt[2]); const FLT x233 = 2 * x49; const FLT x234 = (-1 * x231 * x221) + (-1 * x223 * x222) + (x233 * x232) + (x230 * x229); const FLT x235 = (-1 * x221 * sensor_pt[1]) + (x229 * sensor_pt[2]) + (x215 * sensor_pt[0]); const FLT x236 = 2 * x52; const FLT x237 = 2 * x54; const FLT x238 = (x237 * x222) + (x231 * x204) + (-1 * x233 * x235) + (-1 * x236 * x229); const FLT x239 = (x235 * x223) + (x236 * x221) + (-1 * x230 * x204) + (-1 * x232 * x237); const FLT x240 = (-1 * x234 * (*lh_p).Rot[2]) + (x239 * (*lh_p).Rot[1]) + (x238 * (*lh_p).Rot[0]); const FLT x241 = 2 * (*lh_p).Rot[1]; const FLT x242 = (-1 * x239 * (*lh_p).Rot[3]) + (x238 * (*lh_p).Rot[2]) + (x234 * (*lh_p).Rot[0]); const FLT x243 = 2 * (*lh_p).Rot[3]; const FLT x244 = x239 + (-1 * x240 * x241) + (x242 * x243); const FLT x245 = (x234 * (*lh_p).Rot[3]) + (x239 * (*lh_p).Rot[0]) + (-1 * x238 * (*lh_p).Rot[1]); const FLT x246 = 2 * (*lh_p).Rot[2]; const FLT x247 = (-1 * x243 * x245) + x234 + (x240 * x246); const FLT x248 = x238 + (-1 * x242 * x246) + (x241 * x245); const FLT x249 = (x248 * x112) + (x247 * x106); const FLT x250 = (x73 * x244) + (-1 * x131 * (x249 + (x244 * x130))); const FLT x251 = x250 * x133; const FLT x252 = x76 * x251; const FLT x253 = (x250 * x140) + (x74 * ((-1 * x75 * x251) + x252)); const FLT x254 = (x74 * x253) + (x78 * x251); const FLT x255 = (-1 * x88 * x244) + (x249 * x115); const FLT x256 = ((-1 * x248 * x123) + (x247 * x122)) * x124; const FLT x257 = x256 + (-1 * x255 * x121); const FLT x258 = x256 + (-1 * x100 * (x255 + (x257 * x147) + (-1 * x144 * ((x93 * ((x74 * x254) + (x79 * x251) + (x84 * x251) + (x74 * (x254 + (x74 * ((x82 * x251) + x253 + (x74 * ((x250 * x139) + x252 + (-1 * x251 * x137))))) + (x83 * x251))))) + (x257 * x129))) + (x98 * x254) + (x251 * x145))); const FLT x259 = (x185 * (*_x0).Pose.Rot[1]) + (-1 * x186 * (*_x0).Pose.Rot[3]) + (x187 * (*_x0).Pose.Rot[0]) + (-1 * x184 * (*_x0).Pose.Rot[2]); const FLT x260 = x259 * x190; const FLT x261 = x46 * x260; const FLT x262 = x2 * x260; const FLT x263 = x259 * x195; const FLT x264 = (-1 * x226) + x225 + (-1 * x228) + (-1 * x224); const FLT x265 = x264 + (-1 * x43 * x263) + (-1 * x260 * x194) + (-1 * x17 * x261) + (x38 * x262); const FLT x266 = x43 * x260; const FLT x267 = x38 * x260; const FLT x268 = (x17 * x266) + x220 + (x20 * x267) + (x36 * x262) + (-1 * x46 * x263); const FLT x269 = x212 + (-1 * x36 * x263) + x211 + x209 + (-1 * x213) + (-1 * x17 * x267) + (-1 * x46 * x262) + (x20 * x266); const FLT x270 = (x269 * sensor_pt[0]) + (-1 * x265 * sensor_pt[2]) + (x268 * sensor_pt[1]); const FLT x271 = x203 + (x260 * x227) + (-1 * x43 * x262) + (-1 * x38 * x263) + (-1 * x20 * x261); const FLT x272 = (x265 * sensor_pt[1]) + (-1 * x271 * sensor_pt[0]) + (x268 * sensor_pt[2]); const FLT x273 = (x230 * x271) + (x233 * x272) + (-1 * x270 * x223) + (-1 * x231 * x269); const FLT x274 = (x271 * sensor_pt[2]) + (x268 * sensor_pt[0]) + (-1 * x269 * sensor_pt[1]); const FLT x275 = (x231 * x265) + (x237 * x270) + (-1 * x233 * x274) + (-1 * x236 * x271); const FLT x276 = (x236 * x269) + (-1 * x230 * x265) + (x274 * x223) + (-1 * x237 * x272); const FLT x277 = (x276 * (*lh_p).Rot[1]) + (-1 * x273 * (*lh_p).Rot[2]) + (x275 * (*lh_p).Rot[0]); const FLT x278 = (-1 * x276 * (*lh_p).Rot[3]) + (x275 * (*lh_p).Rot[2]) + (x273 * (*lh_p).Rot[0]); const FLT x279 = x276 + (-1 * x277 * x241) + (x278 * x243); const FLT x280 = (-1 * x275 * (*lh_p).Rot[1]) + (x273 * (*lh_p).Rot[3]) + (x276 * (*lh_p).Rot[0]); const FLT x281 = x273 + (x277 * x246) + (-1 * x280 * x243); const FLT x282 = x275 + (-1 * x278 * x246) + (x280 * x241); const FLT x283 = (x282 * x112) + (x281 * x106); const FLT x284 = (x73 * x279) + (-1 * x131 * (x283 + (x279 * x130))); const FLT x285 = x284 * x159; const FLT x286 = (x284 * x140) + (x74 * ((-1 * x284 * x161) + x285)); const FLT x287 = (x74 * x286) + (x284 * x142); const FLT x288 = (-1 * x88 * x279) + (x283 * x115); const FLT x289 = ((-1 * x282 * x123) + (x281 * x122)) * x124; const FLT x290 = x289 + (-1 * x288 * x121); const FLT x291 = x289 + (-1 * x100 * (x288 + (x290 * x147) + (-1 * x144 * ((x93 * ((x74 * x287) + (x284 * x169) + (x284 * x134) + (x74 * (x287 + (x74 * ((x284 * x135) + x286 + (x74 * ((x284 * x139) + x285 + (-1 * x284 * x167))))) + (x284 * x168))))) + (x290 * x129))) + (x98 * x287) + (x284 * x170))); const FLT x292 = (-1 * x187 * (*_x0).Pose.Rot[1]) + (x185 * (*_x0).Pose.Rot[0]) + (x186 * (*_x0).Pose.Rot[2]) + (-1 * x184 * (*_x0).Pose.Rot[3]); const FLT x293 = x292 * x190; const FLT x294 = x46 * x293; const FLT x295 = x38 * x293; const FLT x296 = x292 * x195; const FLT x297 = x217 + x218 + (-1 * x43 * x296) + (x2 * x295) + (-1 * x17 * x294) + (-1 * x219) + (-1 * x293 * x194) + x216; const FLT x298 = x43 * x293; const FLT x299 = x264 + (-1 * x46 * x296) + (x20 * x295) + (x17 * x298) + (x2 * x36 * x293); const FLT x300 = x203 + (x20 * x298) + (-1 * x2 * x294) + (-1 * x17 * x295) + (-1 * x36 * x296); const FLT x301 = (x300 * sensor_pt[0]) + (-1 * x297 * sensor_pt[2]) + (x299 * sensor_pt[1]); const FLT x302 = (-1 * x20 * x294) + (x293 * x227) + x214 + (-1 * x38 * x296) + (-1 * x2 * x298); const FLT x303 = (x297 * sensor_pt[1]) + (-1 * x302 * sensor_pt[0]) + (x299 * sensor_pt[2]); const FLT x304 = (-1 * x231 * x300) + (x230 * x302) + (-1 * x223 * x301) + (x233 * x303); const FLT x305 = (x302 * sensor_pt[2]) + (-1 * x300 * sensor_pt[1]) + (x299 * sensor_pt[0]); const FLT x306 = (x231 * x297) + (x237 * x301) + (-1 * x233 * x305) + (-1 * x236 * x302); const FLT x307 = (x223 * x305) + (-1 * x230 * x297) + (x236 * x300) + (-1 * x237 * x303); const FLT x308 = (x307 * (*lh_p).Rot[1]) + (-1 * x304 * (*lh_p).Rot[2]) + (x306 * (*lh_p).Rot[0]); const FLT x309 = (x304 * (*lh_p).Rot[3]) + (-1 * x306 * (*lh_p).Rot[1]) + (x307 * (*lh_p).Rot[0]); const FLT x310 = x304 + (x246 * x308) + (-1 * x243 * x309); const FLT x311 = (x306 * (*lh_p).Rot[2]) + (-1 * x307 * (*lh_p).Rot[3]) + (x304 * (*lh_p).Rot[0]); const FLT x312 = x306 + (-1 * x246 * x311) + (x241 * x309); const FLT x313 = (x312 * x112) + (x310 * x106); const FLT x314 = x307 + (x243 * x311) + (-1 * x241 * x308); const FLT x315 = (-1 * x88 * x314) + (x313 * x115); const FLT x316 = ((-1 * x312 * x123) + (x310 * x122)) * x124; const FLT x317 = x316 + (-1 * x315 * x121); const FLT x318 = (x73 * x314) + (-1 * x131 * (x313 + (x314 * x130))); const FLT x319 = x318 * x133; const FLT x320 = x76 * x319; const FLT x321 = (x318 * x140) + (x74 * ((-1 * x75 * x319) + x320)); const FLT x322 = (x74 * x321) + (x78 * x319); const FLT x323 = x316 + (-1 * x100 * (x315 + (x317 * x147) + (x98 * x322) + (-1 * x144 * ((x93 * ((x74 * x322) + (x84 * x319) + (x74 * (x322 + (x74 * ((x82 * x319) + x321 + (x74 * ((x318 * x139) + x320 + (-1 * x319 * x137))))) + (x83 * x319))) + (x79 * x319))) + (x317 * x129))) + (x319 * x145))); const FLT x324 = x241 * (*lh_p).Rot[2]; const FLT x325 = x243 * (*lh_p).Rot[0]; const FLT x326 = x325 + x324; const FLT x327 = 2 * x103; const FLT x328 = -1 * x327; const FLT x329 = 2 * x101; const FLT x330 = -1 * x329; const FLT x331 = 1 + x330 + x328; const FLT x332 = x246 * (*lh_p).Rot[0]; const FLT x333 = x241 * (*lh_p).Rot[3]; const FLT x334 = x333 + (-1 * x332); const FLT x335 = (x334 * x112) + (x331 * x106); const FLT x336 = (x73 * x326) + (-1 * x131 * (x335 + (x326 * x130))); const FLT x337 = x336 * x133; const FLT x338 = x76 * x337; const FLT x339 = (x77 * x337) + (x74 * ((-1 * x75 * x337) + x338)); const FLT x340 = (x74 * x339) + (x78 * x337); const FLT x341 = (-1 * x88 * x326) + (x335 * x115); const FLT x342 = ((-1 * x334 * x123) + (x331 * x122)) * x124; const FLT x343 = x342 + (-1 * x341 * x121); const FLT x344 = x342 + (-1 * x100 * (x341 + (-1 * x144 * ((x93 * ((x79 * x337) + (x84 * x337) + (x74 * x340) + (x74 * (x340 + (x74 * (x339 + (x82 * x337) + (x74 * ((x336 * x139) + x338 + (-1 * x337 * x137))))) + (x83 * x337))))) + (x343 * x129))) + (x343 * x147) + (x337 * x145) + (x98 * x340))); const FLT x345 = 2 * x150; const FLT x346 = 1 + (-1 * x345); const FLT x347 = x346 + x328; const FLT x348 = x324 + (-1 * x325); const FLT x349 = x243 * (*lh_p).Rot[2]; const FLT x350 = x241 * (*lh_p).Rot[0]; const FLT x351 = x350 + x349; const FLT x352 = (x351 * x112) + (x348 * x106); const FLT x353 = (x73 * x347) + (-1 * x131 * (x352 + (x347 * x130))); const FLT x354 = x353 * x133; const FLT x355 = x76 * x354; const FLT x356 = (x353 * x140) + (x74 * ((-1 * x75 * x354) + x355)); const FLT x357 = (x74 * x356) + (x78 * x354); const FLT x358 = (-1 * x88 * x347) + (x352 * x115); const FLT x359 = ((-1 * x351 * x123) + (x348 * x122)) * x124; const FLT x360 = x359 + (-1 * x358 * x121); const FLT x361 = x359 + (-1 * x100 * ((x360 * x147) + (x354 * x145) + (x98 * x357) + x358 + (-1 * x144 * ((x93 * ((x74 * x357) + (x74 * (x357 + (x74 * (x356 + (x82 * x354) + (x74 * (x355 + (x353 * x139) + (-1 * x354 * x137))))) + (x83 * x354))) + (x84 * x354) + (x79 * x354))) + (x360 * x129))))); const FLT x362 = x349 + (-1 * x350); const FLT x363 = x332 + x333; const FLT x364 = x346 + x330; const FLT x365 = (x364 * x112) + (x363 * x106); const FLT x366 = (x73 * x362) + (-1 * x131 * (x365 + (x362 * x130))); const FLT x367 = x366 * x133; const FLT x368 = x76 * x367; const FLT x369 = (x77 * x367) + (x74 * ((-1 * x75 * x367) + x368)); const FLT x370 = (x74 * x369) + (x78 * x367); const FLT x371 = (-1 * x88 * x362) + (x365 * x115); const FLT x372 = ((-1 * x364 * x123) + (x363 * x122)) * x124; const FLT x373 = x372 + (-1 * x371 * x121); const FLT x374 = x372 + (-1 * x100 * (x371 + (x373 * x147) + (x367 * x145) + (x98 * x370) + (-1 * x144 * ((x93 * ((x79 * x367) + (x74 * (x370 + (x74 * (x369 + (x82 * x367) + (x74 * ((x366 * x139) + x368 + (-1 * x367 * x137))))) + (x83 * x367))) + (x74 * x370) + (x84 * x367))) + (x373 * x129))))); const FLT x375 = dt * dt * dt * dt; const FLT x376 = (x17 * x17 * x17) * x375; const FLT x377 = 1.0 * x33; const FLT x378 = (1. / (x23 * sqrt(x23))) * x26; const FLT x379 = x378 * x377; const FLT x380 = x14 * x17; const FLT x381 = 2 * x32; const FLT x382 = 2 * x30 * (1. / (x23 * x23)); const FLT x383 = x375 * x382; const FLT x384 = x15 * x383; const FLT x385 = x379 * x375; const FLT x386 = x21 * x17; const FLT x387 = x27 * x380; const FLT x388 = (-1 * x383 * x386) + (-1 * x377 * x387) + (x381 * x380) + (x385 * x386) + (x379 * x376) + (x15 * x17 * x385) + (-1 * x376 * x382) + (-1 * x17 * x384); const FLT x389 = 1.0/2.0 * (1. / (x34 * sqrt(x34))); const FLT x390 = x40 * x389; const FLT x391 = x390 * x388; const FLT x392 = 0.5 * x13 * x387; const FLT x393 = dt * dt * dt; const FLT x394 = x31 * x393; const FLT x395 = 0.5 * x394; const FLT x396 = x50 * x395; const FLT x397 = x20 * x17; const FLT x398 = x397 * x396; const FLT x399 = x29 * x389; const FLT x400 = x399 * x388; const FLT x401 = x6 * x400; const FLT x402 = x2 * x400; const FLT x403 = x10 * x400; const FLT x404 = x13 * x393; const FLT x405 = x38 * x404; const FLT x406 = x17 * x378; const FLT x407 = x406 * x405; const FLT x408 = x2 * x407; const FLT x409 = x42 * x395; const FLT x410 = x36 * x404; const FLT x411 = x406 * x410; const FLT x412 = x20 * x411; const FLT x413 = x18 * x378; const FLT x414 = x46 * x404; const FLT x415 = x394 * x198; const FLT x416 = x48 * x415; const FLT x417 = x17 * x416; const FLT x418 = (-1 * x417) + x398 + (x9 * x402) + (-1 * x11 * x391) + (-1 * x20 * x401) + (-1 * x413 * x414) + x47 + (-1 * x43 * x392) + (-1 * x17 * x403) + x408 + (x18 * x409) + (-1 * x412); const FLT x419 = -1 * x44; const FLT x420 = x53 * x395; const FLT x421 = x20 * x400; const FLT x422 = x17 * x400; const FLT x423 = x20 * x407; const FLT x424 = x48 * x395; const FLT x425 = x424 * x397; const FLT x426 = x43 * x404; const FLT x427 = x50 * x415; const FLT x428 = (x2 * x411) + (-1 * x17 * x427); const FLT x429 = x428 + (x413 * x426) + (-1 * x46 * x392) + (x2 * x401) + (-1 * x18 * x420) + x419 + (-1 * x425) + (x11 * x422) + (x9 * x421) + (-1 * x10 * x391) + x423; const FLT x430 = x406 * x426; const FLT x431 = (x20 * x430) + (-1 * x420 * x397); const FLT x432 = x42 * x415; const FLT x433 = x406 * x414; const FLT x434 = (-1 * x2 * x433) + (x17 * x432); const FLT x435 = x431 + (-1 * x405 * x413) + (x11 * x421) + (x18 * x424) + (-1 * x2 * x403) + (-1 * x9 * x422) + x434 + x39 + (-1 * x6 * x391) + (-1 * x36 * x392); const FLT x436 = (x435 * sensor_pt[0]) + (-1 * x418 * sensor_pt[2]) + (x429 * sensor_pt[1]); const FLT x437 = x53 * x415; const FLT x438 = x17 * x437; const FLT x439 = x2 * x430; const FLT x440 = -1 * x37; const FLT x441 = (-1 * x20 * x433) + (x409 * x397); const FLT x442 = x441 + (x413 * x410) + (-1 * x439) + (-1 * x20 * x403) + x440 + (-1 * x38 * x392) + (-1 * x11 * x402) + (x17 * x401) + x438 + (-1 * x18 * x396) + (-1 * x9 * x391); const FLT x443 = (-1 * x442 * sensor_pt[0]) + (x418 * sensor_pt[1]) + (x429 * sensor_pt[2]); const FLT x444 = (x443 * x233) + (-1 * x435 * x231) + (-1 * x436 * x223) + (x442 * x230); const FLT x445 = (x442 * sensor_pt[2]) + (-1 * x435 * sensor_pt[1]) + (x429 * sensor_pt[0]); const FLT x446 = (x436 * x237) + (-1 * x445 * x233) + (-1 * x442 * x236) + (x418 * x231); const FLT x447 = (x435 * x236) + (-1 * x418 * x230) + (x445 * x223) + (-1 * x443 * x237); const FLT x448 = (x447 * (*lh_p).Rot[1]) + (-1 * x444 * (*lh_p).Rot[2]) + (x446 * (*lh_p).Rot[0]); const FLT x449 = (x446 * (*lh_p).Rot[2]) + (-1 * x447 * (*lh_p).Rot[3]) + (x444 * (*lh_p).Rot[0]); const FLT x450 = x447 + (-1 * x448 * x241) + (x449 * x243); const FLT x451 = (x444 * (*lh_p).Rot[3]) + (-1 * x446 * (*lh_p).Rot[1]) + (x447 * (*lh_p).Rot[0]); const FLT x452 = x444 + (-1 * x451 * x243) + (x448 * x246); const FLT x453 = x446 + (-1 * x449 * x246) + (x451 * x241); const FLT x454 = (x453 * x112) + (x452 * x106); const FLT x455 = (x73 * x450) + (-1 * x131 * (x454 + (x450 * x130))); const FLT x456 = x455 * x159; const FLT x457 = (x455 * x140) + (x74 * ((-1 * x455 * x161) + x456)); const FLT x458 = (x74 * x457) + (x455 * x142); const FLT x459 = (-1 * x88 * x450) + (x454 * x115); const FLT x460 = ((-1 * x453 * x123) + (x452 * x122)) * x124; const FLT x461 = x460 + (-1 * x459 * x121); const FLT x462 = x460 + (-1 * x100 * ((x455 * x170) + (x461 * x147) + (x98 * x458) + x459 + (-1 * x144 * ((x93 * ((x74 * x458) + (x455 * x134) + (x455 * x169) + (x74 * ((x74 * (x457 + (x455 * x135) + (x74 * ((x455 * x139) + x456 + (-1 * x455 * x167))))) + x458 + (x455 * x168))))) + (x461 * x129))))); const FLT x463 = x27 * x14; const FLT x464 = x13 * x463; const FLT x465 = 0.5 * x464; const FLT x466 = x20 * x465; const FLT x467 = x20 * x377; const FLT x468 = x467 * x375; const FLT x469 = x15 * x378; const FLT x470 = x20 * x20 * x20; const FLT x471 = x14 * x381; const FLT x472 = (-1 * x470 * x383) + (x20 * x471) + (x470 * x385) + (-1 * x20 * x18 * x383) + (x468 * x413) + (x469 * x468) + (-1 * x463 * x467) + (-1 * x20 * x384); const FLT x473 = x472 * x399; const FLT x474 = x20 * x473; const FLT x475 = x17 * x473; const FLT x476 = x21 * x378; const FLT x477 = x472 * x390; const FLT x478 = x2 * x473; const FLT x479 = x2 * x20 * x378; const FLT x480 = (-1 * x20 * x416) + (x479 * x405); const FLT x481 = x441 + (x9 * x478) + x37 + (-1 * x11 * x477) + (x21 * x396) + x480 + (-1 * x476 * x410) + (-1 * x6 * x474) + (-1 * x43 * x466) + (-1 * x10 * x475); const FLT x482 = -1 * x39; const FLT x483 = x20 * x427; const FLT x484 = x479 * x410; const FLT x485 = (-1 * x10 * x477) + x482 + (x11 * x475) + (-1 * x46 * x466) + (-1 * x483) + (-1 * x21 * x424) + (x6 * x478) + x431 + (x9 * x474) + (x476 * x405) + x484; const FLT x486 = (x20 * x432) + (-1 * x479 * x414); const FLT x487 = x486 + (-1 * x21 * x420) + (-1 * x10 * x478) + x419 + (-1 * x465 * x194) + (x476 * x426) + x425 + (-1 * x423) + (-1 * x6 * x477) + (x11 * x474) + (-1 * x9 * x475); const FLT x488 = (-1 * x481 * sensor_pt[2]) + (x487 * sensor_pt[0]) + (x485 * sensor_pt[1]); const FLT x489 = x20 * x437; const FLT x490 = x479 * x426; const FLT x491 = (x6 * x475) + (x21 * x409) + x47 + (-1 * x38 * x466) + x489 + (-1 * x9 * x477) + (-1 * x10 * x474) + (-1 * x398) + (-1 * x11 * x478) + (-1 * x490) + (-1 * x476 * x414) + x412; const FLT x492 = (x481 * sensor_pt[1]) + (-1 * x491 * sensor_pt[0]) + (x485 * sensor_pt[2]); const FLT x493 = (x491 * x230) + (-1 * x488 * x223) + (x492 * x233) + (-1 * x487 * x231); const FLT x494 = (x491 * sensor_pt[2]) + (-1 * x487 * sensor_pt[1]) + (x485 * sensor_pt[0]); const FLT x495 = (-1 * x491 * x236) + (-1 * x494 * x233) + (x488 * x237) + (x481 * x231); const FLT x496 = (-1 * x492 * x237) + (x494 * x223) + (-1 * x481 * x230) + (x487 * x236); const FLT x497 = (x496 * (*lh_p).Rot[1]) + (-1 * x493 * (*lh_p).Rot[2]) + (x495 * (*lh_p).Rot[0]); const FLT x498 = (x495 * (*lh_p).Rot[2]) + (-1 * x496 * (*lh_p).Rot[3]) + (x493 * (*lh_p).Rot[0]); const FLT x499 = (-1 * x497 * x241) + x496 + (x498 * x243); const FLT x500 = (x496 * (*lh_p).Rot[0]) + (x493 * (*lh_p).Rot[3]) + (-1 * x495 * (*lh_p).Rot[1]); const FLT x501 = x493 + (-1 * x500 * x243) + (x497 * x246); const FLT x502 = x495 + (-1 * x498 * x246) + (x500 * x241); const FLT x503 = (x502 * x112) + (x501 * x106); const FLT x504 = (x73 * x499) + (-1 * x131 * (x503 + (x499 * x130))); const FLT x505 = x504 * x133; const FLT x506 = x76 * x505; const FLT x507 = (x77 * x505) + (x74 * ((-1 * x75 * x505) + x506)); const FLT x508 = (x74 * x507) + (x78 * x505); const FLT x509 = (-1 * x88 * x499) + (x503 * x115); const FLT x510 = ((-1 * x502 * x123) + (x501 * x122)) * x124; const FLT x511 = x510 + (-1 * x509 * x121); const FLT x512 = x510 + (-1 * x100 * (x509 + (x511 * x147) + (-1 * x144 * ((x93 * ((x74 * x508) + (x79 * x505) + (x84 * x505) + (x74 * (x508 + (x74 * (x507 + (x82 * x505) + (x74 * ((x504 * x139) + x506 + (-1 * x505 * x137))))) + (x83 * x505))))) + (x511 * x129))) + (x505 * x145) + (x98 * x508))); const FLT x513 = x2 * x377; const FLT x514 = x513 * x375; const FLT x515 = x2 * x2 * x2; const FLT x516 = x2 * x383; const FLT x517 = (-1 * x463 * x513) + (-1 * x515 * x383) + (x515 * x385) + (x413 * x514) + (-1 * x18 * x516) + (x476 * x514) + (x2 * x471) + (-1 * x21 * x516); const FLT x518 = x517 * x390; const FLT x519 = x517 * x399; const FLT x520 = x6 * x519; const FLT x521 = x464 * x198; const FLT x522 = x9 * x519; const FLT x523 = x17 * x519; const FLT x524 = (-1 * x10 * x523) + (-1 * x15 * x424) + x434 + (x469 * x405) + (-1 * x484) + (-1 * x43 * x521) + (-1 * x11 * x518) + (x2 * x522) + x482 + (-1 * x20 * x520) + x483; const FLT x525 = x10 * x519; const FLT x526 = x469 * x404; const FLT x527 = x11 * x519; const FLT x528 = x486 + (-1 * x2 * x527) + (-1 * x9 * x518) + (-1 * x38 * x521) + (-1 * x20 * x525) + x44 + (x15 * x420) + x428 + (-1 * x43 * x526) + (x6 * x523); const FLT x529 = (x11 * x523) + x439 + (-1 * x46 * x521) + (-1 * x438) + x480 + (x20 * x522) + (x36 * x526) + (x2 * x520) + x440 + (-1 * x10 * x518) + (-1 * x15 * x396); const FLT x530 = (x524 * sensor_pt[1]) + (-1 * x528 * sensor_pt[0]) + (x529 * sensor_pt[2]); const FLT x531 = (-1 * x9 * x523) + (-1 * x2 * x525) + (x20 * x527) + x47 + (-1 * x6 * x518) + (-1 * x408) + x417 + (-1 * x36 * x521) + x490 + (-1 * x489) + (x15 * x409) + (-1 * x469 * x414); const FLT x532 = (-1 * x531 * sensor_pt[1]) + (x528 * sensor_pt[2]) + (x529 * sensor_pt[0]); const FLT x533 = (x531 * x236) + (x532 * x223) + (-1 * x524 * x230) + (-1 * x530 * x237); const FLT x534 = (x531 * sensor_pt[0]) + (-1 * x524 * sensor_pt[2]) + (x529 * sensor_pt[1]); const FLT x535 = (-1 * x534 * x223) + (x528 * x230) + (x530 * x233) + (-1 * x531 * x231); const FLT x536 = (-1 * x528 * x236) + (x524 * x231) + (-1 * x532 * x233) + (x534 * x237); const FLT x537 = (x536 * (*lh_p).Rot[2]) + (-1 * x533 * (*lh_p).Rot[3]) + (x535 * (*lh_p).Rot[0]); const FLT x538 = (x533 * (*lh_p).Rot[1]) + (-1 * x535 * (*lh_p).Rot[2]) + (x536 * (*lh_p).Rot[0]); const FLT x539 = x533 + (x537 * x243) + (-1 * x538 * x241); const FLT x540 = (x535 * (*lh_p).Rot[3]) + (-1 * x536 * (*lh_p).Rot[1]) + (x533 * (*lh_p).Rot[0]); const FLT x541 = (-1 * x540 * x243) + x535 + (x538 * x246); const FLT x542 = (-1 * x537 * x246) + x536 + (x540 * x241); const FLT x543 = (x542 * x112) + (x541 * x106); const FLT x544 = (x73 * x539) + (-1 * x131 * (x543 + (x539 * x130))); const FLT x545 = x544 * x133; const FLT x546 = x76 * x545; const FLT x547 = (x544 * x140) + (x74 * ((-1 * x75 * x545) + x546)); const FLT x548 = (x74 * x547) + (x544 * x142); const FLT x549 = (-1 * x88 * x539) + (x543 * x115); const FLT x550 = ((-1 * x542 * x123) + (x541 * x122)) * x124; const FLT x551 = x550 + (-1 * x549 * x121); const FLT x552 = x550 + (-1 * x100 * (x549 + (x551 * x147) + (-1 * x144 * ((x93 * ((x74 * x548) + (x79 * x545) + (x74 * (x548 + (x74 * (x547 + (x544 * x135) + (x74 * ((x544 * x139) + x546 + (-1 * x545 * x137))))) + (x83 * x545))) + (x544 * x134))) + (x551 * x129))) + (x98 * x548) + (x545 * x145))); const FLT x553 = dt * x324; const FLT x554 = dt * x325; const FLT x555 = x554 + x553; const FLT x556 = -1 * dt * x329; const FLT x557 = (-1 * dt * x327) + dt; const FLT x558 = x557 + x556; const FLT x559 = dt * x332; const FLT x560 = dt * x333; const FLT x561 = x560 + (-1 * x559); const FLT x562 = (x561 * x112) + (x558 * x106); const FLT x563 = (x73 * x555) + (-1 * x131 * (x562 + (x555 * x130))); const FLT x564 = x563 * x133; const FLT x565 = x76 * x564; const FLT x566 = (x77 * x564) + (x74 * ((-1 * x75 * x564) + x565)); const FLT x567 = (x74 * x566) + (x78 * x564); const FLT x568 = (-1 * x88 * x555) + (x562 * x115); const FLT x569 = ((-1 * x561 * x123) + (x558 * x122)) * x124; const FLT x570 = x569 + (-1 * x568 * x121); const FLT x571 = x569 + (-1 * x100 * (x568 + (x564 * x145) + (x570 * x147) + (x98 * x567) + (-1 * x144 * ((x93 * ((x79 * x564) + (x74 * x567) + (x84 * x564) + (x74 * (x567 + (x74 * ((x82 * x564) + x566 + (x74 * ((x563 * x139) + x565 + (-1 * x564 * x137))))) + (x83 * x564))))) + (x570 * x129))))); const FLT x572 = -1 * dt * x345; const FLT x573 = x557 + x572; const FLT x574 = x553 + (-1 * x554); const FLT x575 = dt * x349; const FLT x576 = dt * x350; const FLT x577 = x576 + x575; const FLT x578 = (x577 * x112) + (x574 * x106); const FLT x579 = (x73 * x573) + (-1 * x131 * (x578 + (x573 * x130))); const FLT x580 = x579 * x159; const FLT x581 = (x579 * x140) + (x74 * ((-1 * x579 * x161) + x580)); const FLT x582 = (x74 * x581) + (x579 * x142); const FLT x583 = (-1 * x88 * x573) + (x578 * x115); const FLT x584 = ((-1 * x577 * x123) + (x574 * x122)) * x124; const FLT x585 = x584 + (-1 * x583 * x121); const FLT x586 = x584 + (-1 * x100 * (x583 + (x585 * x147) + (x579 * x170) + (-1 * x144 * ((x93 * ((x74 * x582) + (x74 * (x582 + (x74 * (x581 + (x579 * x135) + (x74 * ((x579 * x139) + x580 + (-1 * x579 * x167))))) + (x579 * x168))) + (x579 * x134) + (x579 * x169))) + (x585 * x129))) + (x98 * x582))); const FLT x587 = x575 + (-1 * x576); const FLT x588 = x559 + x560; const FLT x589 = dt + x556 + x572; const FLT x590 = (x589 * x112) + (x588 * x106); const FLT x591 = (x73 * x587) + (-1 * x131 * (x590 + (x587 * x130))); const FLT x592 = x591 * x159; const FLT x593 = (x591 * x140) + (x74 * ((-1 * x591 * x161) + x592)); const FLT x594 = (x74 * x593) + (x591 * x142); const FLT x595 = (-1 * x88 * x587) + (x590 * x115); const FLT x596 = ((-1 * x589 * x123) + (x588 * x122)) * x124; const FLT x597 = x596 + (-1 * x595 * x121); const FLT x598 = x596 + (-1 * x100 * ((-1 * x144 * ((x93 * ((x74 * x594) + (x591 * x134) + (x591 * x169) + (x74 * (x594 + (x74 * ((x591 * x135) + x593 + (x74 * (x592 + (x591 * x139) + (-1 * x591 * x167))))) + (x591 * x168))))) + (x597 * x129))) + (x597 * x147) + (x98 * x594) + x595 + (x591 * x170))); cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x148 + (x148 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x171 + (x171 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), x183 + (x183 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x258 + (x258 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x291 + (x291 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x323 + (x323 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[0])/sizeof(FLT), x344 + (x344 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[1])/sizeof(FLT), x361 + (x361 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.Pos[2])/sizeof(FLT), x374 + (x374 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x462 + (x462 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x512 + (x512 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x552 + (x552 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[0])/sizeof(FLT), x571 + (x571 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[1])/sizeof(FLT), x586 + (x586 * x149)); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Velocity.Pos[2])/sizeof(FLT), x598 + (x598 * x149)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x5 = (*_x0).Pose.Rot[3] + (x4 * (*_x0).Pose.Rot[1]) + (-1 * x2 * (*_x0).Pose.Rot[2]) + (x3 * (*_x0).Pose.Rot[0]); const FLT x6 = dt * dt; const FLT x7 = (x1 * x1) * x6; const FLT x8 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x9 = x6 * (x8 * x8); const FLT x10 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x11 = x6 * (x10 * x10); const FLT x12 = 1e-10 + x11 + x7 + x9; const FLT x13 = sqrt(x12); const FLT x14 = 0.5 * x13; const FLT x15 = sin(x14); const FLT x16 = (-1 * x3 * (*_x0).Pose.Rot[1]) + (x4 * (*_x0).Pose.Rot[0]) + (x2 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x17 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (-1 * x4 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x3 * (*_x0).Pose.Rot[3]); const FLT x18 = (*_x0).Pose.Rot[1] + (x2 * (*_x0).Pose.Rot[0]) + (-1 * x4 * (*_x0).Pose.Rot[3]) + (x3 * (*_x0).Pose.Rot[2]); const FLT x19 = (x15 * x15) * (1. / x12); const FLT x20 = cos(x14); const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x5 * x5) + (x16 * x16))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x11 * x19))); const FLT x22 = dt * x21 * x15 * (1. / x13); const FLT x23 = x5 * x22; const FLT x24 = x22 * x16; const FLT x25 = x20 * x21; const FLT x26 = x22 * x18; const FLT x27 = (-1 * x8 * x26) + (x25 * x17) + (-1 * x1 * x23) + (-1 * x24 * x10); const FLT x28 = x22 * x17; const FLT x29 = x28 * x10; const FLT x30 = x25 * x16; const FLT x31 = x1 * x26; const FLT x32 = x8 * x23; const FLT x33 = (-1 * x32) + x31 + x29 + x30; const FLT x34 = x1 * x28; const FLT x35 = x26 * x10; const FLT x36 = x5 * x25; const FLT x37 = x8 * x24; const FLT x38 = x37 + x36 + x34 + (-1 * x35); const FLT x39 = (-1 * x38 * sensor_pt[1]) + (x27 * sensor_pt[0]) + (x33 * sensor_pt[2]); const FLT x40 = x23 * x10; const FLT x41 = x25 * x18; const FLT x42 = x1 * x24; const FLT x43 = x8 * x28; const FLT x44 = x43 + (-1 * x42) + x40 + x41; const FLT x45 = (-1 * x44 * sensor_pt[2]) + (x27 * sensor_pt[1]) + (x38 * sensor_pt[0]); const FLT x46 = (*_x0).Pose.Pos[2] + (x0 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])) + (*error_model).Pose.Pos[2] + (2 * ((x44 * x45) + (-1 * x33 * x39))) + sensor_pt[2]; const FLT x47 = (-1 * x33 * sensor_pt[0]) + (x27 * sensor_pt[2]) + (x44 * sensor_pt[1]); const FLT x48 = (*_x0).Pose.Pos[1] + (*error_model).Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (2 * ((x38 * x39) + (-1 * x44 * x47))) + (x0 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x49 = (x0 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0] + sensor_pt[0] + (2 * ((x47 * x33) + (-1 * x45 * x38))) + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])); const FLT x50 = (-1 * x49 * (*lh_p).Rot[2]) + (x46 * (*lh_p).Rot[0]) + (x48 * (*lh_p).Rot[1]); const FLT x51 = (-1 * x48 * (*lh_p).Rot[3]) + (x49 * (*lh_p).Rot[0]) + (x46 * (*lh_p).Rot[2]); const FLT x52 = x48 + (*lh_p).Pos[1] + (2 * ((x51 * (*lh_p).Rot[3]) + (-1 * x50 * (*lh_p).Rot[1]))); const FLT x53 = x52 * x52; const FLT x54 = (-1 * x46 * (*lh_p).Rot[1]) + (x48 * (*lh_p).Rot[0]) + (x49 * (*lh_p).Rot[3]); const FLT x55 = x46 + (2 * ((x54 * (*lh_p).Rot[1]) + (-1 * x51 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x56 = x49 + (*lh_p).Pos[0] + (2 * ((x50 * (*lh_p).Rot[2]) + (-1 * x54 * (*lh_p).Rot[3]))); const FLT x57 = x56 * x56; const FLT x58 = x57 + (x55 * x55); const FLT x59 = x58 + x53; const FLT x60 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x61 = cos(x60); const FLT x62 = 1. / x61; const FLT x63 = x62 * (1. / sqrt(x59)); const FLT x64 = asin(x63 * x52); const FLT x65 = 8.0108022e-06 * x64; const FLT x66 = -8.0108022e-06 + (-1 * x65); const FLT x67 = 0.0028679863 + (x64 * x66); const FLT x68 = 5.3685255e-06 + (x64 * x67); const FLT x69 = 0.0076069798 + (x64 * x68); const FLT x70 = x64 * x64; const FLT x71 = atan2(-1 * x55, x56); const FLT x72 = tan(x60); const FLT x73 = x72 * (1. / sqrt(x58)); const FLT x74 = -1 * x73 * x52; const FLT x75 = (-1 * asin(x74)) + (*bsc0).ogeephase + x71; const FLT x76 = (sin(x75) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x77 = x64 * x69; const FLT x78 = -8.0108022e-06 + (-1.60216044e-05 * x64); const FLT x79 = x67 + (x78 * x64); const FLT x80 = x68 + (x79 * x64); const FLT x81 = x69 + (x80 * x64); const FLT x82 = (x81 * x64) + x77; const FLT x83 = sin(x60); const FLT x84 = x83 * x76; const FLT x85 = x61 + (x82 * x84); const FLT x86 = 1. / x85; const FLT x87 = x86 * x76; const FLT x88 = x87 * x70; const FLT x89 = x74 + (x88 * x69); const FLT x90 = 1. / sqrt(1 + (-1 * (x89 * x89))); const FLT x91 = 1. / x58; const FLT x92 = 1. / sqrt(1 + (-1 * (x72 * x72) * x53 * x91)); const FLT x93 = x32 + (-1 * x30) + (-1 * x31) + (-1 * x29); const FLT x94 = 2 * x33; const FLT x95 = 1 + (x93 * x94) + (-2 * (x38 * x38)); const FLT x96 = x94 * x27; const FLT x97 = 2 * x38; const FLT x98 = (x97 * x44) + (-1 * x96); const FLT x99 = 2 * x44; const FLT x100 = x97 * x27; const FLT x101 = x100 + (-1 * x93 * x99); const FLT x102 = (x101 * (*lh_p).Rot[1]) + (-1 * x95 * (*lh_p).Rot[2]) + (x98 * (*lh_p).Rot[0]); const FLT x103 = 2 * (*lh_p).Rot[2]; const FLT x104 = (x95 * (*lh_p).Rot[3]) + (-1 * x98 * (*lh_p).Rot[1]) + (x101 * (*lh_p).Rot[0]); const FLT x105 = 2 * (*lh_p).Rot[3]; const FLT x106 = x95 + (x103 * x102) + (-1 * x105 * x104); const FLT x107 = 2 * x56; const FLT x108 = 2 * (*lh_p).Rot[1]; const FLT x109 = (x98 * (*lh_p).Rot[2]) + (-1 * x101 * (*lh_p).Rot[3]) + (x95 * (*lh_p).Rot[0]); const FLT x110 = x98 + (x108 * x104) + (-1 * x103 * x109); const FLT x111 = 2 * x55; const FLT x112 = (x110 * x111) + (x107 * x106); const FLT x113 = 1.0/2.0 * x52; const FLT x114 = x72 * (1. / (x58 * sqrt(x58))) * x113; const FLT x115 = x101 + (x109 * x105) + (-1 * x102 * x108); const FLT x116 = (-1 * x73 * x115) + (x112 * x114); const FLT x117 = (1. / x57) * x55; const FLT x118 = 1. / x56; const FLT x119 = x57 * x91; const FLT x120 = ((-1 * x110 * x118) + (x106 * x117)) * x119; const FLT x121 = x120 + (-1 * x92 * x116); const FLT x122 = cos(x75) * (*bsc0).ogeemag; const FLT x123 = x82 * x83; const FLT x124 = x123 * x122; const FLT x125 = 1. / sqrt(1 + (-1 * (1. / (x61 * x61)) * x53 * (1. / x59))); const FLT x126 = 2 * x52; const FLT x127 = x62 * (1. / (x59 * sqrt(x59))) * x113; const FLT x128 = x125 * ((x63 * x115) + (-1 * x127 * (x112 + (x115 * x126)))); const FLT x129 = x66 * x128; const FLT x130 = 2.40324066e-05 * x64; const FLT x131 = (x67 * x128) + (x64 * ((-1 * x65 * x128) + x129)); const FLT x132 = (x64 * x131) + (x68 * x128); const FLT x133 = x70 * x69; const FLT x134 = (1. / (x85 * x85)) * x76 * x133; const FLT x135 = 2 * x87 * x77; const FLT x136 = x86 * x133; const FLT x137 = x122 * x136; const FLT x138 = x120 + (-1 * x90 * (x116 + (x121 * x137) + (x128 * x135) + (-1 * x134 * ((x84 * ((x64 * x132) + (x69 * x128) + (x81 * x128) + (x64 * (x132 + (x64 * (x131 + (x79 * x128) + (x64 * ((x78 * x128) + x129 + (-1 * x128 * x130))))) + (x80 * x128))))) + (x124 * x121))) + (x88 * x132))); const FLT x139 = cos((-1 * asin(x89)) + x71 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x140 = (-1 * x34) + (-1 * x37) + x35 + (-1 * x36); const FLT x141 = 1 + (x97 * x140) + (-2 * (x44 * x44)); const FLT x142 = (x94 * x44) + (-1 * x100); const FLT x143 = x99 * x27; const FLT x144 = x143 + (-1 * x94 * x140); const FLT x145 = (x144 * (*lh_p).Rot[2]) + (-1 * x141 * (*lh_p).Rot[3]) + (x142 * (*lh_p).Rot[0]); const FLT x146 = (x141 * (*lh_p).Rot[1]) + (x144 * (*lh_p).Rot[0]) + (-1 * x142 * (*lh_p).Rot[2]); const FLT x147 = x141 + (x105 * x145) + (-1 * x108 * x146); const FLT x148 = (x142 * (*lh_p).Rot[3]) + (-1 * x144 * (*lh_p).Rot[1]) + (x141 * (*lh_p).Rot[0]); const FLT x149 = x142 + (x103 * x146) + (-1 * x105 * x148); const FLT x150 = x144 + (x108 * x148) + (-1 * x103 * x145); const FLT x151 = (x111 * x150) + (x107 * x149); const FLT x152 = x125 * ((x63 * x147) + (-1 * x127 * (x151 + (x126 * x147)))); const FLT x153 = (-1 * x73 * x147) + (x114 * x151); const FLT x154 = ((-1 * x118 * x150) + (x117 * x149)) * x119; const FLT x155 = x154 + (-1 * x92 * x153); const FLT x156 = x66 * x152; const FLT x157 = (x67 * x152) + (x64 * ((-1 * x65 * x152) + x156)); const FLT x158 = (x64 * x157) + (x68 * x152); const FLT x159 = x154 + (-1 * x90 * (x153 + (x88 * x158) + (x137 * x155) + (x135 * x152) + (-1 * x134 * ((x84 * ((x64 * x158) + (x64 * (x158 + (x64 * (x157 + (x79 * x152) + (x64 * ((-1 * x130 * x152) + (x78 * x152) + x156)))) + (x80 * x152))) + (x81 * x152) + (x69 * x152))) + (x124 * x155))))); const FLT x160 = x42 + (-1 * x43) + (-1 * x41) + (-1 * x40); const FLT x161 = x96 + (-1 * x97 * x160); const FLT x162 = 1 + (x99 * x160) + (-2 * (x33 * x33)); const FLT x163 = (x97 * x33) + (-1 * x143); const FLT x164 = (-1 * x161 * (*lh_p).Rot[2]) + (x163 * (*lh_p).Rot[1]) + (x162 * (*lh_p).Rot[0]); const FLT x165 = (x161 * (*lh_p).Rot[3]) + (-1 * x162 * (*lh_p).Rot[1]) + (x163 * (*lh_p).Rot[0]); const FLT x166 = x161 + (x103 * x164) + (-1 * x105 * x165); const FLT x167 = (x162 * (*lh_p).Rot[2]) + (-1 * x163 * (*lh_p).Rot[3]) + (x161 * (*lh_p).Rot[0]); const FLT x168 = x162 + (x108 * x165) + (-1 * x103 * x167); const FLT x169 = (x111 * x168) + (x107 * x166); const FLT x170 = x163 + (x105 * x167) + (-1 * x108 * x164); const FLT x171 = (-1 * x73 * x170) + (x114 * x169); const FLT x172 = ((-1 * x118 * x168) + (x117 * x166)) * x119; const FLT x173 = x122 * (x172 + (-1 * x92 * x171)); const FLT x174 = x125 * ((x63 * x170) + (-1 * x127 * (x169 + (x126 * x170)))); const FLT x175 = x66 * x174; const FLT x176 = (x67 * x174) + (x64 * ((-1 * x65 * x174) + x175)); const FLT x177 = (x64 * x176) + (x68 * x174); const FLT x178 = x172 + (-1 * x90 * (x171 + (x173 * x136) + (x174 * x135) + (-1 * x134 * ((x84 * ((x64 * x177) + (x64 * (x177 + (x64 * ((x79 * x174) + x176 + (x64 * ((x78 * x174) + x175 + (-1 * x174 * x130))))) + (x80 * x174))) + (x69 * x174) + (x81 * x174))) + (x123 * x173))) + (x88 * x177))); cnMatrixOptionalSet(Hx, 0, 0, x138 + (x138 * x139)); cnMatrixOptionalSet(Hx, 0, 1, x159 + (x139 * x159)); cnMatrixOptionalSet(Hx, 0, 2, x178 + (x178 * x139)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_lh_p(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = 1.0/2.0 * dt * fabs(dt); const FLT x1 = x0 * ((*_x0).Acc[2] + (*error_model).Acc[2]); const FLT x2 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x6 = (*_x0).Pose.Rot[3] + (x5 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (x4 * (*_x0).Pose.Rot[0]); const FLT x7 = dt * dt; const FLT x8 = (x2 * x2) * x7; const FLT x9 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x10 = x7 * (x9 * x9); const FLT x11 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x12 = x7 * (x11 * x11); const FLT x13 = 1e-10 + x12 + x8 + x10; const FLT x14 = sqrt(x13); const FLT x15 = 0.5 * x14; const FLT x16 = sin(x15); const FLT x17 = (-1 * x4 * (*_x0).Pose.Rot[1]) + (x5 * (*_x0).Pose.Rot[0]) + (x3 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x18 = (-1 * x3 * (*_x0).Pose.Rot[1]) + (-1 * x5 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x4 * (*_x0).Pose.Rot[3]); const FLT x19 = (*_x0).Pose.Rot[1] + (x3 * (*_x0).Pose.Rot[0]) + (-1 * x5 * (*_x0).Pose.Rot[3]) + (x4 * (*_x0).Pose.Rot[2]); const FLT x20 = (1. / x13) * (x16 * x16); const FLT x21 = cos(x15); const FLT x22 = (1. / sqrt((x6 * x6) + (x19 * x19) + (x18 * x18) + (x17 * x17))) * (1. / sqrt((x20 * x10) + (x21 * x21) + (x8 * x20) + (x20 * x12))); const FLT x23 = dt * x22 * (1. / x14) * x16; const FLT x24 = x6 * x23; const FLT x25 = x23 * x17; const FLT x26 = x22 * x21; const FLT x27 = x23 * x19; const FLT x28 = (x26 * x18) + (-1 * x9 * x27) + (-1 * x2 * x24) + (-1 * x25 * x11); const FLT x29 = x23 * x18; const FLT x30 = x9 * x23; const FLT x31 = (-1 * x6 * x30) + (x29 * x11) + (x2 * x27) + (x26 * x17); const FLT x32 = (x6 * x26) + (x30 * x17) + (x2 * x29) + (-1 * x27 * x11); const FLT x33 = (-1 * x32 * sensor_pt[1]) + (x28 * sensor_pt[0]) + (x31 * sensor_pt[2]); const FLT x34 = (-1 * x2 * x25) + (x24 * x11) + (x30 * x18) + (x26 * x19); const FLT x35 = (-1 * x34 * sensor_pt[2]) + (x28 * sensor_pt[1]) + (x32 * sensor_pt[0]); const FLT x36 = 2 * ((x34 * x35) + (-1 * x31 * x33)); const FLT x37 = dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2]); const FLT x38 = (*_x0).Pose.Pos[2] + x1 + (*error_model).Pose.Pos[2] + x36 + sensor_pt[2] + x37; const FLT x39 = x38 * (*lh_p).Rot[0]; const FLT x40 = (-1 * x31 * sensor_pt[0]) + (x28 * sensor_pt[2]) + (x34 * sensor_pt[1]); const FLT x41 = 2 * ((x32 * x33) + (-1 * x40 * x34)); const FLT x42 = dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1]); const FLT x43 = x0 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x44 = (*_x0).Pose.Pos[1] + x41 + (*error_model).Pose.Pos[1] + sensor_pt[1] + x42 + x43; const FLT x45 = x44 * (*lh_p).Rot[1]; const FLT x46 = 2 * ((x40 * x31) + (-1 * x32 * x35)); const FLT x47 = dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0]); const FLT x48 = x0 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x49 = x48 + x46 + (*_x0).Pose.Pos[0] + (*error_model).Pose.Pos[0] + sensor_pt[0] + x47; const FLT x50 = x49 * (*lh_p).Rot[2]; const FLT x51 = x39 + (-1 * x50) + x45; const FLT x52 = x49 * (*lh_p).Rot[0]; const FLT x53 = x38 * (*lh_p).Rot[2]; const FLT x54 = x44 * (*lh_p).Rot[3]; const FLT x55 = (-1 * x54) + x52 + x53; const FLT x56 = x44 + (*lh_p).Pos[1] + (2 * ((x55 * (*lh_p).Rot[3]) + (-1 * x51 * (*lh_p).Rot[1]))); const FLT x57 = x56 * x56; const FLT x58 = x44 * (*lh_p).Rot[0]; const FLT x59 = x49 * (*lh_p).Rot[3]; const FLT x60 = x38 * (*lh_p).Rot[1]; const FLT x61 = (-1 * x60) + x58 + x59; const FLT x62 = x38 + (2 * ((x61 * (*lh_p).Rot[1]) + (-1 * x55 * (*lh_p).Rot[2]))) + (*lh_p).Pos[2]; const FLT x63 = x49 + (*lh_p).Pos[0] + (2 * ((x51 * (*lh_p).Rot[2]) + (-1 * x61 * (*lh_p).Rot[3]))); const FLT x64 = x63 * x63; const FLT x65 = x64 + (x62 * x62); const FLT x66 = x65 + x57; const FLT x67 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x68 = cos(x67); const FLT x69 = 1. / x68; const FLT x70 = (1. / sqrt(x66)) * x69; const FLT x71 = asin(x70 * x56); const FLT x72 = 8.0108022e-06 * x71; const FLT x73 = -8.0108022e-06 + (-1 * x72); const FLT x74 = 0.0028679863 + (x71 * x73); const FLT x75 = 5.3685255e-06 + (x71 * x74); const FLT x76 = 0.0076069798 + (x71 * x75); const FLT x77 = x71 * x76; const FLT x78 = -8.0108022e-06 + (-1.60216044e-05 * x71); const FLT x79 = x74 + (x71 * x78); const FLT x80 = x75 + (x71 * x79); const FLT x81 = x76 + (x80 * x71); const FLT x82 = (x81 * x71) + x77; const FLT x83 = atan2(-1 * x62, x63); const FLT x84 = tan(x67); const FLT x85 = x84 * (1. / sqrt(x65)); const FLT x86 = -1 * x85 * x56; const FLT x87 = (*bsc0).ogeephase + (-1 * asin(x86)) + x83; const FLT x88 = (sin(x87) * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x89 = sin(x67); const FLT x90 = x88 * x89; const FLT x91 = x68 + (x82 * x90); const FLT x92 = 1. / x91; const FLT x93 = x71 * x71; const FLT x94 = x88 * x93; const FLT x95 = x92 * x94; const FLT x96 = x86 + (x76 * x95); const FLT x97 = 1. / sqrt(1 + (-1 * (x96 * x96))); const FLT x98 = 1. / x65; const FLT x99 = 1. / sqrt(1 + (-1 * (x84 * x84) * x57 * x98)); const FLT x100 = x84 * (1. / (x65 * sqrt(x65))); const FLT x101 = x56 * x100; const FLT x102 = x63 * x101; const FLT x103 = x62 * x98; const FLT x104 = x103 + (-1 * x99 * x102); const FLT x105 = cos(x87) * (*bsc0).ogeemag; const FLT x106 = x76 * x93 * x92; const FLT x107 = x105 * x106; const FLT x108 = x82 * x89; const FLT x109 = x108 * x105; const FLT x110 = 1. / sqrt(1 + (-1 * (1. / (x68 * x68)) * (1. / x66) * x57)); const FLT x111 = (1. / (x66 * sqrt(x66))) * x69; const FLT x112 = x56 * x111; const FLT x113 = x63 * x112; const FLT x114 = x110 * x113; const FLT x115 = -1 * x73 * x114; const FLT x116 = 2.40324066e-05 * x71; const FLT x117 = x74 * x110; const FLT x118 = (-1 * x113 * x117) + (x71 * ((x72 * x114) + x115)); const FLT x119 = (x71 * x118) + (-1 * x75 * x114); const FLT x120 = x81 * x110; const FLT x121 = x76 * (1. / (x91 * x91)) * x94; const FLT x122 = 2 * x56; const FLT x123 = x88 * x77 * x92; const FLT x124 = x110 * x111 * x123 * x122; const FLT x125 = x103 + (-1 * x97 * ((-1 * x63 * x124) + (x95 * x119) + (-1 * x121 * ((x90 * ((x71 * x119) + (x71 * (x119 + (x71 * (x118 + (-1 * x79 * x114) + (x71 * ((-1 * x78 * x114) + x115 + (x114 * x116))))) + (-1 * x80 * x114))) + (-1 * x113 * x120) + (-1 * x76 * x114))) + (x109 * x104))) + (x104 * x107) + x102)); const FLT x126 = cos((-1 * asin(x96)) + x83 + (*bsc0).gibpha) * (*bsc0).gibmag; const FLT x127 = x85 * x99; const FLT x128 = x110 * (x70 + (-1 * x57 * x111)); const FLT x129 = 2 * x123; const FLT x130 = x73 * x128; const FLT x131 = (x74 * x128) + (x71 * ((-1 * x72 * x128) + x130)); const FLT x132 = (x71 * x131) + (x75 * x128); const FLT x133 = x97 * ((x95 * x132) + (-1 * x121 * ((x90 * ((x71 * x132) + (x76 * x128) + (x81 * x128) + (x71 * (x132 + (x71 * (x131 + (x79 * x128) + (x71 * ((x78 * x128) + x130 + (-1 * x116 * x128))))) + (x80 * x128))))) + (x109 * x127))) + (x128 * x129) + (-1 * x85) + (x107 * x127)); const FLT x134 = x62 * x56; const FLT x135 = x100 * x134; const FLT x136 = -1 * x63 * x98; const FLT x137 = x136 + (-1 * x99 * x135); const FLT x138 = x111 * x134; const FLT x139 = x110 * x138; const FLT x140 = -1 * x73 * x139; const FLT x141 = (-1 * x117 * x138) + (x71 * ((x72 * x139) + x140)); const FLT x142 = (x71 * x141) + (-1 * x75 * x139); const FLT x143 = x136 + (-1 * x97 * ((-1 * x62 * x124) + x135 + (-1 * x121 * ((x90 * ((x71 * x142) + (-1 * x76 * x139) + (-1 * x120 * x138) + (x71 * (x142 + (x71 * (x141 + (-1 * x79 * x139) + (x71 * ((-1 * x78 * x139) + (x116 * x139) + x140)))) + (-1 * x80 * x139))))) + (x109 * x137))) + (x107 * x137) + (x95 * x142))); const FLT x144 = 2 * x60; const FLT x145 = (2 * x59) + (-1 * x144); const FLT x146 = 2 * x54; const FLT x147 = (2 * x53) + (-1 * x146); const FLT x148 = 2 * x63; const FLT x149 = 2 * x50; const FLT x150 = (2 * x45) + (-1 * x149); const FLT x151 = 2 * x62; const FLT x152 = (x150 * x151) + (x148 * x147); const FLT x153 = 1.0/2.0 * x112; const FLT x154 = x110 * ((x70 * x145) + (-1 * x153 * (x152 + (x122 * x145)))); const FLT x155 = 1.0/2.0 * x101; const FLT x156 = (-1 * x85 * x145) + (x152 * x155); const FLT x157 = (1. / x64) * x62; const FLT x158 = 1. / x63; const FLT x159 = x64 * x98; const FLT x160 = ((-1 * x150 * x158) + (x147 * x157)) * x159; const FLT x161 = x160 + (-1 * x99 * x156); const FLT x162 = x73 * x154; const FLT x163 = (x74 * x154) + (x71 * ((-1 * x72 * x154) + x162)); const FLT x164 = (x71 * x163) + (x75 * x154); const FLT x165 = x160 + (-1 * x97 * (x156 + (x107 * x161) + (x95 * x164) + (x129 * x154) + (-1 * x121 * ((x90 * ((x71 * x164) + (x71 * (x164 + (x71 * (x163 + (x79 * x154) + (x71 * ((x78 * x154) + x162 + (-1 * x116 * x154))))) + (x80 * x154))) + (x76 * x154) + (x81 * x154))) + (x109 * x161))))); const FLT x166 = 2 * x39; const FLT x167 = (-1 * x166) + x149 + (-4 * x45); const FLT x168 = (-1 * sensor_pt[2]) + (-1 * (*_x0).Pose.Pos[2]) + (-1 * x37) + (-1 * x1) + (-1 * (*error_model).Pose.Pos[2]) + (-1 * x36); const FLT x169 = 2 * (*lh_p).Rot[3]; const FLT x170 = 2 * (*lh_p).Rot[2]; const FLT x171 = (x44 * x170) + (-1 * x169 * x168); const FLT x172 = 2 * (*lh_p).Rot[1]; const FLT x173 = 2 * x58; const FLT x174 = x145 + (x168 * x172) + x173; const FLT x175 = (x174 * x151) + (x171 * x148); const FLT x176 = x110 * ((x70 * x167) + (-1 * x153 * (x175 + (x122 * x167)))); const FLT x177 = (-1 * x85 * x167) + (x175 * x155); const FLT x178 = ((-1 * x174 * x158) + (x171 * x157)) * x159; const FLT x179 = x178 + (-1 * x99 * x177); const FLT x180 = x73 * x176; const FLT x181 = (x74 * x176) + (x71 * ((-1 * x72 * x176) + x180)); const FLT x182 = (x71 * x181) + (x75 * x176); const FLT x183 = x178 + (-1 * x97 * ((x95 * x182) + (x129 * x176) + x177 + (x107 * x179) + (-1 * x121 * ((x90 * ((x71 * x182) + (x76 * x176) + (x81 * x176) + (x71 * ((x71 * (x181 + (x79 * x176) + (x71 * ((x78 * x176) + (-1 * x116 * x176) + x180)))) + x182 + (x80 * x176))))) + (x109 * x179))))); const FLT x184 = (-1 * sensor_pt[0]) + (-1 * (*_x0).Pose.Pos[0]) + (-1 * x46) + (-1 * x47) + (-1 * x48) + (-1 * (*error_model).Pose.Pos[0]); const FLT x185 = (x38 * x169) + (-1 * x172 * x184); const FLT x186 = x150 + (x170 * x184) + x166; const FLT x187 = 2 * x52; const FLT x188 = x146 + (-1 * x187) + (-4 * x53); const FLT x189 = (x188 * x151) + (x186 * x148); const FLT x190 = x110 * ((x70 * x185) + (-1 * x153 * (x189 + (x122 * x185)))); const FLT x191 = x73 * x190; const FLT x192 = (x74 * x190) + (x71 * ((-1 * x72 * x190) + x191)); const FLT x193 = (x71 * x192) + (x75 * x190); const FLT x194 = (-1 * x85 * x185) + (x189 * x155); const FLT x195 = ((-1 * x188 * x158) + (x186 * x157)) * x159; const FLT x196 = x105 * (x195 + (-1 * x99 * x194)); const FLT x197 = x195 + (-1 * x97 * (x194 + (-1 * x121 * ((x90 * ((x71 * x193) + (x76 * x190) + (x71 * (x193 + (x71 * ((x79 * x190) + x192 + (x71 * (x191 + (x78 * x190) + (-1 * x116 * x190))))) + (x80 * x190))) + (x81 * x190))) + (x108 * x196))) + (x129 * x190) + (x106 * x196) + (x95 * x193))); const FLT x198 = (-1 * x42) + (-1 * x41) + (-1 * (*error_model).Pose.Pos[1]) + (-1 * sensor_pt[1]) + (-1 * x43) + (-1 * (*_x0).Pose.Pos[1]); const FLT x199 = x147 + x187 + (x169 * x198); const FLT x200 = (-1 * x173) + x144 + (-4 * x59); const FLT x201 = (x49 * x172) + (-1 * x170 * x198); const FLT x202 = (x201 * x151) + (x200 * x148); const FLT x203 = x110 * ((x70 * x199) + (-1 * x153 * (x202 + (x122 * x199)))); const FLT x204 = (-1 * x85 * x199) + (x202 * x155); const FLT x205 = ((-1 * x201 * x158) + (x200 * x157)) * x159; const FLT x206 = x205 + (-1 * x99 * x204); const FLT x207 = x73 * x203; const FLT x208 = (x74 * x203) + (x71 * ((-1 * x72 * x203) + x207)); const FLT x209 = (x71 * x208) + (x75 * x203); const FLT x210 = x205 + (-1 * x97 * (x204 + (x95 * x209) + (x203 * x129) + (x206 * x107) + (-1 * x121 * ((x90 * ((x71 * x209) + (x76 * x203) + (x81 * x203) + (x71 * (x209 + (x71 * (x208 + (x79 * x203) + (x71 * ((x78 * x203) + x207 + (-1 * x203 * x116))))) + (x80 * x203))))) + (x206 * x109))))); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[0])/sizeof(FLT), x125 + (x126 * x125)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[1])/sizeof(FLT), (-1 * x126 * x133) + (-1 * x133)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Pos[2])/sizeof(FLT), x143 + (x126 * x143)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), x165 + (x126 * x165)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), x183 + (x126 * x183)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), x197 + (x126 * x197)); cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), x210 + (x210 * x126)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [(*lh_p).Pos[0], (*lh_p).Pos[1], (*lh_p).Pos[2], (*lh_p).Rot[0], (*lh_p).Rot[1], (*lh_p).Rot[2], (*lh_p).Rot[3]] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_lh_p_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen2_jac_lh_p(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } // Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_bsc0(CnMat* Hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { const FLT x0 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x1 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x2 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x3 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x4 = (x3 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x1 * (*_x0).Pose.Rot[2]) + (x2 * (*_x0).Pose.Rot[0]); const FLT x5 = dt * dt; const FLT x6 = (x0 * x0) * x5; const FLT x7 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x8 = x5 * (x7 * x7); const FLT x9 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x10 = x5 * (x9 * x9); const FLT x11 = 1e-10 + x10 + x6 + x8; const FLT x12 = sqrt(x11); const FLT x13 = 0.5 * x12; const FLT x14 = sin(x13); const FLT x15 = (-1 * x2 * (*_x0).Pose.Rot[1]) + (x3 * (*_x0).Pose.Rot[0]) + (x1 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x16 = (-1 * x1 * (*_x0).Pose.Rot[1]) + (-1 * x3 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x2 * (*_x0).Pose.Rot[3]); const FLT x17 = (x1 * (*_x0).Pose.Rot[0]) + (*_x0).Pose.Rot[1] + (-1 * x3 * (*_x0).Pose.Rot[3]) + (x2 * (*_x0).Pose.Rot[2]); const FLT x18 = (x14 * x14) * (1. / x11); const FLT x19 = cos(x13); const FLT x20 = (1. / sqrt((x17 * x17) + (x16 * x16) + (x4 * x4) + (x15 * x15))) * (1. / sqrt((x8 * x18) + (x6 * x18) + (x19 * x19) + (x10 * x18))); const FLT x21 = dt * x20 * x14 * (1. / x12); const FLT x22 = x4 * x21; const FLT x23 = x21 * x15; const FLT x24 = x20 * x19; const FLT x25 = x21 * x17; const FLT x26 = (-1 * x7 * x25) + (-1 * x0 * x22) + (x24 * x16) + (-1 * x9 * x23); const FLT x27 = x0 * x21; const FLT x28 = (x7 * x23) + (x4 * x24) + (x27 * x16) + (-1 * x9 * x25); const FLT x29 = x21 * x16; const FLT x30 = (x7 * x29) + (-1 * x27 * x15) + (x9 * x22) + (x24 * x17); const FLT x31 = (x26 * sensor_pt[1]) + (-1 * x30 * sensor_pt[2]) + (x28 * sensor_pt[0]); const FLT x32 = (x27 * x17) + (x9 * x29) + (-1 * x7 * x22) + (x24 * x15); const FLT x33 = (-1 * x32 * sensor_pt[0]) + (x26 * sensor_pt[2]) + (x30 * sensor_pt[1]); const FLT x34 = 1.0/2.0 * dt * fabs(dt); const FLT x35 = (x34 * ((*_x0).Acc[0] + (*error_model).Acc[0])) + (2 * ((x32 * x33) + (-1 * x31 * x28))) + sensor_pt[0] + (dt * ((*_x0).Velocity.Pos[0] + (*error_model).Velocity.Pos[0])) + (*error_model).Pose.Pos[0] + (*_x0).Pose.Pos[0]; const FLT x36 = (-1 * x28 * sensor_pt[1]) + (x26 * sensor_pt[0]) + (x32 * sensor_pt[2]); const FLT x37 = (*_x0).Pose.Pos[2] + (x34 * ((*_x0).Acc[2] + (*error_model).Acc[2])) + (*error_model).Pose.Pos[2] + (2 * ((x30 * x31) + (-1 * x32 * x36))) + sensor_pt[2] + (dt * ((*_x0).Velocity.Pos[2] + (*error_model).Velocity.Pos[2])); const FLT x38 = (*_x0).Pose.Pos[1] + (2 * ((x36 * x28) + (-1 * x30 * x33))) + sensor_pt[1] + (dt * ((*_x0).Velocity.Pos[1] + (*error_model).Velocity.Pos[1])) + (*error_model).Pose.Pos[1] + (x34 * ((*_x0).Acc[1] + (*error_model).Acc[1])); const FLT x39 = (-1 * x38 * (*lh_p).Rot[3]) + (x35 * (*lh_p).Rot[0]) + (x37 * (*lh_p).Rot[2]); const FLT x40 = (-1 * x37 * (*lh_p).Rot[1]) + (x38 * (*lh_p).Rot[0]) + (x35 * (*lh_p).Rot[3]); const FLT x41 = (2 * ((x40 * (*lh_p).Rot[1]) + (-1 * x39 * (*lh_p).Rot[2]))) + x37 + (*lh_p).Pos[2]; const FLT x42 = (-1 * x35 * (*lh_p).Rot[2]) + (x37 * (*lh_p).Rot[0]) + (x38 * (*lh_p).Rot[1]); const FLT x43 = (*lh_p).Pos[0] + x35 + (2 * ((x42 * (*lh_p).Rot[2]) + (-1 * x40 * (*lh_p).Rot[3]))); const FLT x44 = atan2(-1 * x41, x43); const FLT x45 = 0.523598775598299 + (-1 * (*bsc0).tilt); const FLT x46 = tan(x45); const FLT x47 = (*lh_p).Pos[1] + x38 + (2 * ((x39 * (*lh_p).Rot[3]) + (-1 * x42 * (*lh_p).Rot[1]))); const FLT x48 = (x43 * x43) + (x41 * x41); const FLT x49 = x47 * (1. / sqrt(x48)); const FLT x50 = -1 * x46 * x49; const FLT x51 = (-1 * asin(x50)) + (*bsc0).ogeephase + x44; const FLT x52 = sin(x51); const FLT x53 = (x52 * (*bsc0).ogeemag) + (*bsc0).curve; const FLT x54 = cos(x45); const FLT x55 = x47 * x47; const FLT x56 = x48 + x55; const FLT x57 = (1. / sqrt(x56)) * x47; const FLT x58 = asin((1. / x54) * x57); const FLT x59 = 8.0108022e-06 * x58; const FLT x60 = -8.0108022e-06 + (-1 * x59); const FLT x61 = 0.0028679863 + (x60 * x58); const FLT x62 = 5.3685255e-06 + (x61 * x58); const FLT x63 = 0.0076069798 + (x62 * x58); const FLT x64 = x58 * x58; const FLT x65 = x63 * x58; const FLT x66 = -8.0108022e-06 + (-1.60216044e-05 * x58); const FLT x67 = x61 + (x66 * x58); const FLT x68 = x62 + (x67 * x58); const FLT x69 = x63 + (x68 * x58); const FLT x70 = (x69 * x58) + x65; const FLT x71 = sin(x45); const FLT x72 = x71 * x53; const FLT x73 = x70 * x72; const FLT x74 = x54 + x73; const FLT x75 = 1. / x74; const FLT x76 = x75 * x64; const FLT x77 = x76 * x63; const FLT x78 = x50 + (x77 * x53); const FLT x79 = 1. / sqrt(1 + (-1 * (x78 * x78))); const FLT x80 = (1. / (x74 * x74)) * x63 * x64; const FLT x81 = x80 * x73; const FLT x82 = ((-1 * x81) + x77) * x79; const FLT x83 = (-1 * asin(x78)) + x44 + (*bsc0).gibpha; const FLT x84 = cos(x83) * (*bsc0).gibmag; const FLT x85 = ((-1 * x81 * x52) + (x77 * x52)) * x79; const FLT x86 = cos(x51) * (*bsc0).ogeemag; const FLT x87 = x86 * x77; const FLT x88 = x79 * ((-1 * x81 * x86) + x87); const FLT x89 = x46 * x46; const FLT x90 = x49 * (1 + x89); const FLT x91 = x90 * (1. / sqrt(1 + (-1 * x89 * x55 * (1. / x48)))); const FLT x92 = 1. / (x54 * x54); const FLT x93 = x57 * x92 * (1. / sqrt(1 + (-1 * (1. / x56) * x55 * x92))); const FLT x94 = x71 * x93; const FLT x95 = -1 * x60 * x94; const FLT x96 = (-1 * x61 * x94) + (x58 * ((x59 * x94) + x95)); const FLT x97 = (x58 * x96) + (-1 * x62 * x94); const FLT x98 = x79 * ((-1 * x80 * x53 * ((-1 * x86 * x71 * x70 * x91) + (x72 * ((-1 * x69 * x94) + (x58 * x97) + (x58 * (x97 + (x58 * (x96 + (-1 * x67 * x94) + (x58 * ((-1 * x66 * x94) + x95 + (2.40324066e-05 * x58 * x94))))) + (-1 * x68 * x94))) + (-1 * x63 * x94))) + x71 + (-1 * x70 * x54 * x53))) + (-2 * x72 * x75 * x65 * x93) + x90 + (x76 * x53 * x97) + (-1 * x87 * x91)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, curve)/sizeof(FLT), (-1 * x82 * x84) + (-1 * x82)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibmag)/sizeof(FLT), sin(x83)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, gibpha)/sizeof(FLT), x84); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeemag)/sizeof(FLT), (-1 * x84 * x85) + (-1 * x85)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, ogeephase)/sizeof(FLT), (-1 * x88 * x84) + (-1 * x88)); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, phase)/sizeof(FLT), -1); cnMatrixOptionalSet(Hx, 0, offsetof(BaseStationCal, tilt)/sizeof(FLT), (-1 * x84 * x98) + (-1 * x98)); } // Full version Jacobian of SurviveKalmanErrorModel_LightMeas_y_gen2 wrt [curve, gibmag, gibpha, ogeemag, ogeephase, phase, tilt] static inline void SurviveKalmanErrorModel_LightMeas_y_gen2_jac_bsc0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model, const FLT* sensor_pt, const SurvivePose* lh_p, const BaseStationCal* bsc0) { if(hx != 0) { hx->data[0] = SurviveKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt, lh_p, bsc0); } if(Hx != 0) { SurviveKalmanErrorModel_LightMeas_y_gen2_jac_bsc0(Hx, dt, _x0, error_model, sensor_pt, lh_p, bsc0); } } static inline void IMUMeasurementModel(CnMat* out, const SurviveKalmanModel* model) { const FLT x0 = (1. / sqrt(((*model).Pose.Rot[1] * (*model).Pose.Rot[1]) + ((*model).Pose.Rot[0] * (*model).Pose.Rot[0]) + ((*model).Pose.Rot[3] * (*model).Pose.Rot[3]) + ((*model).Pose.Rot[2] * (*model).Pose.Rot[2]))) * (1. / sqrt(((*model).IMUBias.IMUCorrection[1] * (*model).IMUBias.IMUCorrection[1]) + ((*model).IMUBias.IMUCorrection[0] * (*model).IMUBias.IMUCorrection[0]) + ((*model).IMUBias.IMUCorrection[3] * (*model).IMUBias.IMUCorrection[3]) + ((*model).IMUBias.IMUCorrection[2] * (*model).IMUBias.IMUCorrection[2]))); const FLT x1 = x0 * (*model).Pose.Rot[2]; const FLT x2 = x0 * (*model).Pose.Rot[3]; const FLT x3 = x0 * (*model).IMUBias.IMUCorrection[3]; const FLT x4 = x0 * (*model).Pose.Rot[1]; const FLT x5 = (-1 * x4 * (*model).IMUBias.IMUCorrection[2]) + (x3 * (*model).Pose.Rot[0]) + (x1 * (*model).IMUBias.IMUCorrection[1]) + (-1 * x2 * (*model).IMUBias.IMUCorrection[0]); const FLT x6 = x0 * (*model).Pose.Rot[0]; const FLT x7 = (x4 * (*model).IMUBias.IMUCorrection[1]) + (x1 * (*model).IMUBias.IMUCorrection[2]) + (x3 * (*model).Pose.Rot[3]) + (x6 * (*model).IMUBias.IMUCorrection[0]); const FLT x8 = 0.101971621297793 * (*model).Acc[1]; const FLT x9 = 0.101971621297793 * (*model).Acc[0]; const FLT x10 = (-1 * x4 * (*model).IMUBias.IMUCorrection[0]) + (x6 * (*model).IMUBias.IMUCorrection[1]) + (x2 * (*model).IMUBias.IMUCorrection[2]) + (-1 * x3 * (*model).Pose.Rot[2]); const FLT x11 = 1 + (0.101971621297793 * (*model).Acc[2]); const FLT x12 = (-1 * x11 * x10) + (x8 * x7) + (x5 * x9); const FLT x13 = (x3 * (*model).Pose.Rot[1]) + (x6 * (*model).IMUBias.IMUCorrection[2]) + (-1 * x2 * (*model).IMUBias.IMUCorrection[1]) + (-1 * x1 * (*model).IMUBias.IMUCorrection[0]); const FLT x14 = (x7 * x11) + (-1 * x9 * x13) + (x8 * x10); const FLT x15 = (-1 * x5 * x8) + (x7 * x9) + (x13 * x11); const FLT x16 = (-1 * x10 * (*model).Velocity.AxisAngleRot[2]) + (x7 * (*model).Velocity.AxisAngleRot[1]) + (x5 * (*model).Velocity.AxisAngleRot[0]); const FLT x17 = (-1 * x13 * (*model).Velocity.AxisAngleRot[0]) + (x7 * (*model).Velocity.AxisAngleRot[2]) + (x10 * (*model).Velocity.AxisAngleRot[1]); const FLT x18 = (-1 * x5 * (*model).Velocity.AxisAngleRot[1]) + (x7 * (*model).Velocity.AxisAngleRot[0]) + (x13 * (*model).Velocity.AxisAngleRot[2]); cnMatrixOptionalSet(out, 0, 0, ((x9 + (2 * ((x14 * x13) + (-1 * x5 * x12)))) * (*model).IMUBias.AccScale[0]) + (*model).IMUBias.AccBias[0]); cnMatrixOptionalSet(out, 1, 0, ((x8 + (2 * ((x5 * x15) + (-1 * x14 * x10)))) * (*model).IMUBias.AccScale[1]) + (*model).IMUBias.AccBias[1]); cnMatrixOptionalSet(out, 2, 0, ((x11 + (2 * ((x12 * x10) + (-1 * x15 * x13)))) * (*model).IMUBias.AccScale[2]) + (*model).IMUBias.AccBias[2]); cnMatrixOptionalSet(out, 3, 0, (2 * ((x13 * x17) + (-1 * x5 * x16))) + (*model).IMUBias.GyroBias[0] + (*model).Velocity.AxisAngleRot[0]); cnMatrixOptionalSet(out, 4, 0, (2 * ((x5 * x18) + (-1 * x10 * x17))) + (*model).IMUBias.GyroBias[1] + (*model).Velocity.AxisAngleRot[1]); cnMatrixOptionalSet(out, 5, 0, (2 * ((x10 * x16) + (-1 * x13 * x18))) + (*model).IMUBias.GyroBias[2] + (*model).Velocity.AxisAngleRot[2]); } // Jacobian of IMUMeasurementModel wrt [(*model).Acc[0], (*model).Acc[1], (*model).Acc[2], (*model).IMUBias.AccBias[0], (*model).IMUBias.AccBias[1], (*model).IMUBias.AccBias[2], (*model).IMUBias.AccScale[0], (*model).IMUBias.AccScale[1], (*model).IMUBias.AccScale[2], (*model).IMUBias.GyroBias[0], (*model).IMUBias.GyroBias[1], (*model).IMUBias.GyroBias[2], (*model).IMUBias.IMUCorrection[0], (*model).IMUBias.IMUCorrection[1], (*model).IMUBias.IMUCorrection[2], (*model).IMUBias.IMUCorrection[3], (*model).Pose.Pos[0], (*model).Pose.Pos[1], (*model).Pose.Pos[2], (*model).Pose.Rot[0], (*model).Pose.Rot[1], (*model).Pose.Rot[2], (*model).Pose.Rot[3], (*model).Velocity.AxisAngleRot[0], (*model).Velocity.AxisAngleRot[1], (*model).Velocity.AxisAngleRot[2], (*model).Velocity.Pos[0], (*model).Velocity.Pos[1], (*model).Velocity.Pos[2]] static inline void IMUMeasurementModel_jac_model(CnMat* Hx, const SurviveKalmanModel* model) { const FLT x0 = (*model).Pose.Rot[3] * (*model).Pose.Rot[3]; const FLT x1 = (*model).Pose.Rot[2] * (*model).Pose.Rot[2]; const FLT x2 = (*model).Pose.Rot[0] * (*model).Pose.Rot[0]; const FLT x3 = (*model).Pose.Rot[1] * (*model).Pose.Rot[1]; const FLT x4 = x2 + x3 + x0 + x1; const FLT x5 = 1. / sqrt(x4); const FLT x6 = (*model).IMUBias.IMUCorrection[3] * (*model).IMUBias.IMUCorrection[3]; const FLT x7 = (*model).IMUBias.IMUCorrection[2] * (*model).IMUBias.IMUCorrection[2]; const FLT x8 = (*model).IMUBias.IMUCorrection[0] * (*model).IMUBias.IMUCorrection[0]; const FLT x9 = (*model).IMUBias.IMUCorrection[1] * (*model).IMUBias.IMUCorrection[1]; const FLT x10 = x8 + x9 + x6 + x7; const FLT x11 = 1. / sqrt(x10); const FLT x12 = x5 * x11; const FLT x13 = x12 * (*model).Pose.Rot[3]; const FLT x14 = x13 * (*model).IMUBias.IMUCorrection[0]; const FLT x15 = 0.101971621297793 * x14; const FLT x16 = x12 * (*model).Pose.Rot[0]; const FLT x17 = x16 * (*model).IMUBias.IMUCorrection[3]; const FLT x18 = 0.101971621297793 * x17; const FLT x19 = x12 * (*model).Pose.Rot[1]; const FLT x20 = x19 * (*model).IMUBias.IMUCorrection[2]; const FLT x21 = 0.101971621297793 * x20; const FLT x22 = x12 * (*model).Pose.Rot[2]; const FLT x23 = x22 * (*model).IMUBias.IMUCorrection[1]; const FLT x24 = 0.101971621297793 * x23; const FLT x25 = x24 + (-1 * x21) + (-1 * x15) + x18; const FLT x26 = (-1 * x20) + x17 + x23 + (-1 * x14); const FLT x27 = 2 * x26; const FLT x28 = x19 * (*model).IMUBias.IMUCorrection[3]; const FLT x29 = x22 * (*model).IMUBias.IMUCorrection[0]; const FLT x30 = x13 * (*model).IMUBias.IMUCorrection[1]; const FLT x31 = x16 * (*model).IMUBias.IMUCorrection[2]; const FLT x32 = (0.101971621297793 * x30) + (-0.101971621297793 * x28) + (-0.101971621297793 * x31) + (0.101971621297793 * x29); const FLT x33 = x28 + (-1 * x30) + x31 + (-1 * x29); const FLT x34 = 2 * x33; const FLT x35 = x16 * (*model).IMUBias.IMUCorrection[0]; const FLT x36 = x13 * (*model).IMUBias.IMUCorrection[3]; const FLT x37 = x19 * (*model).IMUBias.IMUCorrection[1]; const FLT x38 = x22 * (*model).IMUBias.IMUCorrection[2]; const FLT x39 = (0.101971621297793 * x38) + (0.101971621297793 * x37) + (0.101971621297793 * x35) + (0.101971621297793 * x36); const FLT x40 = x39 * x27; const FLT x41 = x13 * (*model).IMUBias.IMUCorrection[2]; const FLT x42 = x22 * (*model).IMUBias.IMUCorrection[3]; const FLT x43 = x16 * (*model).IMUBias.IMUCorrection[1]; const FLT x44 = x19 * (*model).IMUBias.IMUCorrection[0]; const FLT x45 = (-0.101971621297793 * x44) + (0.101971621297793 * x43) + (0.101971621297793 * x41) + (-0.101971621297793 * x42); const FLT x46 = (-1 * x44) + x43 + x41 + (-1 * x42); const FLT x47 = 0.203943242595586 * x46; const FLT x48 = x37 + x38 + x36 + x35; const FLT x49 = 0.203943242595586 * x33; const FLT x50 = 0.101971621297793 * (*model).Acc[1]; const FLT x51 = 0.101971621297793 * (*model).Acc[0]; const FLT x52 = 1 + (0.101971621297793 * (*model).Acc[2]); const FLT x53 = (-1 * x52 * x46) + (x50 * x48) + (x51 * x26); const FLT x54 = (-1 * x51 * x33) + (x52 * x48) + (x50 * x46); const FLT x55 = x5 * (1. / (x10 * sqrt(x10))); const FLT x56 = x55 * (*model).Pose.Rot[3]; const FLT x57 = x55 * (*model).Pose.Rot[0]; const FLT x58 = x57 * (*model).IMUBias.IMUCorrection[3]; const FLT x59 = -1 * x58 * (*model).IMUBias.IMUCorrection[0]; const FLT x60 = x55 * (*model).Pose.Rot[2]; const FLT x61 = (*model).IMUBias.IMUCorrection[1] * (*model).IMUBias.IMUCorrection[0]; const FLT x62 = x60 * x61; const FLT x63 = x55 * (*model).Pose.Rot[1]; const FLT x64 = x63 * (*model).IMUBias.IMUCorrection[2]; const FLT x65 = x64 * (*model).IMUBias.IMUCorrection[0]; const FLT x66 = -1 * x13; const FLT x67 = x65 + x66 + (-1 * x62) + (x8 * x56) + x59; const FLT x68 = 2 * x53; const FLT x69 = x60 * (*model).IMUBias.IMUCorrection[2]; const FLT x70 = x69 * (*model).IMUBias.IMUCorrection[0]; const FLT x71 = x63 * x61; const FLT x72 = x56 * (*model).IMUBias.IMUCorrection[3]; const FLT x73 = x72 * (*model).IMUBias.IMUCorrection[0]; const FLT x74 = x16 + (-1 * x73) + (-1 * x70) + (-1 * x8 * x57) + (-1 * x71); const FLT x75 = -1 * x19; const FLT x76 = -1 * x61 * x57; const FLT x77 = x56 * (*model).IMUBias.IMUCorrection[2]; const FLT x78 = x77 * (*model).IMUBias.IMUCorrection[0]; const FLT x79 = x60 * (*model).IMUBias.IMUCorrection[3]; const FLT x80 = x79 * (*model).IMUBias.IMUCorrection[0]; const FLT x81 = (-1 * x78) + x80 + x76 + (x8 * x63) + x75; const FLT x82 = (x67 * x51) + (x74 * x50) + (-1 * x81 * x52); const FLT x83 = x63 * (*model).IMUBias.IMUCorrection[3]; const FLT x84 = x83 * (*model).IMUBias.IMUCorrection[0]; const FLT x85 = x61 * x56; const FLT x86 = x57 * (*model).IMUBias.IMUCorrection[2]; const FLT x87 = -1 * x86 * (*model).IMUBias.IMUCorrection[0]; const FLT x88 = -1 * x22; const FLT x89 = x88 + x85 + (-1 * x84) + x87 + (x8 * x60); const FLT x90 = 2 * x54; const FLT x91 = (x81 * x50) + (-1 * x89 * x51) + (x74 * x52); const FLT x92 = x64 * (*model).IMUBias.IMUCorrection[1]; const FLT x93 = -1 * x58 * (*model).IMUBias.IMUCorrection[1]; const FLT x94 = (-1 * x9 * x60) + x22 + x93 + x85 + x92; const FLT x95 = -1 * x86 * (*model).IMUBias.IMUCorrection[1]; const FLT x96 = -1 * x83 * (*model).IMUBias.IMUCorrection[1]; const FLT x97 = x96 + x95 + (x9 * x56) + x66 + x62; const FLT x98 = x77 * (*model).IMUBias.IMUCorrection[1]; const FLT x99 = x79 * (*model).IMUBias.IMUCorrection[1]; const FLT x100 = (-1 * x98) + x16 + x99 + x71 + (-1 * x9 * x57); const FLT x101 = -1 * x69 * (*model).IMUBias.IMUCorrection[1]; const FLT x102 = x72 * (*model).IMUBias.IMUCorrection[1]; const FLT x103 = (-1 * x102) + x76 + x19 + (-1 * x9 * x63) + x101; const FLT x104 = (x51 * x94) + (-1 * x52 * x100) + (x50 * x103); const FLT x105 = (-1 * x51 * x97) + (x50 * x100) + (x52 * x103); const FLT x106 = -1 * x86 * (*model).IMUBias.IMUCorrection[3]; const FLT x107 = x78 + x101 + x106 + (x7 * x63) + x75; const FLT x108 = x83 * (*model).IMUBias.IMUCorrection[2]; const FLT x109 = (-1 * x108) + x70 + x98 + (-1 * x7 * x57) + x16; const FLT x110 = x7 * x55; const FLT x111 = -1 * x72 * (*model).IMUBias.IMUCorrection[2]; const FLT x112 = x87 + x22 + (-1 * x110 * (*model).Pose.Rot[2]) + (-1 * x92) + x111; const FLT x113 = x79 * (*model).IMUBias.IMUCorrection[2]; const FLT x114 = x13 + x65 + x95 + (-1 * x110 * (*model).Pose.Rot[3]) + x113; const FLT x115 = (x50 * x114) + (-1 * x51 * x109) + (x52 * x112); const FLT x116 = (x51 * x107) + (-1 * x52 * x114) + (x50 * x112); const FLT x117 = x73 + (-1 * x6 * x57) + x16 + (-1 * x99) + x108; const FLT x118 = x59 + (-1 * x113) + (-1 * x6 * x56) + x96 + x13; const FLT x119 = x102 + (-1 * x6 * x63) + x80 + x106 + x19; const FLT x120 = x88 + x111 + (x6 * x60) + x93 + x84; const FLT x121 = (x50 * x120) + (x52 * x118) + (-1 * x51 * x119); const FLT x122 = (x51 * x117) + (-1 * x52 * x120) + (x50 * x118); const FLT x123 = (1. / (x4 * sqrt(x4))) * x11; const FLT x124 = x123 * (*model).Pose.Rot[1]; const FLT x125 = (*model).Pose.Rot[0] * (*model).IMUBias.IMUCorrection[2]; const FLT x126 = x124 * x125; const FLT x127 = x123 * (*model).Pose.Rot[2]; const FLT x128 = x127 * (*model).Pose.Rot[0]; const FLT x129 = -1 * x128 * (*model).IMUBias.IMUCorrection[1]; const FLT x130 = x2 * x123; const FLT x131 = x12 * (*model).IMUBias.IMUCorrection[3]; const FLT x132 = x123 * (*model).IMUBias.IMUCorrection[0]; const FLT x133 = x132 * (*model).Pose.Rot[0]; const FLT x134 = x133 * (*model).Pose.Rot[3]; const FLT x135 = x131 + x134 + (-1 * x130 * (*model).IMUBias.IMUCorrection[3]) + x126 + x129; const FLT x136 = x133 * (*model).Pose.Rot[1]; const FLT x137 = x12 * (*model).IMUBias.IMUCorrection[1]; const FLT x138 = x123 * (*model).Pose.Rot[3]; const FLT x139 = -1 * x125 * x138; const FLT x140 = x128 * (*model).IMUBias.IMUCorrection[3]; const FLT x141 = x140 + x139 + x137 + x136 + (-1 * x130 * (*model).IMUBias.IMUCorrection[1]); const FLT x142 = -1 * x127 * x125; const FLT x143 = x124 * (*model).IMUBias.IMUCorrection[1]; const FLT x144 = -1 * x143 * (*model).Pose.Rot[0]; const FLT x145 = x138 * (*model).Pose.Rot[0]; const FLT x146 = -1 * x145 * (*model).IMUBias.IMUCorrection[3]; const FLT x147 = x12 * (*model).IMUBias.IMUCorrection[0]; const FLT x148 = x147 + x146 + x142 + x144 + (-1 * x2 * x132); const FLT x149 = (x51 * x135) + (-1 * x52 * x141) + (x50 * x148); const FLT x150 = x124 * (*model).IMUBias.IMUCorrection[3]; const FLT x151 = -1 * x150 * (*model).Pose.Rot[0]; const FLT x152 = x12 * (*model).IMUBias.IMUCorrection[2]; const FLT x153 = x145 * (*model).IMUBias.IMUCorrection[1]; const FLT x154 = x133 * (*model).Pose.Rot[2]; const FLT x155 = x154 + x153 + x152 + x151 + (-1 * x130 * (*model).IMUBias.IMUCorrection[2]); const FLT x156 = 2 * x155; const FLT x157 = (x50 * x141) + (x52 * x148) + (-1 * x51 * x155); const FLT x158 = x3 * x123; const FLT x159 = -1 * x143 * (*model).Pose.Rot[2]; const FLT x160 = x132 * (*model).Pose.Rot[1]; const FLT x161 = x160 * (*model).Pose.Rot[3]; const FLT x162 = x161 + x151 + x159 + (x158 * (*model).IMUBias.IMUCorrection[2]) + (-1 * x152); const FLT x163 = x143 * (*model).Pose.Rot[3]; const FLT x164 = x160 * (*model).Pose.Rot[2]; const FLT x165 = x164 + (-1 * x126) + x163 + x131 + (-1 * x158 * (*model).IMUBias.IMUCorrection[3]); const FLT x166 = -1 * x147; const FLT x167 = x124 * (*model).IMUBias.IMUCorrection[2]; const FLT x168 = x167 * (*model).Pose.Rot[3]; const FLT x169 = x150 * (*model).Pose.Rot[2]; const FLT x170 = (-1 * x168) + x169 + x144 + (x3 * x132) + x166; const FLT x171 = x167 * (*model).Pose.Rot[2]; const FLT x172 = -1 * x150 * (*model).Pose.Rot[3]; const FLT x173 = (-1 * x136) + x172 + (-1 * x171) + (-1 * x158 * (*model).IMUBias.IMUCorrection[1]) + x137; const FLT x174 = (x51 * x162) + (-1 * x52 * x170) + (x50 * x173); const FLT x175 = (-1 * x51 * x165) + (x50 * x170) + (x52 * x173); const FLT x176 = x1 * x123; const FLT x177 = x127 * (*model).Pose.Rot[3]; const FLT x178 = x177 * (*model).IMUBias.IMUCorrection[3]; const FLT x179 = (-1 * x154) + (-1 * x178) + x159 + (-1 * x176 * (*model).IMUBias.IMUCorrection[2]) + x152; const FLT x180 = x177 * (*model).IMUBias.IMUCorrection[1]; const FLT x181 = x180 + x166 + x142 + (x1 * x132) + (-1 * x169); const FLT x182 = -1 * x177 * (*model).IMUBias.IMUCorrection[2]; const FLT x183 = (-1 * x131) + x164 + (x176 * (*model).IMUBias.IMUCorrection[3]) + x182 + x129; const FLT x184 = (x50 * x183) + (x52 * x179) + (-1 * x51 * x181); const FLT x185 = x132 * (*model).Pose.Rot[3] * (*model).Pose.Rot[2]; const FLT x186 = (-1 * x176 * (*model).IMUBias.IMUCorrection[1]) + x137 + x185 + x171 + (-1 * x140); const FLT x187 = (x51 * x186) + (-1 * x52 * x183) + (x50 * x179); const FLT x188 = (-1 * x180) + (x0 * x132) + x168 + x166 + x146; const FLT x189 = x0 * x123; const FLT x190 = (x189 * (*model).IMUBias.IMUCorrection[1]) + (-1 * x137) + x185 + x172 + x139; const FLT x191 = (-1 * x189 * (*model).IMUBias.IMUCorrection[3]) + (-1 * x134) + x182 + x131 + (-1 * x163); const FLT x192 = (-1 * x189 * (*model).IMUBias.IMUCorrection[2]) + x152 + x161 + x178 + (-1 * x153); const FLT x193 = (x50 * x192) + (-1 * x51 * x190) + (x52 * x191); const FLT x194 = (x51 * x188) + (-1 * x52 * x192) + (x50 * x191); const FLT x195 = 2 * x46; const FLT x196 = (-1 * x24) + x21 + x15 + (-1 * x18); const FLT x197 = (-1 * x50 * x26) + (x51 * x48) + (x52 * x33); const FLT x198 = 2 * x197; const FLT x199 = (x89 * x52) + (-1 * x67 * x50) + (x74 * x51); const FLT x200 = (x52 * x97) + (x51 * x103) + (-1 * x50 * x94); const FLT x201 = (x52 * x109) + (-1 * x50 * x107) + (x51 * x112); const FLT x202 = (x52 * x119) + (-1 * x50 * x117) + (x51 * x118); const FLT x203 = (x52 * x155) + (-1 * x50 * x135) + (x51 * x148); const FLT x204 = (x52 * x165) + (-1 * x50 * x162) + (x51 * x173); const FLT x205 = (x52 * x181) + (-1 * x50 * x186) + (x51 * x179); const FLT x206 = (x52 * x190) + (-1 * x50 * x188) + (x51 * x191); const FLT x207 = x33 * x33; const FLT x208 = x46 * x46; const FLT x209 = 2 * ((-1 * x46 * (*model).Velocity.AxisAngleRot[2]) + (x48 * (*model).Velocity.AxisAngleRot[1]) + (x26 * (*model).Velocity.AxisAngleRot[0])); const FLT x210 = (x81 * (*model).Velocity.AxisAngleRot[1]) + (x74 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x89 * (*model).Velocity.AxisAngleRot[0]); const FLT x211 = (x67 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x81 * (*model).Velocity.AxisAngleRot[2]) + (x74 * (*model).Velocity.AxisAngleRot[1]); const FLT x212 = (-1 * x33 * (*model).Velocity.AxisAngleRot[0]) + (x48 * (*model).Velocity.AxisAngleRot[2]) + (x46 * (*model).Velocity.AxisAngleRot[1]); const FLT x213 = 2 * x212; const FLT x214 = (-1 * x97 * (*model).Velocity.AxisAngleRot[0]) + (x100 * (*model).Velocity.AxisAngleRot[1]) + (x103 * (*model).Velocity.AxisAngleRot[2]); const FLT x215 = (x94 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x100 * (*model).Velocity.AxisAngleRot[2]) + (x103 * (*model).Velocity.AxisAngleRot[1]); const FLT x216 = (-1 * x109 * (*model).Velocity.AxisAngleRot[0]) + (x114 * (*model).Velocity.AxisAngleRot[1]) + (x112 * (*model).Velocity.AxisAngleRot[2]); const FLT x217 = (x107 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x114 * (*model).Velocity.AxisAngleRot[2]) + (x112 * (*model).Velocity.AxisAngleRot[1]); const FLT x218 = (-1 * x120 * (*model).Velocity.AxisAngleRot[2]) + (x117 * (*model).Velocity.AxisAngleRot[0]) + (x118 * (*model).Velocity.AxisAngleRot[1]); const FLT x219 = (-1 * x119 * (*model).Velocity.AxisAngleRot[0]) + (x120 * (*model).Velocity.AxisAngleRot[1]) + (x118 * (*model).Velocity.AxisAngleRot[2]); const FLT x220 = (x135 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x141 * (*model).Velocity.AxisAngleRot[2]) + (x148 * (*model).Velocity.AxisAngleRot[1]); const FLT x221 = (x141 * (*model).Velocity.AxisAngleRot[1]) + (x148 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x155 * (*model).Velocity.AxisAngleRot[0]); const FLT x222 = (x162 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x170 * (*model).Velocity.AxisAngleRot[2]) + (x173 * (*model).Velocity.AxisAngleRot[1]); const FLT x223 = (x170 * (*model).Velocity.AxisAngleRot[1]) + (-1 * x165 * (*model).Velocity.AxisAngleRot[0]) + (x173 * (*model).Velocity.AxisAngleRot[2]); const FLT x224 = (-1 * x181 * (*model).Velocity.AxisAngleRot[0]) + (x183 * (*model).Velocity.AxisAngleRot[1]) + (x179 * (*model).Velocity.AxisAngleRot[2]); const FLT x225 = (x186 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x183 * (*model).Velocity.AxisAngleRot[2]) + (x179 * (*model).Velocity.AxisAngleRot[1]); const FLT x226 = (x188 * (*model).Velocity.AxisAngleRot[0]) + (x191 * (*model).Velocity.AxisAngleRot[1]) + (-1 * x192 * (*model).Velocity.AxisAngleRot[2]); const FLT x227 = (-1 * x190 * (*model).Velocity.AxisAngleRot[0]) + (x192 * (*model).Velocity.AxisAngleRot[1]) + (x191 * (*model).Velocity.AxisAngleRot[2]); const FLT x228 = x30 + (-1 * x28) + (-1 * x31) + x29; const FLT x229 = x48 * x27; const FLT x230 = x44 + (-1 * x43) + (-1 * x41) + x42; const FLT x231 = x48 * x34; const FLT x232 = (x89 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x67 * (*model).Velocity.AxisAngleRot[1]) + (x74 * (*model).Velocity.AxisAngleRot[0]); const FLT x233 = 2 * ((-1 * x26 * (*model).Velocity.AxisAngleRot[1]) + (x48 * (*model).Velocity.AxisAngleRot[0]) + (x33 * (*model).Velocity.AxisAngleRot[2])); const FLT x234 = (x97 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x94 * (*model).Velocity.AxisAngleRot[1]) + (x103 * (*model).Velocity.AxisAngleRot[0]); const FLT x235 = (x109 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x107 * (*model).Velocity.AxisAngleRot[1]) + (x112 * (*model).Velocity.AxisAngleRot[0]); const FLT x236 = (x119 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x117 * (*model).Velocity.AxisAngleRot[1]) + (x118 * (*model).Velocity.AxisAngleRot[0]); const FLT x237 = (x155 * (*model).Velocity.AxisAngleRot[2]) + (x148 * (*model).Velocity.AxisAngleRot[0]) + (-1 * x135 * (*model).Velocity.AxisAngleRot[1]); const FLT x238 = (x165 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x162 * (*model).Velocity.AxisAngleRot[1]) + (x173 * (*model).Velocity.AxisAngleRot[0]); const FLT x239 = (x181 * (*model).Velocity.AxisAngleRot[2]) + (-1 * x186 * (*model).Velocity.AxisAngleRot[1]) + (x179 * (*model).Velocity.AxisAngleRot[0]); const FLT x240 = (-1 * x188 * (*model).Velocity.AxisAngleRot[1]) + (x190 * (*model).Velocity.AxisAngleRot[2]) + (x191 * (*model).Velocity.AxisAngleRot[0]); const FLT x241 = x20 + x14 + (-1 * x23) + (-1 * x17); const FLT x242 = x48 * x195; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), (0.101971621297793 + (x32 * x34) + (-1 * x25 * x27)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), ((x45 * x34) + (-1 * x40)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), ((x48 * x49) + (x47 * x26)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), x51 + (2 * ((x54 * x33) + (-1 * x53 * x26)))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), ((x91 * x34) + (x89 * x90) + (-1 * x67 * x68) + (-1 * x82 * x27)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), ((x34 * x105) + (-1 * x68 * x94) + (-1 * x27 * x104) + (x90 * x97)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), ((-1 * x27 * x116) + (x90 * x109) + (-1 * x68 * x107) + (x34 * x115)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), ((x90 * x119) + (-1 * x68 * x117) + (-1 * x27 * x122) + (x34 * x121)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), ((x34 * x157) + (x54 * x156) + (-1 * x68 * x135) + (-1 * x27 * x149)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), ((x34 * x175) + (-1 * x68 * x162) + (-1 * x27 * x174) + (x90 * x165)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), ((x90 * x181) + (-1 * x27 * x187) + (x34 * x184) + (-1 * x68 * x186)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), ((x90 * x190) + (-1 * x68 * x188) + (-1 * x27 * x194) + (x34 * x193)) * (*model).IMUBias.AccScale[0]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), (x40 + (-1 * x32 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), (0.101971621297793 + (x27 * x196) + (-1 * x45 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), ((x49 * x26) + (-1 * x47 * x48)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), x50 + (2 * ((x26 * x197) + (-1 * x54 * x46)))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), ((x27 * x199) + (x67 * x198) + (-1 * x81 * x90) + (-1 * x91 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (*model).IMUBias.AccScale[1] * ((x27 * x200) + (x94 * x198) + (-1 * x90 * x100) + (-1 * x105 * x195))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), ((-1 * x115 * x195) + (x27 * x201) + (-1 * x90 * x114) + (x107 * x198)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), ((-1 * x121 * x195) + (x117 * x198) + (-1 * x90 * x120) + (x27 * x202)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), ((x198 * x135) + (-1 * x90 * x141) + (x27 * x203) + (-1 * x195 * x157)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), ((x27 * x204) + (x162 * x198) + (-1 * x90 * x170) + (-1 * x175 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), ((x27 * x205) + (x186 * x198) + (-1 * x90 * x183) + (-1 * x184 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), ((x27 * x206) + (x188 * x198) + (-1 * x90 * x192) + (-1 * x193 * x195)) * (*model).IMUBias.AccScale[1]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), ((x25 * x195) + (-1 * x34 * x39)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), ((x39 * x195) + (-1 * x34 * x196)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), (0.101971621297793 + (-0.203943242595586 * x208) + (-0.203943242595586 * x207)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), x52 + (2 * ((x53 * x46) + (-1 * x33 * x197)))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), ((x81 * x68) + (-1 * x34 * x199) + (-1 * x89 * x198) + (x82 * x195)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (*model).IMUBias.AccScale[2] * ((x104 * x195) + (-1 * x34 * x200) + (x68 * x100) + (-1 * x97 * x198))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), ((x116 * x195) + (-1 * x34 * x201) + (-1 * x109 * x198) + (x68 * x114)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), ((-1 * x34 * x202) + (-1 * x119 * x198) + (x68 * x120) + (x122 * x195)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), ((-1 * x34 * x203) + (x68 * x141) + (-1 * x197 * x156) + (x195 * x149)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), ((x68 * x170) + (-1 * x165 * x198) + (x174 * x195) + (-1 * x34 * x204)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), ((x187 * x195) + (x68 * x183) + (-1 * x181 * x198) + (-1 * x34 * x205)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), ((x194 * x195) + (-1 * x190 * x198) + (-1 * x34 * x206) + (x68 * x192)) * (*model).IMUBias.AccScale[2]); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (x89 * x213) + (-1 * x27 * x211) + (-1 * x67 * x209) + (x34 * x210)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x27 * x215) + (x34 * x214) + (-1 * x94 * x209) + (x97 * x213)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x213 * x109) + (-1 * x209 * x107) + (-1 * x27 * x217) + (x34 * x216)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (x213 * x119) + (x34 * x219) + (-1 * x209 * x117) + (-1 * x27 * x218)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (x34 * x221) + (-1 * x27 * x220) + (-1 * x209 * x135) + (x212 * x156)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x213 * x165) + (x34 * x223) + (-1 * x209 * x162) + (-1 * x27 * x222)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x213 * x181) + (-1 * x27 * x225) + (-1 * x209 * x186) + (x34 * x224)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x209 * x188) + (x34 * x227) + (x213 * x190) + (-1 * x27 * x226)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1 + (x34 * x228) + (-2 * (x26 * x26))); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (x46 * x34) + (-1 * x229)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x231 + (-1 * x27 * x230)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (x67 * x233) + (-1 * x81 * x213) + (-1 * x210 * x195) + (x27 * x232)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x27 * x234) + (-1 * x214 * x195) + (x94 * x233) + (-1 * x213 * x100)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x27 * x235) + (x233 * x107) + (-1 * x213 * x114) + (-1 * x216 * x195)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (x233 * x117) + (-1 * x219 * x195) + (-1 * x213 * x120) + (x27 * x236)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (x27 * x237) + (x233 * x135) + (-1 * x213 * x141) + (-1 * x221 * x195)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x27 * x238) + (-1 * x213 * x170) + (x233 * x162) + (-1 * x223 * x195)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x233 * x186) + (-1 * x213 * x183) + (-1 * x224 * x195) + (x27 * x239)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x27 * x240) + (x233 * x188) + (-1 * x213 * x192) + (-1 * x227 * x195)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x229 + (-1 * x228 * x195)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1 + (x27 * x241) + (-2 * x208)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (x34 * x26) + (-1 * x242)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (x81 * x209) + (-1 * x34 * x232) + (-1 * x89 * x233) + (x211 * x195)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x215 * x195) + (x209 * x100) + (-1 * x97 * x233) + (-1 * x34 * x234)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x34 * x235) + (-1 * x233 * x109) + (x209 * x114) + (x217 * x195)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (x209 * x120) + (x218 * x195) + (-1 * x233 * x119) + (-1 * x34 * x236)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x34 * x237) + (x209 * x141) + (-1 * x233 * x155) + (x220 * x195)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x222 * x195) + (x209 * x170) + (-1 * x233 * x165) + (-1 * x34 * x238)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x225 * x195) + (x209 * x183) + (-1 * x233 * x181) + (-1 * x34 * x239)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x226 * x195) + (x209 * x192) + (-1 * x233 * x190) + (-1 * x34 * x240)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (x46 * x27) + (-1 * x231)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x242 + (-1 * x34 * x241)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1 + (x230 * x195) + (-2 * x207)); } // Full version Jacobian of IMUMeasurementModel wrt [(*model).Acc[0], (*model).Acc[1], (*model).Acc[2], (*model).IMUBias.AccBias[0], (*model).IMUBias.AccBias[1], (*model).IMUBias.AccBias[2], (*model).IMUBias.AccScale[0], (*model).IMUBias.AccScale[1], (*model).IMUBias.AccScale[2], (*model).IMUBias.GyroBias[0], (*model).IMUBias.GyroBias[1], (*model).IMUBias.GyroBias[2], (*model).IMUBias.IMUCorrection[0], (*model).IMUBias.IMUCorrection[1], (*model).IMUBias.IMUCorrection[2], (*model).IMUBias.IMUCorrection[3], (*model).Pose.Pos[0], (*model).Pose.Pos[1], (*model).Pose.Pos[2], (*model).Pose.Rot[0], (*model).Pose.Rot[1], (*model).Pose.Rot[2], (*model).Pose.Rot[3], (*model).Velocity.AxisAngleRot[0], (*model).Velocity.AxisAngleRot[1], (*model).Velocity.AxisAngleRot[2], (*model).Velocity.Pos[0], (*model).Velocity.Pos[1], (*model).Velocity.Pos[2]] static inline void IMUMeasurementModel_jac_model_with_hx(CnMat* Hx, CnMat* hx, const SurviveKalmanModel* model) { if(hx != 0) { IMUMeasurementModel(hx, model); } if(Hx != 0) { IMUMeasurementModel_jac_model(Hx, model); } } static inline void IMUMeasurementErrorModel(CnMat* out, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x1 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x2 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x3 = (-1 * x2 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[0] + (-1 * x0 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x1 * (*error_model).IMUBias.IMUCorrection[1]); const FLT x4 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x6 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x7 = (x6 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x4 * (*_x0).Pose.Rot[2]) + (x5 * (*_x0).Pose.Rot[0]); const FLT x8 = x7 * x7; const FLT x9 = (-1 * x5 * (*_x0).Pose.Rot[1]) + (x6 * (*_x0).Pose.Rot[0]) + (x4 * (*_x0).Pose.Rot[3]) + (*_x0).Pose.Rot[2]; const FLT x10 = x9 * x9; const FLT x11 = (-1 * x4 * (*_x0).Pose.Rot[1]) + (-1 * x6 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x5 * (*_x0).Pose.Rot[3]); const FLT x12 = x11 * x11; const FLT x13 = (*_x0).Pose.Rot[1] + (-1 * x6 * (*_x0).Pose.Rot[3]) + (x4 * (*_x0).Pose.Rot[0]) + (x5 * (*_x0).Pose.Rot[2]); const FLT x14 = x13 * x13; const FLT x15 = x14 + x12 + x8 + x10; const FLT x16 = 1. / x15; const FLT x17 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x18 = (-1 * x2 * (*error_model).IMUBias.IMUCorrection[2]) + (x17 * (*error_model).IMUBias.IMUCorrection[1]) + (x0 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x19 = x18 * x18; const FLT x20 = (x17 * (*error_model).IMUBias.IMUCorrection[2]) + (*_x0).IMUBias.IMUCorrection[3] + (x2 * (*error_model).IMUBias.IMUCorrection[1]) + (-1 * x1 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x21 = x20 * x20; const FLT x22 = x3 * x3; const FLT x23 = (*_x0).IMUBias.IMUCorrection[1] + (x1 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x0 * (*error_model).IMUBias.IMUCorrection[1]) + (x17 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x24 = x23 * x23; const FLT x25 = x22 + x19 + x24 + x21; const FLT x26 = 1. / x25; const FLT x27 = (1. / sqrt((x24 * x26) + (x22 * x26) + (x21 * x26) + (x26 * x19))) * (1. / sqrt(x25)) * (1. / sqrt(x15)) * (1. / sqrt((x14 * x16) + (x10 * x16) + (x8 * x16) + (x12 * x16))); const FLT x28 = x7 * x27; const FLT x29 = x23 * x27; const FLT x30 = x27 * x11; const FLT x31 = x27 * x13; const FLT x32 = (-1 * x3 * x28) + (-1 * x31 * x18) + (x30 * x20) + (x9 * x29); const FLT x33 = x9 * x27; const FLT x34 = (x29 * x13) + (x20 * x28) + (x3 * x30) + (x33 * x18); const FLT x35 = 0.101971621297793 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x36 = 0.101971621297793 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x37 = (-1 * x3 * x31) + (x28 * x18) + (-1 * x33 * x20) + (x29 * x11); const FLT x38 = 1 + (0.101971621297793 * ((*_x0).Acc[2] + (*error_model).Acc[2])); const FLT x39 = (-1 * x38 * x37) + (x34 * x35) + (x32 * x36); const FLT x40 = (x31 * x20) + (x30 * x18) + (-1 * x7 * x29) + (-1 * x3 * x33); const FLT x41 = (-1 * x40 * x36) + (x35 * x37) + (x34 * x38); const FLT x42 = (-1 * x32 * x35) + (x40 * x38) + (x34 * x36); const FLT x43 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x44 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x45 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x46 = (x43 * x34) + (-1 * x45 * x37) + (x44 * x32); const FLT x47 = (-1 * x40 * x44) + (x45 * x34) + (x43 * x37); const FLT x48 = (-1 * x43 * x32) + (x44 * x34) + (x40 * x45); cnMatrixOptionalSet(out, 0, 0, (*error_model).IMUBias.AccBias[0] + (*_x0).IMUBias.AccBias[0] + ((x36 + (2 * ((x40 * x41) + (-1 * x32 * x39)))) * ((*_x0).IMUBias.AccScale[0] + (*error_model).IMUBias.AccScale[0]))); cnMatrixOptionalSet(out, 1, 0, (*error_model).IMUBias.AccBias[1] + ((x35 + (2 * ((x42 * x32) + (-1 * x41 * x37)))) * ((*_x0).IMUBias.AccScale[1] + (*error_model).IMUBias.AccScale[1])) + (*_x0).IMUBias.AccBias[1]); cnMatrixOptionalSet(out, 2, 0, (*error_model).IMUBias.AccBias[2] + (*_x0).IMUBias.AccBias[2] + ((x38 + (2 * ((x37 * x39) + (-1 * x40 * x42)))) * ((*_x0).IMUBias.AccScale[2] + (*error_model).IMUBias.AccScale[2]))); cnMatrixOptionalSet(out, 3, 0, x44 + (*_x0).IMUBias.GyroBias[0] + (2 * ((x40 * x47) + (-1 * x46 * x32))) + (*error_model).IMUBias.GyroBias[0]); cnMatrixOptionalSet(out, 4, 0, x43 + (*error_model).IMUBias.GyroBias[1] + (*_x0).IMUBias.GyroBias[1] + (2 * ((x48 * x32) + (-1 * x47 * x37)))); cnMatrixOptionalSet(out, 5, 0, x45 + (2 * ((x46 * x37) + (-1 * x40 * x48))) + (*_x0).IMUBias.GyroBias[2] + (*error_model).IMUBias.GyroBias[2]); } // Jacobian of IMUMeasurementErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void IMUMeasurementErrorModel_jac_x0(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = (*_x0).IMUBias.AccScale[0] + (*error_model).IMUBias.AccScale[0]; const FLT x1 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x2 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x3 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x4 = (x3 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x1 * (*error_model).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[1] + (x2 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x6 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x8 = (x6 * (*error_model).Pose.AxisAngleRot[1]) + (x5 * (*_x0).Pose.Rot[3]) + (-1 * x7 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[2]; const FLT x9 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x10 = (-1 * x9 * (*error_model).IMUBias.IMUCorrection[2]) + (x2 * (*error_model).IMUBias.IMUCorrection[1]) + (x1 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x11 = x10 * x10; const FLT x12 = (x9 * (*error_model).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[3] + (x2 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x3 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x13 = x12 * x12; const FLT x14 = (-1 * x9 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[0] + (-1 * x1 * (*error_model).IMUBias.IMUCorrection[2]) + (-1 * x3 * (*error_model).IMUBias.IMUCorrection[1]); const FLT x15 = x14 * x14; const FLT x16 = x4 * x4; const FLT x17 = x16 + x15 + x11 + x13; const FLT x18 = 1. / x17; const FLT x19 = (x18 * x16) + (x15 * x18) + (x13 * x18) + (x11 * x18); const FLT x20 = 1. / sqrt(x19); const FLT x21 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x22 = (*_x0).Pose.Rot[3] + (x21 * (*_x0).Pose.Rot[1]) + (-1 * x5 * (*_x0).Pose.Rot[2]) + (x6 * (*error_model).Pose.AxisAngleRot[2]); const FLT x23 = x22 * x22; const FLT x24 = x8 * x8; const FLT x25 = (-1 * x21 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x5 * (*_x0).Pose.Rot[1]) + (-1 * x7 * (*_x0).Pose.Rot[3]); const FLT x26 = x25 * x25; const FLT x27 = (*_x0).Pose.Rot[1] + (x6 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x21 * (*_x0).Pose.Rot[3]) + (x7 * (*_x0).Pose.Rot[2]); const FLT x28 = x27 * x27; const FLT x29 = x28 + x26 + x23 + x24; const FLT x30 = 1. / x29; const FLT x31 = (x30 * x28) + (x30 * x24) + (x30 * x23) + (x30 * x26); const FLT x32 = 1. / sqrt(x31); const FLT x33 = 1. / sqrt(x29); const FLT x34 = 1. / sqrt(x17); const FLT x35 = x32 * x34 * x33; const FLT x36 = x35 * x20; const FLT x37 = x8 * x36; const FLT x38 = x4 * x37; const FLT x39 = 0.101971621297793 * x38; const FLT x40 = x36 * x22; const FLT x41 = x40 * x14; const FLT x42 = 0.101971621297793 * x41; const FLT x43 = x36 * x27; const FLT x44 = x43 * x10; const FLT x45 = 0.101971621297793 * x44; const FLT x46 = x36 * x25; const FLT x47 = x46 * x12; const FLT x48 = 0.101971621297793 * x47; const FLT x49 = x48 + (-1 * x45) + x39 + (-1 * x42); const FLT x50 = (-1 * x44) + x47 + (-1 * x41) + x38; const FLT x51 = 2 * x50; const FLT x52 = x37 * x14; const FLT x53 = x4 * x40; const FLT x54 = x43 * x12; const FLT x55 = x46 * x10; const FLT x56 = (-0.101971621297793 * x55) + (-0.101971621297793 * x54) + (0.101971621297793 * x52) + (0.101971621297793 * x53); const FLT x57 = x54 + x55 + (-1 * x53) + (-1 * x52); const FLT x58 = 2 * x57; const FLT x59 = x37 * x10; const FLT x60 = x46 * x14; const FLT x61 = x4 * x43; const FLT x62 = x40 * x12; const FLT x63 = (0.101971621297793 * x62) + (0.101971621297793 * x59) + (0.101971621297793 * x61) + (0.101971621297793 * x60); const FLT x64 = x63 * x51; const FLT x65 = x4 * x46; const FLT x66 = x37 * x12; const FLT x67 = x43 * x14; const FLT x68 = x40 * x10; const FLT x69 = (0.101971621297793 * x68) + (-0.101971621297793 * x67) + (0.101971621297793 * x65) + (-0.101971621297793 * x66); const FLT x70 = (-1 * x67) + x68 + (-1 * x66) + x65; const FLT x71 = 0.203943242595586 * x70; const FLT x72 = x61 + x60 + x62 + x59; const FLT x73 = 0.203943242595586 * x57; const FLT x74 = 0.101971621297793 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x75 = 0.101971621297793 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x76 = 1 + (0.101971621297793 * ((*_x0).Acc[2] + (*error_model).Acc[2])); const FLT x77 = (-1 * x70 * x76) + (x72 * x74) + (x75 * x50); const FLT x78 = (-1 * x75 * x57) + (x70 * x74) + (x72 * x76); const FLT x79 = 1.0/2.0 * x10; const FLT x80 = x79 * x27; const FLT x81 = x32 * x20; const FLT x82 = x80 * x81; const FLT x83 = 1.0 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x84 = x4 * x83; const FLT x85 = 2 * x14; const FLT x86 = 1.0 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x87 = x86 * x10; const FLT x88 = 1.0 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x89 = x88 * x12; const FLT x90 = x89 + x87 + x84 + x85; const FLT x91 = x33 * (1. / (x17 * sqrt(x17))); const FLT x92 = x91 * x90; const FLT x93 = 1.0/2.0 * x12; const FLT x94 = x93 * x25; const FLT x95 = x81 * x92; const FLT x96 = 1.0/2.0 * x14; const FLT x97 = x96 * x22; const FLT x98 = x81 * x97; const FLT x99 = -1 * x40; const FLT x100 = x22 * x14; const FLT x101 = 1. / (x17 * x17); const FLT x102 = x90 * x101; const FLT x103 = (-1 * x11 * x102) + (x87 * x18) + (x85 * x18) + (-1 * x15 * x102) + (-1 * x16 * x102) + (-1 * x13 * x102) + (x89 * x18) + (x84 * x18); const FLT x104 = x35 * (1. / (x19 * sqrt(x19))); const FLT x105 = 1.0/2.0 * x104; const FLT x106 = x103 * x105; const FLT x107 = x4 * x8; const FLT x108 = 1.0/2.0 * x4; const FLT x109 = x8 * x108; const FLT x110 = x81 * x109; const FLT x111 = 0.5 * x46; const FLT x112 = x111 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x113 = x103 * x104; const FLT x114 = 0.5 * x37; const FLT x115 = x114 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x116 = 0.5 * x43; const FLT x117 = x116 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x118 = (-1 * x117) + x115; const FLT x119 = (x80 * x113) + x118 + x112 + (-1 * x92 * x110) + (-1 * x94 * x113) + (-1 * x107 * x106) + (-1 * x95 * x94) + (x82 * x92) + (x92 * x98) + x99 + (x100 * x106); const FLT x120 = 2 * x77; const FLT x121 = x93 * x27; const FLT x122 = 0.5 * x40; const FLT x123 = x122 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x124 = -1 * x123; const FLT x125 = x4 * x22; const FLT x126 = 1.0/2.0 * x125; const FLT x127 = x81 * x126; const FLT x128 = x8 * x14; const FLT x129 = x111 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x130 = x79 * x25; const FLT x131 = 1.0/2.0 * x128; const FLT x132 = x116 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x133 = x132 + (-1 * x37); const FLT x134 = (x95 * x131) + x133 + (-1 * x95 * x130) + x129 + (x106 * x125) + x124 + (-1 * x95 * x121) + (-1 * x113 * x130) + (-1 * x113 * x121) + (x92 * x127) + (x106 * x128); const FLT x135 = 2 * x78; const FLT x136 = x114 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x137 = -1 * x136; const FLT x138 = x8 * x93; const FLT x139 = x81 * x138; const FLT x140 = x79 * x22; const FLT x141 = x81 * x140; const FLT x142 = x4 * x106; const FLT x143 = x25 * x108; const FLT x144 = x96 * x27; const FLT x145 = x27 * x14; const FLT x146 = -1 * x43; const FLT x147 = x122 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x148 = x111 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x149 = x148 + x147; const FLT x150 = x146 + (-1 * x95 * x143) + x149 + (x92 * x139) + (-1 * x113 * x140) + (x106 * x145) + x137 + (-1 * x92 * x141) + (x113 * x138) + (x95 * x144) + (-1 * x25 * x142); const FLT x151 = x122 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x152 = x116 * (*error_model).IMUBias.IMUCorrection[0]; const FLT x153 = x27 * x108; const FLT x154 = x8 * x79; const FLT x155 = x93 * x22; const FLT x156 = x25 * x14; const FLT x157 = x81 * x155; const FLT x158 = x96 * x25; const FLT x159 = x114 * (*error_model).IMUBias.IMUCorrection[1]; const FLT x160 = x159 + x46; const FLT x161 = x160 + (-1 * x27 * x142) + (-1 * x95 * x154) + (-1 * x106 * x156) + x152 + (-1 * x95 * x158) + x151 + (-1 * x95 * x153) + (-1 * x113 * x154) + (-1 * x92 * x157) + (-1 * x113 * x155); const FLT x162 = (x75 * x119) + (-1 * x76 * x150) + (x74 * x161); const FLT x163 = (x76 * x161) + (-1 * x75 * x134) + (x74 * x150); const FLT x164 = x83 * x14; const FLT x165 = x88 * x10; const FLT x166 = 2 * x4; const FLT x167 = x86 * x12; const FLT x168 = x167 + x166 + (-1 * x164) + (-1 * x165); const FLT x169 = x91 * x168; const FLT x170 = x81 * x169; const FLT x171 = x25 * x170; const FLT x172 = x101 * x168; const FLT x173 = (-1 * x15 * x172) + (-1 * x18 * x165) + (-1 * x11 * x172) + (-1 * x13 * x172) + (-1 * x16 * x172) + (x18 * x167) + (-1 * x18 * x164) + (x18 * x166); const FLT x174 = x104 * x173; const FLT x175 = x105 * x173; const FLT x176 = x4 * x175; const FLT x177 = x37 + x132; const FLT x178 = x177 + (x100 * x175) + (-1 * x8 * x176) + x123 + (x80 * x174) + (-1 * x94 * x174) + x129 + (x82 * x169) + (-1 * x110 * x169) + (x97 * x170) + (-1 * x93 * x171); const FLT x179 = -1 * x112; const FLT x180 = x117 + x115; const FLT x181 = x180 + (-1 * x174 * x130) + (-1 * x170 * x130) + (-1 * x121 * x170) + x179 + (-1 * x121 * x174) + (x170 * x131) + (x125 * x175) + (x126 * x170) + x99 + (x128 * x175); const FLT x182 = x96 * x170; const FLT x183 = -1 * x151; const FLT x184 = (-1 * x159) + x46; const FLT x185 = x184 + (x175 * x145) + (-1 * x169 * x141) + (-1 * x174 * x140) + x183 + (x27 * x182) + (x174 * x138) + (-1 * x108 * x171) + x152 + (-1 * x25 * x176) + (x169 * x139); const FLT x186 = (-1 * x148) + x147; const FLT x187 = (-1 * x27 * x176) + (-1 * x170 * x153) + x137 + x43 + (-1 * x170 * x154) + (-1 * x174 * x154) + x186 + (-1 * x25 * x182) + (-1 * x169 * x157) + (-1 * x175 * x156) + (-1 * x174 * x155); const FLT x188 = (-1 * x76 * x185) + (x75 * x178) + (x74 * x187); const FLT x189 = (x76 * x187) + (-1 * x75 * x181) + (x74 * x185); const FLT x190 = x86 * x14; const FLT x191 = x4 * x88; const FLT x192 = 2 * x10; const FLT x193 = x83 * x12; const FLT x194 = (-1 * x193) + x192 + (-1 * x190) + x191; const FLT x195 = x91 * x194; const FLT x196 = x101 * x194; const FLT x197 = ((-1 * x18 * x190) + (x18 * x192) + (-1 * x11 * x196) + (-1 * x18 * x193) + (-1 * x15 * x196) + (-1 * x16 * x196) + (-1 * x13 * x196) + (x18 * x191)) * x104; const FLT x198 = x22 * x197; const FLT x199 = 1.0/2.0 * x198; const FLT x200 = x81 * x195; const FLT x201 = x22 * x200; const FLT x202 = x25 * x197; const FLT x203 = x25 * x200; const FLT x204 = (-1 * x93 * x203) + (x80 * x197) + (-1 * x93 * x202) + (x96 * x201) + (-1 * x200 * x109) + x186 + x146 + x136 + (x14 * x199) + (-1 * x109 * x197) + (x82 * x195); const FLT x205 = -1 * x129; const FLT x206 = x27 * x197; const FLT x207 = x27 * x200; const FLT x208 = x177 + (-1 * x96 * x203) + (-1 * x96 * x202) + (-1 * x207 * x108) + (-1 * x200 * x154) + (-1 * x206 * x108) + (-1 * x197 * x155) + x124 + x205 + (-1 * x200 * x155) + (-1 * x197 * x154); const FLT x209 = (-1 * x202 * x108) + (-1 * x203 * x108) + (x96 * x206) + (-1 * x79 * x201) + (x197 * x138) + x112 + x180 + (x96 * x207) + (-1 * x79 * x198) + x40 + (x195 * x139); const FLT x210 = (x75 * x204) + (x74 * x208) + (-1 * x76 * x209); const FLT x211 = -1 * x152; const FLT x212 = x160 + (-1 * x197 * x130) + (x200 * x131) + (-1 * x93 * x206) + (x200 * x126) + x211 + (-1 * x93 * x207) + (-1 * x200 * x130) + x183 + (x197 * x131) + (x4 * x199); const FLT x213 = (x76 * x208) + (x74 * x209) + (-1 * x75 * x212); const FLT x214 = x4 * x86; const FLT x215 = x88 * x14; const FLT x216 = x83 * x10; const FLT x217 = 2 * x12; const FLT x218 = x217 + x216 + (-1 * x214) + (-1 * x215); const FLT x219 = x91 * x218; const FLT x220 = x81 * x219; const FLT x221 = x96 * x220; const FLT x222 = x218 * x101; const FLT x223 = (x18 * x216) + (-1 * x18 * x214) + (x18 * x217) + (-1 * x11 * x222) + (-1 * x16 * x222) + (-1 * x18 * x215) + (-1 * x15 * x222) + (-1 * x13 * x222); const FLT x224 = x223 * x105; const FLT x225 = x25 * x220; const FLT x226 = x223 * x104; const FLT x227 = x184 + x211 + (x224 * x100) + (-1 * x94 * x226) + (-1 * x220 * x109) + (x80 * x226) + (-1 * x93 * x225) + (x22 * x221) + x151 + (-1 * x224 * x107) + (x82 * x219); const FLT x228 = x25 * x224; const FLT x229 = (-1 * x226 * x140) + x123 + (-1 * x4 * x228) + x133 + (x224 * x145) + (-1 * x225 * x108) + (x27 * x221) + (x226 * x138) + x205 + (x219 * x139) + (-1 * x219 * x141); const FLT x230 = x118 + (-1 * x25 * x221) + (-1 * x4 * x27 * x224) + x179 + (-1 * x220 * x154) + (-1 * x220 * x153) + (-1 * x220 * x155) + x40 + (-1 * x14 * x228) + (-1 * x226 * x155) + (-1 * x226 * x154); const FLT x231 = (x75 * x227) + (-1 * x76 * x229) + (x74 * x230); const FLT x232 = (-1 * x220 * x130) + (-1 * x226 * x130) + (x224 * x128) + x43 + (x220 * x126) + x149 + x136 + (x220 * x131) + (-1 * x220 * x121) + (-1 * x226 * x121) + (x224 * x125); const FLT x233 = (x76 * x230) + (-1 * x75 * x232) + (x74 * x229); const FLT x234 = x36 * x10; const FLT x235 = x5 * x234; const FLT x236 = -1 * x235; const FLT x237 = 2 * x25; const FLT x238 = 1.0 * x22; const FLT x239 = x238 * (*error_model).Pose.AxisAngleRot[2]; const FLT x240 = 1.0 * (*error_model).Pose.AxisAngleRot[0]; const FLT x241 = x27 * x240; const FLT x242 = 1.0 * x8; const FLT x243 = x242 * (*error_model).Pose.AxisAngleRot[1]; const FLT x244 = x241 + x243 + x237 + x239; const FLT x245 = x34 * (1. / (x29 * sqrt(x29))); const FLT x246 = x244 * x245; const FLT x247 = x4 * x36; const FLT x248 = x21 * x247; const FLT x249 = 1. / (x29 * x29); const FLT x250 = x244 * x249; const FLT x251 = (1. / (x31 * sqrt(x31))) * x34 * x33 * x20; const FLT x252 = ((x30 * x243) + (-1 * x26 * x250) + (x30 * x239) + (-1 * x28 * x250) + (x30 * x241) + (-1 * x23 * x250) + (-1 * x24 * x250) + (x30 * x237)) * x251; const FLT x253 = x81 * x246; const FLT x254 = x36 * x12; const FLT x255 = x36 * x14; const FLT x256 = x7 * x255; const FLT x257 = -1 * x256; const FLT x258 = x257 + x254; const FLT x259 = (x98 * x246) + (x80 * x252) + (x97 * x252) + x258 + (-1 * x246 * x110) + (-1 * x94 * x253) + (x82 * x246) + x248 + (-1 * x94 * x252) + x236 + (-1 * x252 * x109); const FLT x260 = x5 * x254; const FLT x261 = x7 * x247; const FLT x262 = -1 * x261; const FLT x263 = x21 * x255; const FLT x264 = -1 * x263; const FLT x265 = x264 + x262; const FLT x266 = x265 + x260 + (-1 * x253 * x121) + (x253 * x131) + (x252 * x131) + x234 + (x246 * x127) + (-1 * x253 * x130) + (x252 * x126) + (-1 * x252 * x121) + (-1 * x252 * x130); const FLT x267 = x21 * x254; const FLT x268 = -1 * x267; const FLT x269 = x7 * x234; const FLT x270 = x25 * x252; const FLT x271 = x5 * x255; const FLT x272 = -1 * x271; const FLT x273 = x247 + x272; const FLT x274 = (x252 * x144) + (-1 * x270 * x108) + x273 + (-1 * x253 * x143) + x269 + x268 + (x246 * x139) + (x252 * x138) + (-1 * x246 * x141) + (x253 * x144) + (-1 * x252 * x140); const FLT x275 = x245 * x157; const FLT x276 = x5 * x247; const FLT x277 = x7 * x254; const FLT x278 = x21 * x234; const FLT x279 = x278 + x277; const FLT x280 = x279 + x276 + (-1 * x253 * x158) + (-1 * x96 * x270) + (-1 * x253 * x153) + (-1 * x253 * x154) + (-1 * x252 * x153) + (-1 * x252 * x155) + (-1 * x275 * x244) + (-1 * x252 * x154) + x255; const FLT x281 = (x76 * x280) + (-1 * x75 * x266) + (x74 * x274); const FLT x282 = (x75 * x259) + (x74 * x280) + (-1 * x76 * x274); const FLT x283 = 2 * x27; const FLT x284 = x25 * x240; const FLT x285 = x238 * (*error_model).Pose.AxisAngleRot[1]; const FLT x286 = x242 * (*error_model).Pose.AxisAngleRot[2]; const FLT x287 = x285 + (-1 * x286) + x283 + (-1 * x284); const FLT x288 = x287 * x249; const FLT x289 = ((-1 * x30 * x286) + (-1 * x26 * x288) + (-1 * x23 * x288) + (-1 * x28 * x288) + (x30 * x285) + (-1 * x30 * x284) + (x30 * x283) + (-1 * x24 * x288)) * x251; const FLT x290 = x287 * x245; const FLT x291 = -1 * x260; const FLT x292 = x81 * x290; const FLT x293 = x25 * x292; const FLT x294 = x265 + (x80 * x289) + (-1 * x234) + (-1 * x292 * x109) + (-1 * x94 * x289) + (-1 * x93 * x293) + (x82 * x290) + x291 + (x97 * x289) + (-1 * x289 * x109) + (x97 * x292); const FLT x295 = -1 * x255; const FLT x296 = (-1 * x290 * x141) + x295 + (-1 * x289 * x140) + (-1 * x289 * x143) + (-1 * x293 * x108) + (-1 * x276) + x279 + (x292 * x144) + (x289 * x138) + (x290 * x139) + (x289 * x144); const FLT x297 = -1 * x269; const FLT x298 = x273 + x297 + (-1 * x292 * x154) + (-1 * x289 * x155) + (-1 * x290 * x157) + (-1 * x289 * x154) + (-1 * x292 * x153) + (-1 * x289 * x158) + (-1 * x96 * x293) + (-1 * x289 * x153) + x267; const FLT x299 = (x75 * x294) + (-1 * x76 * x296) + (x74 * x298); const FLT x300 = -1 * x248; const FLT x301 = x236 + x300; const FLT x302 = x254 + (-1 * x292 * x130) + (-1 * x289 * x130) + (x292 * x131) + (x289 * x126) + (-1 * x289 * x121) + (x289 * x131) + x301 + (-1 * x292 * x121) + x256 + (x292 * x126); const FLT x303 = (x76 * x298) + (-1 * x75 * x302) + (x74 * x296); const FLT x304 = 1.0 * x27; const FLT x305 = x304 * (*error_model).Pose.AxisAngleRot[2]; const FLT x306 = 1.0 * x25; const FLT x307 = x306 * (*error_model).Pose.AxisAngleRot[1]; const FLT x308 = x22 * x240; const FLT x309 = 2 * x8; const FLT x310 = x309 + (-1 * x308) + x305 + (-1 * x307); const FLT x311 = x249 * x310; const FLT x312 = ((x30 * x309) + (-1 * x26 * x311) + (-1 * x30 * x307) + (-1 * x24 * x311) + (-1 * x28 * x311) + (x30 * x305) + (-1 * x23 * x311) + (-1 * x30 * x308)) * x251; const FLT x313 = x245 * x310; const FLT x314 = x81 * x313; const FLT x315 = x297 + x268; const FLT x316 = (x80 * x312) + (-1 * x94 * x314) + (-1 * x94 * x312) + x315 + (x97 * x312) + (-1 * x312 * x109) + (x98 * x313) + (-1 * x313 * x110) + x247 + x271 + (x82 * x313); const FLT x317 = x25 * x312; const FLT x318 = x301 + (x312 * x144) + (-1 * x317 * x108) + x257 + (x314 * x144) + (-1 * x313 * x141) + (-1 * x254) + (-1 * x312 * x140) + (x313 * x139) + (x312 * x138) + (-1 * x314 * x143); const FLT x319 = x234 + x291; const FLT x320 = x319 + (-1 * x96 * x317) + (-1 * x313 * x157) + x261 + (-1 * x314 * x158) + (-1 * x312 * x153) + (-1 * x314 * x153) + (-1 * x312 * x154) + (-1 * x314 * x154) + x264 + (-1 * x312 * x155); const FLT x321 = (x75 * x316) + (-1 * x76 * x318) + (x74 * x320); const FLT x322 = x276 + x295; const FLT x323 = (-1 * x312 * x121) + (-1 * x312 * x130) + (-1 * x278) + x322 + (x314 * x131) + (x312 * x131) + (-1 * x314 * x121) + x277 + (x313 * x127) + (-1 * x314 * x130) + (x312 * x126); const FLT x324 = (x76 * x320) + (-1 * x75 * x323) + (x74 * x318); const FLT x325 = x304 * (*error_model).Pose.AxisAngleRot[1]; const FLT x326 = x306 * (*error_model).Pose.AxisAngleRot[2]; const FLT x327 = 2 * x22; const FLT x328 = x8 * x240; const FLT x329 = x328 + x327 + (-1 * x325) + (-1 * x326); const FLT x330 = x245 * x329; const FLT x331 = x81 * x330; const FLT x332 = x249 * x329; const FLT x333 = ((-1 * x28 * x332) + (-1 * x26 * x332) + (-1 * x30 * x326) + (-1 * x24 * x332) + (-1 * x30 * x325) + (-1 * x23 * x332) + (x30 * x328) + (x30 * x327)) * x251; const FLT x334 = (-1 * x277) + (-1 * x94 * x333) + x322 + (-1 * x330 * x110) + x278 + (x82 * x330) + (-1 * x94 * x331) + (-1 * x333 * x109) + (x80 * x333) + (x98 * x330) + (x97 * x333); const FLT x335 = (-1 * x333 * x130) + (x331 * x131) + (-1 * x331 * x130) + (-1 * x333 * x121) + (-1 * x331 * x121) + x272 + (x333 * x126) + (-1 * x247) + x315 + (x333 * x131) + (x330 * x127); const FLT x336 = x25 * x331; const FLT x337 = x319 + x263 + (-1 * x333 * x140) + (x333 * x144) + (-1 * x330 * x141) + (x331 * x144) + x262 + (x333 * x138) + (-1 * x336 * x108) + (x330 * x139) + (-1 * x333 * x143); const FLT x338 = (-1 * x333 * x153) + (-1 * x333 * x158) + (-1 * x333 * x155) + (-1 * x331 * x153) + x300 + (-1 * x96 * x336) + x235 + (-1 * x331 * x154) + x258 + (-1 * x275 * x329) + (-1 * x333 * x154); const FLT x339 = (x76 * x338) + (-1 * x75 * x335) + (x74 * x337); const FLT x340 = (x75 * x334) + (-1 * x76 * x337) + (x74 * x338); const FLT x341 = (*_x0).IMUBias.AccScale[1] + (*error_model).IMUBias.AccScale[1]; const FLT x342 = 2 * x70; const FLT x343 = (-1 * x48) + x45 + (-1 * x39) + x42; const FLT x344 = (-1 * x74 * x50) + (x76 * x57) + (x72 * x75); const FLT x345 = 2 * x344; const FLT x346 = (x75 * x161) + (-1 * x74 * x119) + (x76 * x134); const FLT x347 = (x75 * x187) + (-1 * x74 * x178) + (x76 * x181); const FLT x348 = (x75 * x208) + (-1 * x74 * x204) + (x76 * x212); const FLT x349 = (x75 * x230) + (x76 * x232) + (-1 * x74 * x227); const FLT x350 = (x75 * x280) + (x76 * x266) + (-1 * x74 * x259); const FLT x351 = (x75 * x298) + (-1 * x74 * x294) + (x76 * x302); const FLT x352 = (x76 * x323) + (x75 * x320) + (-1 * x74 * x316); const FLT x353 = (x75 * x338) + (-1 * x74 * x334) + (x76 * x335); const FLT x354 = (*_x0).IMUBias.AccScale[2] + (*error_model).IMUBias.AccScale[2]; const FLT x355 = x57 * x57; const FLT x356 = x70 * x70; const FLT x357 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x358 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x359 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x360 = 2 * ((-1 * x70 * x359) + (x72 * x357) + (x50 * x358)); const FLT x361 = 2 * ((-1 * x57 * x358) + (x72 * x359) + (x70 * x357)); const FLT x362 = (-1 * x358 * x134) + (x357 * x150) + (x359 * x161); const FLT x363 = (-1 * x359 * x150) + (x358 * x119) + (x357 * x161); const FLT x364 = (x357 * x185) + (-1 * x358 * x181) + (x359 * x187); const FLT x365 = (x358 * x178) + (x357 * x187) + (-1 * x359 * x185); const FLT x366 = (x204 * x358) + (-1 * x209 * x359) + (x208 * x357); const FLT x367 = (x209 * x357) + (-1 * x212 * x358) + (x208 * x359); const FLT x368 = (x227 * x358) + (x230 * x357) + (-1 * x229 * x359); const FLT x369 = (x229 * x357) + (x230 * x359) + (-1 * x232 * x358); const FLT x370 = (x274 * x357) + (-1 * x266 * x358) + (x280 * x359); const FLT x371 = (x259 * x358) + (-1 * x274 * x359) + (x280 * x357); const FLT x372 = (x296 * x357) + (-1 * x358 * x302) + (x298 * x359); const FLT x373 = (x294 * x358) + (-1 * x296 * x359) + (x298 * x357); const FLT x374 = (x358 * x316) + (x357 * x320) + (-1 * x359 * x318); const FLT x375 = (-1 * x358 * x323) + (x357 * x318) + (x359 * x320); const FLT x376 = (x357 * x337) + (x359 * x338) + (-1 * x358 * x335); const FLT x377 = (x358 * x334) + (-1 * x359 * x337) + (x357 * x338); const FLT x378 = (-1 * x55) + (-1 * x54) + x52 + x53; const FLT x379 = x72 * x51; const FLT x380 = (-1 * x68) + x67 + (-1 * x65) + x66; const FLT x381 = x72 * x58; const FLT x382 = 2 * ((-1 * x50 * x357) + (x72 * x358) + (x57 * x359)); const FLT x383 = (x359 * x134) + (-1 * x357 * x119) + (x358 * x161); const FLT x384 = (x359 * x181) + (-1 * x357 * x178) + (x358 * x187); const FLT x385 = (-1 * x204 * x357) + (x212 * x359) + (x208 * x358); const FLT x386 = (x232 * x359) + (x230 * x358) + (-1 * x227 * x357); const FLT x387 = (x266 * x359) + (-1 * x259 * x357) + (x280 * x358); const FLT x388 = (x359 * x302) + (-1 * x294 * x357) + (x298 * x358); const FLT x389 = (x359 * x323) + (-1 * x357 * x316) + (x358 * x320); const FLT x390 = (x359 * x335) + (-1 * x357 * x334) + (x358 * x338); const FLT x391 = (-1 * x47) + x44 + (-1 * x38) + x41; const FLT x392 = x72 * x342; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x0 * (0.101971621297793 + (x58 * x56) + (-1 * x51 * x49))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x0 * ((x69 * x58) + (-1 * x64))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), ((x73 * x72) + (x71 * x50)) * x0); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.AccScale[0])/sizeof(FLT), x75 + (2 * ((x78 * x57) + (-1 * x77 * x50)))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x0 * ((x58 * x163) + (-1 * x51 * x162) + (-1 * x119 * x120) + (x134 * x135))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x0 * ((x58 * x189) + (-1 * x51 * x188) + (-1 * x120 * x178) + (x181 * x135))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x0 * ((x58 * x213) + (x212 * x135) + (-1 * x204 * x120) + (-1 * x51 * x210))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x0 * ((x58 * x233) + (x232 * x135) + (-1 * x227 * x120) + (-1 * x51 * x231))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x0 * ((x266 * x135) + (-1 * x51 * x282) + (-1 * x259 * x120) + (x58 * x281))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x0 * ((x58 * x303) + (x302 * x135) + (-1 * x294 * x120) + (-1 * x51 * x299))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x0 * ((x58 * x324) + (x323 * x135) + (-1 * x316 * x120) + (-1 * x51 * x321))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x0 * ((x335 * x135) + (-1 * x51 * x340) + (-1 * x334 * x120) + (x58 * x339))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x341 * (x64 + (-1 * x56 * x342))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), x341 * (0.101971621297793 + (x51 * x343) + (-1 * x69 * x342))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), ((x73 * x50) + (-1 * x71 * x72)) * x341); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.AccScale[1])/sizeof(FLT), x74 + (2 * ((x50 * x344) + (-1 * x70 * x78)))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x341 * ((x51 * x346) + (x345 * x119) + (-1 * x135 * x150) + (-1 * x342 * x163))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x341 * ((x51 * x347) + (x345 * x178) + (-1 * x185 * x135) + (-1 * x342 * x189))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x341 * ((x51 * x348) + (x204 * x345) + (-1 * x209 * x135) + (-1 * x213 * x342))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x341 * ((-1 * x233 * x342) + (x227 * x345) + (-1 * x229 * x135) + (x51 * x349))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x341 * ((x51 * x350) + (-1 * x281 * x342) + (-1 * x274 * x135) + (x259 * x345))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x341 * ((x51 * x351) + (-1 * x303 * x342) + (-1 * x296 * x135) + (x294 * x345))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x341 * ((-1 * x324 * x342) + (x316 * x345) + (-1 * x318 * x135) + (x51 * x352))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x341 * ((x334 * x345) + (-1 * x337 * x135) + (x51 * x353) + (-1 * x339 * x342))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[0])/sizeof(FLT), x354 * ((x49 * x342) + (-1 * x63 * x58))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[1])/sizeof(FLT), ((x63 * x342) + (-1 * x58 * x343)) * x354); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Acc[2])/sizeof(FLT), x354 * (0.101971621297793 + (-0.203943242595586 * x356) + (-0.203943242595586 * x355))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.AccScale[2])/sizeof(FLT), x76 + (2 * ((x70 * x77) + (-1 * x57 * x344)))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x354 * ((x342 * x162) + (-1 * x345 * x134) + (x120 * x150) + (-1 * x58 * x346))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x354 * ((x120 * x185) + (-1 * x58 * x347) + (x342 * x188) + (-1 * x345 * x181))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x354 * ((x210 * x342) + (x209 * x120) + (-1 * x212 * x345) + (-1 * x58 * x348))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), x354 * ((x231 * x342) + (-1 * x58 * x349) + (-1 * x232 * x345) + (x229 * x120))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), x354 * ((x274 * x120) + (-1 * x58 * x350) + (x282 * x342) + (-1 * x266 * x345))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), x354 * ((x296 * x120) + (-1 * x302 * x345) + (x299 * x342) + (-1 * x58 * x351))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), x354 * ((-1 * x58 * x352) + (x321 * x342) + (-1 * x323 * x345) + (x318 * x120))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), x354 * ((x342 * x340) + (-1 * x58 * x353) + (x337 * x120) + (-1 * x335 * x345))); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x51 * x363) + (x58 * x362) + (-1 * x360 * x119) + (x361 * x134)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x361 * x181) + (-1 * x51 * x365) + (-1 * x360 * x178) + (x58 * x364)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x58 * x367) + (x212 * x361) + (-1 * x204 * x360) + (-1 * x51 * x366)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (x232 * x361) + (x58 * x369) + (-1 * x51 * x368) + (-1 * x227 * x360)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x51 * x371) + (-1 * x259 * x360) + (x266 * x361) + (x58 * x370)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x58 * x372) + (-1 * x51 * x373) + (-1 * x294 * x360) + (x361 * x302)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x58 * x375) + (x361 * x323) + (-1 * x360 * x316) + (-1 * x51 * x374)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x51 * x377) + (-1 * x360 * x334) + (x361 * x335) + (x58 * x376)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1 + (x58 * x378) + (-2 * (x50 * x50))); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (x57 * x342) + (-1 * x379)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x381 + (-1 * x51 * x380)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x361 * x150) + (x51 * x383) + (x382 * x119) + (-1 * x362 * x342)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x382 * x178) + (-1 * x361 * x185) + (x51 * x384) + (-1 * x364 * x342)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x204 * x382) + (x51 * x385) + (-1 * x209 * x361) + (-1 * x367 * x342)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x369 * x342) + (x227 * x382) + (-1 * x229 * x361) + (x51 * x386)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (-1 * x370 * x342) + (-1 * x274 * x361) + (x51 * x387) + (x259 * x382)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (-1 * x372 * x342) + (x294 * x382) + (-1 * x296 * x361) + (x51 * x388)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (x382 * x316) + (-1 * x361 * x318) + (x51 * x389) + (-1 * x375 * x342)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (x51 * x390) + (-1 * x376 * x342) + (x382 * x334) + (-1 * x361 * x337)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x379 + (-1 * x378 * x342)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1 + (x51 * x391) + (-2 * x356)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (x51 * x57) + (-1 * x392)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x382 * x134) + (x363 * x342) + (x360 * x150) + (-1 * x58 * x383)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x365 * x342) + (x360 * x185) + (-1 * x382 * x181) + (-1 * x58 * x384)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x58 * x385) + (-1 * x212 * x382) + (x366 * x342) + (x209 * x360)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, IMUBias.IMUCorrection[3])/sizeof(FLT), (x368 * x342) + (x229 * x360) + (-1 * x232 * x382) + (-1 * x58 * x386)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[0])/sizeof(FLT), (x371 * x342) + (x274 * x360) + (-1 * x266 * x382) + (-1 * x58 * x387)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[1])/sizeof(FLT), (x373 * x342) + (x296 * x360) + (-1 * x382 * x302) + (-1 * x58 * x388)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[2])/sizeof(FLT), (-1 * x58 * x389) + (x374 * x342) + (-1 * x382 * x323) + (x360 * x318)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Pose.Rot[3])/sizeof(FLT), (-1 * x58 * x390) + (x377 * x342) + (-1 * x382 * x335) + (x360 * x337)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (x70 * x51) + (-1 * x381)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x392 + (-1 * x58 * x391)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1 + (x380 * x342) + (-2 * x355)); } // Full version Jacobian of IMUMeasurementErrorModel wrt [(*_x0).Acc[0], (*_x0).Acc[1], (*_x0).Acc[2], (*_x0).IMUBias.AccBias[0], (*_x0).IMUBias.AccBias[1], (*_x0).IMUBias.AccBias[2], (*_x0).IMUBias.AccScale[0], (*_x0).IMUBias.AccScale[1], (*_x0).IMUBias.AccScale[2], (*_x0).IMUBias.GyroBias[0], (*_x0).IMUBias.GyroBias[1], (*_x0).IMUBias.GyroBias[2], (*_x0).IMUBias.IMUCorrection[0], (*_x0).IMUBias.IMUCorrection[1], (*_x0).IMUBias.IMUCorrection[2], (*_x0).IMUBias.IMUCorrection[3], (*_x0).Pose.Pos[0], (*_x0).Pose.Pos[1], (*_x0).Pose.Pos[2], (*_x0).Pose.Rot[0], (*_x0).Pose.Rot[1], (*_x0).Pose.Rot[2], (*_x0).Pose.Rot[3], (*_x0).Velocity.AxisAngleRot[0], (*_x0).Velocity.AxisAngleRot[1], (*_x0).Velocity.AxisAngleRot[2], (*_x0).Velocity.Pos[0], (*_x0).Velocity.Pos[1], (*_x0).Velocity.Pos[2]] static inline void IMUMeasurementErrorModel_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { if(hx != 0) { IMUMeasurementErrorModel(hx, _x0, error_model); } if(Hx != 0) { IMUMeasurementErrorModel_jac_x0(Hx, _x0, error_model); } } // Jacobian of IMUMeasurementErrorModel wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void IMUMeasurementErrorModel_jac_error_model(CnMat* Hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { const FLT x0 = (*_x0).IMUBias.AccScale[0] + (*error_model).IMUBias.AccScale[0]; const FLT x1 = 0.5 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x2 = 0.5 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x3 = 0.5 * (*error_model).IMUBias.IMUCorrection[2]; const FLT x4 = (x3 * (*_x0).IMUBias.IMUCorrection[2]) + (-1 * x1 * (*error_model).IMUBias.IMUCorrection[1]) + (*_x0).IMUBias.IMUCorrection[1] + (x2 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x5 = 0.5 * (*error_model).Pose.AxisAngleRot[0]; const FLT x6 = 0.5 * (*_x0).Pose.Rot[0]; const FLT x7 = 0.5 * (*error_model).Pose.AxisAngleRot[2]; const FLT x8 = (x6 * (*error_model).Pose.AxisAngleRot[1]) + (x5 * (*_x0).Pose.Rot[3]) + (-1 * x7 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[2]; const FLT x9 = 0.5 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x10 = (-1 * x9 * (*error_model).IMUBias.IMUCorrection[2]) + (x2 * (*error_model).IMUBias.IMUCorrection[1]) + (x1 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[2]; const FLT x11 = x10 * x10; const FLT x12 = 0.5 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x13 = (x9 * (*error_model).IMUBias.IMUCorrection[1]) + (x3 * (*_x0).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[3] + (-1 * x12 * (*error_model).IMUBias.IMUCorrection[0]); const FLT x14 = x13 * x13; const FLT x15 = (-1 * x9 * (*error_model).IMUBias.IMUCorrection[0]) + (*_x0).IMUBias.IMUCorrection[0] + (-1 * x3 * (*_x0).IMUBias.IMUCorrection[3]) + (-1 * x12 * (*error_model).IMUBias.IMUCorrection[1]); const FLT x16 = x15 * x15; const FLT x17 = x4 * x4; const FLT x18 = x16 + x17 + x11 + x14; const FLT x19 = 1. / sqrt(x18); const FLT x20 = 0.5 * (*error_model).Pose.AxisAngleRot[1]; const FLT x21 = (x20 * (*_x0).Pose.Rot[1]) + (*_x0).Pose.Rot[3] + (-1 * x5 * (*_x0).Pose.Rot[2]) + (x6 * (*error_model).Pose.AxisAngleRot[2]); const FLT x22 = x21 * x21; const FLT x23 = x8 * x8; const FLT x24 = (-1 * x20 * (*_x0).Pose.Rot[2]) + (*_x0).Pose.Rot[0] + (-1 * x5 * (*_x0).Pose.Rot[1]) + (-1 * x7 * (*_x0).Pose.Rot[3]); const FLT x25 = x24 * x24; const FLT x26 = (*_x0).Pose.Rot[1] + (x6 * (*error_model).Pose.AxisAngleRot[0]) + (-1 * x20 * (*_x0).Pose.Rot[3]) + (x7 * (*_x0).Pose.Rot[2]); const FLT x27 = x26 * x26; const FLT x28 = x27 + x25 + x22 + x23; const FLT x29 = 1. / x28; const FLT x30 = (x23 * x29) + (x22 * x29) + (x29 * x27) + (x25 * x29); const FLT x31 = 1. / sqrt(x30); const FLT x32 = 1. / sqrt(x28); const FLT x33 = 1. / x18; const FLT x34 = (x33 * x16) + (x33 * x17) + (x33 * x14) + (x33 * x11); const FLT x35 = 1. / sqrt(x34); const FLT x36 = x32 * x31 * x35; const FLT x37 = x36 * x19; const FLT x38 = x8 * x37; const FLT x39 = x4 * x38; const FLT x40 = 0.101971621297793 * x39; const FLT x41 = x37 * x15; const FLT x42 = x41 * x21; const FLT x43 = 0.101971621297793 * x42; const FLT x44 = x37 * x10; const FLT x45 = x44 * x26; const FLT x46 = 0.101971621297793 * x45; const FLT x47 = x37 * x24; const FLT x48 = x47 * x13; const FLT x49 = 0.101971621297793 * x48; const FLT x50 = (-1 * x46) + x49 + x40 + (-1 * x43); const FLT x51 = (-1 * x45) + x48 + (-1 * x42) + x39; const FLT x52 = 2 * x51; const FLT x53 = x8 * x41; const FLT x54 = x4 * x37; const FLT x55 = x54 * x21; const FLT x56 = x37 * x13; const FLT x57 = x56 * x26; const FLT x58 = x47 * x10; const FLT x59 = (-0.101971621297793 * x58) + (-0.101971621297793 * x57) + (0.101971621297793 * x53) + (0.101971621297793 * x55); const FLT x60 = x58 + (-1 * x55) + x57 + (-1 * x53); const FLT x61 = 2 * x60; const FLT x62 = x38 * x10; const FLT x63 = x41 * x24; const FLT x64 = x54 * x26; const FLT x65 = x56 * x21; const FLT x66 = (0.101971621297793 * x65) + (0.101971621297793 * x64) + (0.101971621297793 * x62) + (0.101971621297793 * x63); const FLT x67 = x66 * x52; const FLT x68 = x4 * x47; const FLT x69 = x38 * x13; const FLT x70 = x41 * x26; const FLT x71 = x44 * x21; const FLT x72 = (0.101971621297793 * x71) + (-0.101971621297793 * x70) + (0.101971621297793 * x68) + (-0.101971621297793 * x69); const FLT x73 = (-1 * x70) + x71 + (-1 * x69) + x68; const FLT x74 = 0.203943242595586 * x51; const FLT x75 = x64 + x63 + x65 + x62; const FLT x76 = 0.203943242595586 * x75; const FLT x77 = 0.101971621297793 * ((*_x0).Acc[1] + (*error_model).Acc[1]); const FLT x78 = 0.101971621297793 * ((*_x0).Acc[0] + (*error_model).Acc[0]); const FLT x79 = 1 + (0.101971621297793 * ((*_x0).Acc[2] + (*error_model).Acc[2])); const FLT x80 = (-1 * x73 * x79) + (x75 * x77) + (x78 * x51); const FLT x81 = (-1 * x78 * x60) + (x73 * x77) + (x79 * x75); const FLT x82 = 1.0/2.0 * x26; const FLT x83 = 1.0 * x4; const FLT x84 = x83 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x85 = 1.0 * x15; const FLT x86 = x85 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x87 = 1.0 * x10; const FLT x88 = x87 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x89 = 1.0 * x13; const FLT x90 = x89 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x91 = (-1 * x90) + x88 + x84 + (-1 * x86); const FLT x92 = x36 * (1. / (x18 * sqrt(x18))); const FLT x93 = x92 * x91; const FLT x94 = x93 * x15; const FLT x95 = 1.0/2.0 * x8; const FLT x96 = x93 * x13; const FLT x97 = x95 * x13; const FLT x98 = 1. / (x18 * x18); const FLT x99 = x91 * x98; const FLT x100 = x98 * x17; const FLT x101 = x32 * x31 * (1. / (x34 * sqrt(x34))) * x19; const FLT x102 = x101 * ((x88 * x33) + (-1 * x91 * x100) + (-1 * x99 * x16) + (x84 * x33) + (-1 * x99 * x14) + (-1 * x99 * x11) + (-1 * x90 * x33) + (-1 * x86 * x33)); const FLT x103 = 1.0/2.0 * x24; const FLT x104 = x93 * x103; const FLT x105 = 1.0/2.0 * x10; const FLT x106 = x21 * x105; const FLT x107 = x4 * x103; const FLT x108 = x93 * x105; const FLT x109 = x82 * x102; const FLT x110 = x37 * x21; const FLT x111 = x37 * x26; const FLT x112 = (x9 * x111) + (x1 * x110) + (x38 * x12) + (x2 * x47); const FLT x113 = x112 + (x15 * x109) + (-1 * x21 * x108) + (x96 * x95) + (x82 * x94) + (-1 * x102 * x107) + (x97 * x102) + (-1 * x4 * x104) + (-1 * x102 * x106); const FLT x114 = x4 * x82; const FLT x115 = x8 * x105; const FLT x116 = 1.0/2.0 * x21; const FLT x117 = x13 * x116; const FLT x118 = x15 * x103; const FLT x119 = x1 * x38; const FLT x120 = x9 * x47; const FLT x121 = x37 * x12; const FLT x122 = x21 * x121; const FLT x123 = x2 * x111; const FLT x124 = (-1 * x122) + x119 + x123 + (-1 * x120); const FLT x125 = x124 + (-1 * x8 * x108) + (-1 * x102 * x115) + (-1 * x102 * x114) + (-1 * x102 * x118) + (-1 * x96 * x116) + (-1 * x93 * x114) + (-1 * x102 * x117) + (-1 * x94 * x103); const FLT x126 = x82 * x10; const FLT x127 = x15 * x116; const FLT x128 = x13 * x103; const FLT x129 = x4 * x95; const FLT x130 = x2 * x38; const FLT x131 = x9 * x110; const FLT x132 = x1 * x111; const FLT x133 = x47 * x12; const FLT x134 = (-1 * x133) + (-1 * x132) + x130 + x131; const FLT x135 = x134 + (-1 * x102 * x129) + (x93 * x127) + (x102 * x126) + (-1 * x93 * x129) + (x93 * x126) + (-1 * x102 * x128) + (-1 * x13 * x104) + (x102 * x127); const FLT x136 = (-1 * x79 * x113) + (x78 * x135) + (x77 * x125); const FLT x137 = 2 * x80; const FLT x138 = x4 * x116; const FLT x139 = x2 * x110; const FLT x140 = x1 * x47; const FLT x141 = x26 * x121; const FLT x142 = x95 * x15; const FLT x143 = x9 * x38; const FLT x144 = x24 * x105; const FLT x145 = (-1 * x24 * x108) + (x95 * x94) + (-1 * x139) + (x102 * x138) + x140 + (-1 * x82 * x96) + (-1 * x141) + (-1 * x13 * x109) + (x102 * x142) + (x93 * x138) + x143 + (-1 * x102 * x144); const FLT x146 = (x79 * x125) + (-1 * x78 * x145) + (x77 * x113); const FLT x147 = 2 * x81; const FLT x148 = x83 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x149 = x85 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x150 = x87 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x151 = x89 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x152 = x151 + x150 + (-1 * x148) + (-1 * x149); const FLT x153 = x92 * x152; const FLT x154 = x98 * x152; const FLT x155 = x101 * ((-1 * x100 * x152) + (-1 * x33 * x149) + (-1 * x14 * x154) + (x33 * x150) + (-1 * x11 * x154) + (-1 * x16 * x154) + (-1 * x33 * x148) + (x33 * x151)); const FLT x156 = x122 + (x127 * x153) + (x126 * x153) + (-1 * x129 * x153) + (-1 * x119) + (x126 * x155) + (-1 * x129 * x155) + (-1 * x128 * x153) + (x127 * x155) + x120 + (-1 * x123) + (-1 * x128 * x155); const FLT x157 = x15 * x153; const FLT x158 = x82 * x155; const FLT x159 = x103 * x155; const FLT x160 = x13 * x153; const FLT x161 = x13 * x155; const FLT x162 = x141 + x139 + (-1 * x140) + (-1 * x143); const FLT x163 = (-1 * x106 * x155) + x162 + (-1 * x106 * x153) + (-1 * x4 * x159) + (x82 * x157) + (-1 * x107 * x153) + (x95 * x161) + (x15 * x158) + (x95 * x160); const FLT x164 = (-1 * x15 * x159) + (-1 * x115 * x153) + (-1 * x114 * x155) + (-1 * x116 * x161) + x134 + (-1 * x116 * x160) + (-1 * x103 * x157) + (-1 * x115 * x155) + (-1 * x114 * x153); const FLT x165 = (-1 * x79 * x163) + (x78 * x156) + (x77 * x164); const FLT x166 = (-1 * x13 * x158) + (-1 * x144 * x153) + (x95 * x157) + (x142 * x155) + (x138 * x153) + (-1 * x82 * x160) + (-1 * x144 * x155) + x112 + (x138 * x155); const FLT x167 = (x79 * x164) + (-1 * x78 * x166) + (x77 * x163); const FLT x168 = x83 * (*_x0).IMUBias.IMUCorrection[2]; const FLT x169 = x85 * (*_x0).IMUBias.IMUCorrection[3]; const FLT x170 = x87 * (*_x0).IMUBias.IMUCorrection[1]; const FLT x171 = x89 * (*_x0).IMUBias.IMUCorrection[0]; const FLT x172 = (-1 * x170) + x168 + x171 + (-1 * x169); const FLT x173 = x98 * x172; const FLT x174 = x101 * ((-1 * x11 * x173) + (-1 * x33 * x170) + (-1 * x100 * x172) + (-1 * x33 * x169) + (x33 * x168) + (-1 * x14 * x173) + (-1 * x16 * x173) + (x33 * x171)); const FLT x175 = x92 * x172; const FLT x176 = x4 * x175; const FLT x177 = x13 * x175; const FLT x178 = x112 + (-1 * x103 * x177) + (-1 * x128 * x174) + (x127 * x175) + (x126 * x174) + (-1 * x129 * x174) + (x126 * x175) + (x127 * x174) + (-1 * x95 * x176); const FLT x179 = x15 * x175; const FLT x180 = x105 * x175; const FLT x181 = x82 * x174; const FLT x182 = x132 + (-1 * x21 * x180) + (-1 * x106 * x174) + x133 + (x97 * x174) + (x95 * x177) + (-1 * x131) + (-1 * x107 * x174) + (x82 * x179) + (x15 * x181) + (-1 * x130) + (-1 * x103 * x176); const FLT x183 = (-1 * x103 * x179) + (-1 * x8 * x180) + (-1 * x114 * x174) + (-1 * x115 * x174) + (-1 * x114 * x175) + (-1 * x116 * x177) + x162 + (-1 * x117 * x174) + (-1 * x118 * x174); const FLT x184 = (x78 * x178) + (-1 * x79 * x182) + (x77 * x183); const FLT x185 = (-1 * x13 * x181) + (-1 * x175 * x144) + (x174 * x138) + (x174 * x142) + (x175 * x138) + (-1 * x82 * x177) + x124 + (x95 * x179) + (-1 * x174 * x144); const FLT x186 = (x79 * x183) + (x77 * x182) + (-1 * x78 * x185); const FLT x187 = x35 * x19; const FLT x188 = 1.0 * x24; const FLT x189 = x188 * (*_x0).Pose.Rot[1]; const FLT x190 = 1.0 * x21; const FLT x191 = x190 * (*_x0).Pose.Rot[2]; const FLT x192 = 1.0 * x26; const FLT x193 = x192 * (*_x0).Pose.Rot[0]; const FLT x194 = 1.0 * x8; const FLT x195 = x194 * (*_x0).Pose.Rot[3]; const FLT x196 = x195 + x193 + (-1 * x189) + (-1 * x191); const FLT x197 = x31 * (1. / (x28 * sqrt(x28))); const FLT x198 = x196 * x197; const FLT x199 = x187 * x198; const FLT x200 = 0.5 * x41; const FLT x201 = x200 * (*_x0).Pose.Rot[2]; const FLT x202 = 0.5 * x54; const FLT x203 = x202 * (*_x0).Pose.Rot[3]; const FLT x204 = 1. / (x28 * x28); const FLT x205 = x204 * x196; const FLT x206 = (x29 * x195) + (-1 * x25 * x205) + (-1 * x29 * x189) + (-1 * x23 * x205) + (-1 * x22 * x205) + (-1 * x27 * x205) + (-1 * x29 * x191) + (x29 * x193); const FLT x207 = (1. / (x30 * sqrt(x30))) * x32; const FLT x208 = x206 * x207; const FLT x209 = x208 * x187; const FLT x210 = x4 * x199; const FLT x211 = x6 * x44; const FLT x212 = 0.5 * x56; const FLT x213 = x212 * (*_x0).Pose.Rot[1]; const FLT x214 = x4 * x187; const FLT x215 = x95 * x208; const FLT x216 = x13 * x187; const FLT x217 = x216 * x103; const FLT x218 = x126 * x187; const FLT x219 = (x218 * x208) + (x209 * x127) + (x127 * x199) + (-1 * x211) + (-1 * x217 * x208) + x203 + x201 + (x126 * x199) + (-1 * x95 * x210) + (-1 * x215 * x214) + (-1 * x213) + (-1 * x217 * x198); const FLT x220 = x15 * x199; const FLT x221 = x15 * x187; const FLT x222 = x82 * x221; const FLT x223 = x214 * x103; const FLT x224 = x216 * x198; const FLT x225 = x105 * x199; const FLT x226 = 0.5 * x44; const FLT x227 = (-1 * x226 * (*_x0).Pose.Rot[2]) + (-1 * x212 * (*_x0).Pose.Rot[3]) + (-1 * x6 * x41) + (-1 * x202 * (*_x0).Pose.Rot[1]); const FLT x228 = x227 + (-1 * x21 * x225) + (x95 * x224) + (x82 * x220) + (-1 * x209 * x106) + (-1 * x208 * x223) + (x208 * x222) + (x215 * x216) + (-1 * x210 * x103); const FLT x229 = x221 * x103; const FLT x230 = x114 * x187; const FLT x231 = x208 * x116; const FLT x232 = x226 * (*_x0).Pose.Rot[3]; const FLT x233 = x6 * x54; const FLT x234 = x212 * (*_x0).Pose.Rot[2]; const FLT x235 = x200 * (*_x0).Pose.Rot[1]; const FLT x236 = (-1 * x235) + (-1 * x234) + x232 + x233; const FLT x237 = (-1 * x220 * x103) + (-1 * x216 * x231) + (-1 * x208 * x229) + x236 + (-1 * x209 * x115) + (-1 * x8 * x225) + (-1 * x82 * x210) + (-1 * x224 * x116) + (-1 * x230 * x208); const FLT x238 = 2 * ((x78 * x219) + (-1 * x79 * x228) + (x77 * x237)); const FLT x239 = x95 * x221; const FLT x240 = x82 * x216; const FLT x241 = x207 * x240; const FLT x242 = x200 * (*_x0).Pose.Rot[3]; const FLT x243 = x202 * (*_x0).Pose.Rot[2]; const FLT x244 = x226 * (*_x0).Pose.Rot[1]; const FLT x245 = x6 * x56; const FLT x246 = x245 + (-1 * x244) + (-1 * x242) + x243; const FLT x247 = x246 + (x210 * x116) + (-1 * x206 * x241) + (-1 * x209 * x144) + (x239 * x208) + (-1 * x82 * x224) + (-1 * x24 * x225) + (x214 * x231) + (x95 * x220); const FLT x248 = (-1 * x78 * x247) + (x79 * x237) + (x77 * x228); const FLT x249 = x192 * (*_x0).Pose.Rot[3]; const FLT x250 = x188 * (*_x0).Pose.Rot[2]; const FLT x251 = x190 * (*_x0).Pose.Rot[1]; const FLT x252 = x194 * (*_x0).Pose.Rot[0]; const FLT x253 = x252 + x251 + (-1 * x249) + (-1 * x250); const FLT x254 = x253 * x197; const FLT x255 = x254 * x187; const FLT x256 = x204 * x253; const FLT x257 = (x29 * x252) + (-1 * x29 * x250) + (-1 * x23 * x256) + (-1 * x27 * x256) + (-1 * x29 * x249) + (x29 * x251) + (-1 * x22 * x256) + (-1 * x25 * x256); const FLT x258 = x207 * x257; const FLT x259 = x95 * x258; const FLT x260 = x258 * x187; const FLT x261 = x95 * x254; const FLT x262 = x236 + (-1 * x217 * x258) + (x255 * x127) + (x218 * x258) + (x255 * x126) + (-1 * x217 * x254) + (-1 * x214 * x259) + (-1 * x214 * x261) + (x260 * x127); const FLT x263 = x258 * x116; const FLT x264 = x254 * x103; const FLT x265 = x254 * x116; const FLT x266 = x255 * x105; const FLT x267 = (-1 * x201) + (-1 * x203) + x211 + x213; const FLT x268 = (-1 * x230 * x258) + (-1 * x258 * x229) + (-1 * x216 * x263) + (-1 * x264 * x221) + (-1 * x260 * x115) + (-1 * x8 * x266) + x267 + (-1 * x230 * x254) + (-1 * x216 * x265); const FLT x269 = (x258 * x222) + x244 + (-1 * x243) + (x216 * x261) + (-1 * x260 * x106) + (-1 * x21 * x266) + (-1 * x245) + (x216 * x259) + (-1 * x258 * x223) + (-1 * x214 * x264) + x242 + (x254 * x222); const FLT x270 = (x78 * x262) + (x77 * x268) + (-1 * x79 * x269); const FLT x271 = x227 + (x214 * x263) + (-1 * x255 * x144) + (x239 * x258) + (-1 * x254 * x240) + (-1 * x257 * x241) + (x214 * x265) + (x239 * x254) + (-1 * x260 * x144); const FLT x272 = (x79 * x268) + (x77 * x269) + (-1 * x78 * x271); const FLT x273 = x192 * (*_x0).Pose.Rot[2]; const FLT x274 = x188 * (*_x0).Pose.Rot[3]; const FLT x275 = x190 * (*_x0).Pose.Rot[0]; const FLT x276 = x194 * (*_x0).Pose.Rot[1]; const FLT x277 = (-1 * x276) + x275 + x273 + (-1 * x274); const FLT x278 = x277 * x197; const FLT x279 = x278 * x187; const FLT x280 = x204 * x277; const FLT x281 = ((-1 * x29 * x276) + (x29 * x275) + (-1 * x25 * x280) + (-1 * x22 * x280) + (-1 * x29 * x274) + (-1 * x23 * x280) + (-1 * x27 * x280) + (x29 * x273)) * x207; const FLT x282 = x281 * x187; const FLT x283 = x214 * x278; const FLT x284 = x95 * x281; const FLT x285 = x227 + (x282 * x126) + (-1 * x214 * x284) + (x218 * x278) + (x282 * x127) + (-1 * x217 * x281) + (x279 * x127) + (-1 * x95 * x283) + (-1 * x217 * x278); const FLT x286 = x281 * x116; const FLT x287 = x282 * x105; const FLT x288 = (x283 * x116) + (-1 * x278 * x240) + x234 + x235 + (x214 * x286) + (x239 * x281) + (-1 * x232) + (-1 * x281 * x240) + (-1 * x233) + (x239 * x278) + (-1 * x24 * x287) + (-1 * x279 * x144); const FLT x289 = x281 * x103; const FLT x290 = x216 * x278; const FLT x291 = x267 + (x281 * x222) + (x216 * x284) + (-1 * x283 * x103) + (-1 * x214 * x289) + (x278 * x222) + (-1 * x279 * x106) + (x95 * x290) + (-1 * x21 * x287); const FLT x292 = x246 + (-1 * x278 * x229) + (-1 * x230 * x281) + (-1 * x279 * x115) + (-1 * x290 * x116) + (-1 * x230 * x278) + (-1 * x8 * x287) + (-1 * x289 * x221) + (-1 * x216 * x286); const FLT x293 = (x79 * x292) + (-1 * x78 * x288) + (x77 * x291); const FLT x294 = (x78 * x285) + (-1 * x79 * x291) + (x77 * x292); const FLT x295 = (*_x0).IMUBias.AccScale[1] + (*error_model).IMUBias.AccScale[1]; const FLT x296 = 2 * x73; const FLT x297 = x46 + (-1 * x49) + (-1 * x40) + x43; const FLT x298 = (-1 * x77 * x51) + (x79 * x60) + (x78 * x75); const FLT x299 = 2 * x298; const FLT x300 = (x78 * x125) + (-1 * x77 * x135) + (x79 * x145); const FLT x301 = (x78 * x164) + (-1 * x77 * x156) + (x79 * x166); const FLT x302 = (-1 * x77 * x178) + (x78 * x183) + (x79 * x185); const FLT x303 = (x78 * x237) + (x79 * x247) + (-1 * x77 * x219); const FLT x304 = (x78 * x268) + (-1 * x77 * x262) + (x79 * x271); const FLT x305 = (x78 * x292) + (-1 * x77 * x285) + (x79 * x288); const FLT x306 = (*_x0).IMUBias.AccScale[2] + (*error_model).IMUBias.AccScale[2]; const FLT x307 = x60 * x60; const FLT x308 = x73 * x73; const FLT x309 = (*_x0).Velocity.AxisAngleRot[1] + (*error_model).Velocity.AxisAngleRot[1]; const FLT x310 = (*_x0).Velocity.AxisAngleRot[0] + (*error_model).Velocity.AxisAngleRot[0]; const FLT x311 = (*_x0).Velocity.AxisAngleRot[2] + (*error_model).Velocity.AxisAngleRot[2]; const FLT x312 = 2 * ((-1 * x73 * x311) + (x75 * x309) + (x51 * x310)); const FLT x313 = 2 * ((x75 * x311) + (-1 * x60 * x310) + (x73 * x309)); const FLT x314 = (x309 * x113) + (-1 * x310 * x145) + (x311 * x125); const FLT x315 = (x310 * x135) + (-1 * x311 * x113) + (x309 * x125); const FLT x316 = (-1 * x311 * x163) + (x310 * x156) + (x309 * x164); const FLT x317 = (-1 * x310 * x166) + (x309 * x163) + (x311 * x164); const FLT x318 = (x310 * x178) + (-1 * x311 * x182) + (x309 * x183); const FLT x319 = (x309 * x182) + (-1 * x310 * x185) + (x311 * x183); const FLT x320 = (x219 * x310) + (-1 * x228 * x311) + (x237 * x309); const FLT x321 = (-1 * x247 * x310) + (x228 * x309) + (x237 * x311); const FLT x322 = (x262 * x310) + (-1 * x269 * x311) + (x268 * x309); const FLT x323 = (x269 * x309) + (-1 * x271 * x310) + (x268 * x311); const FLT x324 = (x291 * x309) + (x292 * x311) + (-1 * x288 * x310); const FLT x325 = (x285 * x310) + (-1 * x291 * x311) + (x292 * x309); const FLT x326 = (-1 * x58) + (-1 * x57) + x53 + x55; const FLT x327 = x75 * x52; const FLT x328 = (-1 * x71) + x70 + (-1 * x68) + x69; const FLT x329 = x75 * x61; const FLT x330 = 2 * ((-1 * x51 * x309) + (x75 * x310) + (x60 * x311)); const FLT x331 = (x311 * x145) + (-1 * x309 * x135) + (x310 * x125); const FLT x332 = (x311 * x166) + (-1 * x309 * x156) + (x310 * x164); const FLT x333 = (x311 * x185) + (-1 * x309 * x178) + (x310 * x183); const FLT x334 = (x247 * x311) + (-1 * x219 * x309) + (x237 * x310); const FLT x335 = (x271 * x311) + (-1 * x262 * x309) + (x268 * x310); const FLT x336 = (-1 * x285 * x309) + (x288 * x311) + (x292 * x310); const FLT x337 = (-1 * x48) + x45 + (-1 * x39) + x42; const FLT x338 = x75 * x296; cnSetZero(Hx); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x0 * (0.101971621297793 + (x61 * x59) + (-1 * x50 * x52))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x0 * ((x72 * x61) + (-1 * x67))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), ((x76 * x60) + (x73 * x74)) * x0); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[0])/sizeof(FLT), x78 + (2 * ((x81 * x60) + (-1 * x80 * x51)))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x0 * ((-1 * x52 * x136) + (x145 * x147) + (x61 * x146) + (-1 * x137 * x135))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x0 * ((x61 * x167) + (x166 * x147) + (-1 * x137 * x156) + (-1 * x52 * x165))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x0 * ((-1 * x178 * x137) + (x61 * x186) + (x185 * x147) + (-1 * x52 * x184))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x0 * ((x61 * x248) + (x247 * x147) + (-1 * x219 * x137) + (-1 * x51 * x238))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x0 * ((x61 * x272) + (x271 * x147) + (-1 * x262 * x137) + (-1 * x52 * x270))); cnMatrixOptionalSet(Hx, 0, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x0 * ((-1 * x52 * x294) + (x288 * x147) + (-1 * x285 * x137) + (x61 * x293))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x295 * (x67 + (-1 * x59 * x296))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), x295 * (0.101971621297793 + (x52 * x297) + (-1 * x72 * x296))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), ((x74 * x60) + (-1 * x73 * x76)) * x295); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[1])/sizeof(FLT), x77 + (2 * ((x51 * x298) + (-1 * x81 * x73)))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x295 * ((-1 * x296 * x146) + (x52 * x300) + (-1 * x113 * x147) + (x299 * x135))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x295 * ((-1 * x296 * x167) + (x299 * x156) + (-1 * x163 * x147) + (x52 * x301))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x295 * ((-1 * x296 * x186) + (x52 * x302) + (-1 * x182 * x147) + (x299 * x178))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), x295 * ((x52 * x303) + (-1 * x228 * x147) + (-1 * x296 * x248) + (x219 * x299))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x295 * ((x52 * x304) + (x299 * x262) + (-1 * x272 * x296) + (-1 * x269 * x147))); cnMatrixOptionalSet(Hx, 1, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x295 * ((-1 * x296 * x293) + (x299 * x285) + (x52 * x305) + (-1 * x291 * x147))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Acc[0])/sizeof(FLT), x306 * ((x50 * x296) + (-1 * x61 * x66))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Acc[1])/sizeof(FLT), ((x66 * x296) + (-1 * x61 * x297)) * x306); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Acc[2])/sizeof(FLT), x306 * (0.101971621297793 + (-0.203943242595586 * x308) + (-0.203943242595586 * x307))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, IMUBias.AccBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, IMUBias.AccScale[2])/sizeof(FLT), x79 + (2 * ((x80 * x73) + (-1 * x60 * x298)))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), x306 * ((x296 * x136) + (x113 * x137) + (-1 * x299 * x145) + (-1 * x61 * x300))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), x306 * ((x296 * x165) + (-1 * x61 * x301) + (-1 * x299 * x166) + (x163 * x137))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), x306 * ((x182 * x137) + (-1 * x61 * x302) + (-1 * x299 * x185) + (x296 * x184))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), ((-1 * x61 * x303) + (x73 * x238) + (-1 * x299 * x247) + (x228 * x137)) * x306); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), x306 * ((x270 * x296) + (-1 * x271 * x299) + (x269 * x137) + (-1 * x61 * x304))); cnMatrixOptionalSet(Hx, 2, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), x306 * ((x291 * x137) + (-1 * x61 * x305) + (-1 * x299 * x288) + (x296 * x294))); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[0])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x52 * x315) + (x61 * x314) + (-1 * x312 * x135) + (x313 * x145)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x61 * x317) + (x313 * x166) + (-1 * x312 * x156) + (-1 * x52 * x316)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x61 * x319) + (x313 * x185) + (-1 * x312 * x178) + (-1 * x52 * x318)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x61 * x321) + (-1 * x219 * x312) + (x247 * x313) + (-1 * x52 * x320)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x52 * x322) + (x61 * x323) + (-1 * x262 * x312) + (x271 * x313)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x52 * x325) + (x61 * x324) + (-1 * x285 * x312) + (x288 * x313)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), 1 + (x61 * x326) + (-2 * (x51 * x51))); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), (x73 * x61) + (-1 * x327)); cnMatrixOptionalSet(Hx, 3, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), x329 + (-1 * x52 * x328)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[1])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (x52 * x331) + (x330 * x135) + (-1 * x313 * x113) + (-1 * x296 * x314)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x52 * x332) + (x330 * x156) + (-1 * x313 * x163) + (-1 * x296 * x317)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x52 * x333) + (x330 * x178) + (-1 * x313 * x182) + (-1 * x296 * x319)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x52 * x334) + (x219 * x330) + (-1 * x228 * x313) + (-1 * x296 * x321)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (x52 * x335) + (x262 * x330) + (-1 * x269 * x313) + (-1 * x296 * x323)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x296 * x324) + (x52 * x336) + (-1 * x291 * x313) + (x285 * x330)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), x327 + (-1 * x296 * x326)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), 1 + (x52 * x337) + (-2 * x308)); cnMatrixOptionalSet(Hx, 4, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), (x61 * x51) + (-1 * x338)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, IMUBias.GyroBias[2])/sizeof(FLT), 1); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[0])/sizeof(FLT), (x296 * x315) + (-1 * x61 * x331) + (-1 * x330 * x145) + (x312 * x113)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[1])/sizeof(FLT), (x296 * x316) + (-1 * x61 * x332) + (-1 * x330 * x166) + (x312 * x163)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, IMUBias.IMUCorrection[2])/sizeof(FLT), (x312 * x182) + (-1 * x330 * x185) + (-1 * x61 * x333) + (x296 * x318)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[0])/sizeof(FLT), (x296 * x320) + (x228 * x312) + (-1 * x247 * x330) + (-1 * x61 * x334)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x61 * x335) + (x296 * x322) + (-1 * x271 * x330) + (x269 * x312)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Pose.AxisAngleRot[2])/sizeof(FLT), (x296 * x325) + (-1 * x288 * x330) + (x291 * x312) + (-1 * x61 * x336)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[0])/sizeof(FLT), (x73 * x52) + (-1 * x329)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[1])/sizeof(FLT), x338 + (-1 * x61 * x337)); cnMatrixOptionalSet(Hx, 5, offsetof(SurviveKalmanErrorModel, Velocity.AxisAngleRot[2])/sizeof(FLT), 1 + (x296 * x328) + (-2 * x307)); } // Full version Jacobian of IMUMeasurementErrorModel wrt [(*error_model).Acc[0], (*error_model).Acc[1], (*error_model).Acc[2], (*error_model).IMUBias.AccBias[0], (*error_model).IMUBias.AccBias[1], (*error_model).IMUBias.AccBias[2], (*error_model).IMUBias.AccScale[0], (*error_model).IMUBias.AccScale[1], (*error_model).IMUBias.AccScale[2], (*error_model).IMUBias.GyroBias[0], (*error_model).IMUBias.GyroBias[1], (*error_model).IMUBias.GyroBias[2], (*error_model).IMUBias.IMUCorrection[0], (*error_model).IMUBias.IMUCorrection[1], (*error_model).IMUBias.IMUCorrection[2], (*error_model).Pose.AxisAngleRot[0], (*error_model).Pose.AxisAngleRot[1], (*error_model).Pose.AxisAngleRot[2], (*error_model).Pose.Pos[0], (*error_model).Pose.Pos[1], (*error_model).Pose.Pos[2], (*error_model).Velocity.AxisAngleRot[0], (*error_model).Velocity.AxisAngleRot[1], (*error_model).Velocity.AxisAngleRot[2], (*error_model).Velocity.Pos[0], (*error_model).Velocity.Pos[1], (*error_model).Velocity.Pos[2]] static inline void IMUMeasurementErrorModel_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const SurviveKalmanModel* _x0, const SurviveKalmanErrorModel* error_model) { if(hx != 0) { IMUMeasurementErrorModel(hx, _x0, error_model); } if(Hx != 0) { IMUMeasurementErrorModel_jac_error_model(Hx, _x0, error_model); } }